#include <stdarg.h>
#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"
#include "msgbuild.h"
#ifndef UNUSED
# define UNUSED(x) ((x)=(x))
#endif
#ifndef TARGET
extern unsigned int Armsd_BufferSize;
#endif
unsigned int vmsgbuild(unsigned char *buffer, const char *format, va_list args)
{
unsigned int blen = 0;
int ch;
while ((ch = *format++) != '\0')
{
if (ch != '%')
{
if (buffer != NULL)
*buffer++ = (unsigned char)ch;
blen++;
}
else
{
switch (ch = *format++)
{
case 'w':
case 'W':
case 'p':
case 'P':
{
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':
{
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':
ch = va_arg(args, int);
case '%':
default:
if (buffer != NULL)
*buffer++ = (unsigned char)ch;
blen++;
break;
}
}
}
return blen;
}
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)
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
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);
while ((ch = *format++) != '\0')
{
if (ch != '%')
{
if (buffer != NULL)
ch = (unsigned char)*buffer++;
blen++;
}
else
{
switch (ch = *format++)
{
case 'w':
case 'W':
{
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':
{
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':
chp = va_arg(args, char*);
case '%':
default:
if (buffer != NULL)
*chp = (unsigned char)*buffer++;
blen++;
break;
}
}
}
va_end(args);
return(blen);
}