msgbuild.c   [plain text]

/* 
 * 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-*-
 *
 * 1.1.1.2
 *     1999/11/02 04:44:26
 *
 *
 * 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 */