Subversion Repositories or1k

/*

 * Copyright (C) 1995 Advanced RISC Machines Limited. All rights reserved.

 *

 * This software may be freely used, copied, modified, and distributed

 * provided that the above copyright notice is preserved in all copies of the

 * software.

 */

/* -*-C-*-

 *

 * $Revision: 1.2 $

 *     $Date: 2003-08-15 01:06:10 $

 *

 *

 * msgbuild.c - utilities for assembling and interpreting ADP messages

 *

 */

#include <stdarg.h>     /* ANSI varargs support */

#ifdef TARGET

# include "angel.h"

# include "devconf.h"

#else

# include "host.h"

# include "hostchan.h"

#endif

#include "channels.h"

#include "buffers.h"

#include "angel_endian.h"     /* Endianness support macros */

#include "msgbuild.h"   /* Header file for this source code */

#ifndef UNUSED

# define UNUSED(x) ((x)=(x))

#endif

#ifndef TARGET

extern unsigned int Armsd_BufferSize;

#endif /* ndef TARGET */

unsigned int vmsgbuild(unsigned char *buffer, const char *format, va_list args)

{

    unsigned int blen = 0;

    int ch;

    /* Step through the format string */

    while ((ch = *format++) != '\0')

    {

        if (ch != '%')

        {

            if (buffer != NULL)

                *buffer++ = (unsigned char)ch;

            blen++;

        }

        else

        {

            switch (ch = *format++)

            {

              case 'w':

              case 'W':

              /* 32bit pointer */

              case 'p':

              case 'P':

              {

                  /* 32bit word / 32bit pointer */

                  unsigned int na = va_arg(args, unsigned int);

                  if (buffer != NULL)

                  {

                      PUT32LE(buffer, na);

                      buffer += sizeof(unsigned int);

                  }

                  blen += sizeof(unsigned int);

                  break;

              }

              case 'h':

              case 'H':

              {

                  /* 16bit value */

                  unsigned int na = va_arg(args, unsigned int);

                  if (buffer != NULL)

                  {

                      PUT16LE(buffer, na);

                      buffer += sizeof(unsigned short);

                  }

                  blen += sizeof(unsigned short);

                  break;

              }

              case 'c':

              case 'C':

              case 'b':

              case 'B':

                  /* 8bit character / 8bit byte */

                  ch = va_arg(args, int);

                  /*

                   * XXX

                   *

                   * fall through to the normal character processing

                   */

              case '%':

              default:

                  /* normal '%' character, or a different normal character */

                  if (buffer != NULL)

                      *buffer++ = (unsigned char)ch;

                  blen++;

                  break;

            }

        }

    }

    return blen;

}

/*

 * msgbuild

 * --------

 * Simple routine to aid in construction of Angel messages. See the

 * "msgbuild.h" header file for a detailed description of the operation

 * of this routine.

 */

unsigned int msgbuild(unsigned char *buffer, const char *format, ...)

{

    va_list args;

    unsigned int blen;

    va_start(args, format);

    blen = vmsgbuild(buffer, format, args);

    va_end(args);

    return blen;

}

#if !defined(JTAG_ADP_SUPPORTED) && !defined(MSG_UTILS_ONLY)

/*

 * This routine allocates a buffer, puts the data supplied as

 * parameters into the buffer and sends the message. It does *NOT*

 * wait for a reply.

 */

extern int msgsend(ChannelID chan, const char *format,...)

{

    unsigned int length;

    p_Buffer buffer;

    va_list args;

# ifndef TARGET

    Packet *packet;

    packet = DevSW_AllocatePacket(Armsd_BufferSize);

    buffer = packet->pk_buffer;

# else

    buffer = angel_ChannelAllocBuffer(Angel_ChanBuffSize);

# endif

    if (buffer != NULL)

    {

        va_start(args, format);

        length = vmsgbuild(BUFFERDATA(buffer), format, args);

# ifdef TARGET

        angel_ChannelSend(CH_DEFAULT_DEV, chan, buffer, length);

# else

        packet->pk_length = length;

        Adp_ChannelWrite(chan, packet);

# endif

        va_end(args);

        return 0;

    }

    else

        return -1;

}

#endif /* ndef JTAG_ADP_SUPPORTED && ndef MSG_UTILS_ONLY */

/*

 * unpack_message

 * --------------

 */

extern unsigned int unpack_message(unsigned char *buffer, const char *format, ...)

{

    va_list args;

    unsigned int blen = 0;

    int ch;

    char *chp = NULL;

    va_start(args, format);

    /* Step through the format string. */

    while ((ch = *format++) != '\0')

    {

        if (ch != '%')

        {

            if (buffer != NULL)

                ch = (unsigned char)*buffer++;

            blen++;

        }

        else

        {

            switch (ch = *format++)

            {

              case 'w':

              case 'W':

              {

                  /* 32bit word. */

                  unsigned int *nap = va_arg(args, unsigned int*);

                  if (buffer != NULL)

                  {

                      *nap = PREAD32(LE, buffer);

                      buffer += sizeof(unsigned int);

                  }

                  blen += sizeof(unsigned int);

                  break;

              }

              case 'h':

              case 'H':

              {

                  /* 16bit value. */

                  unsigned int *nap = va_arg(args, unsigned int*);

                  if (buffer != NULL)

                  {

                      *nap = PREAD16(LE,buffer);

                      buffer += sizeof(unsigned short);

                  }

                  blen += sizeof(unsigned short);

                  break;

              }

              case 'c':

              case 'C':

              case 'b':

              case 'B':

                  /* 8-bit character, or 8-bit byte */

                  chp = va_arg(args, char*);

                  /*

                   * XXX

                   *

                   * fall through to the normal character processing.

                   */

              case '%':

              default:

                  /* normal '%' character, or a different normal character */

                  if (buffer != NULL)

                      *chp = (unsigned char)*buffer++;

                  blen++;

                  break;

            }

        }

    }

    va_end(args);

    return(blen);

}

/* EOF msgbuild.c */