1 /*
2  * Copyright (C) 1995 Advanced RISC Machines Limited. All rights reserved.
3  *
4  * This software may be freely used, copied, modified, and distributed
5  * provided that the above copyright notice is preserved in all copies of the
6  * software.
7  */
8 
9 /* -*-C-*-
10  *
11  * $Revision: 1.3 $
12  *     $Date: 2004/12/27 14:00:54 $
13  *
14  *
15  * msgbuild.c - utilities for assembling and interpreting ADP messages
16  *
17  */
18 
19 #include <stdarg.h>     /* ANSI varargs support */
20 
21 #ifdef TARGET
22 # include "angel.h"
23 # include "devconf.h"
24 #else
25 # include "host.h"
26 # include "hostchan.h"
27 #endif
28 
29 #include "channels.h"
30 #include "buffers.h"
31 #include "angel_endian.h"     /* Endianness support macros */
32 #include "msgbuild.h"   /* Header file for this source code */
33 
34 #ifndef UNUSED
35 # define UNUSED(x) ((x)=(x))
36 #endif
37 
38 #ifndef TARGET
39 
40 extern unsigned int Armsd_BufferSize;
41 
42 #endif /* ndef TARGET */
43 
44 
45 unsigned int vmsgbuild(unsigned char *buffer, const char *format, va_list args)
46 {
47     unsigned int blen = 0;
48     int ch;
49 
50     /* Step through the format string */
51     while ((ch = *format++) != '\0')
52     {
53         if (ch != '%')
54         {
55             if (buffer != NULL)
56                 *buffer++ = (unsigned char)ch;
57 
58             blen++;
59         }
60         else
61         {
62             switch (ch = *format++)
63             {
64               case 'w':
65               case 'W':
66               /* 32bit pointer */
67               case 'p':
68               case 'P':
69               {
70                   /* 32bit word / 32bit pointer */
71                   unsigned int na = va_arg(args, unsigned int);
72 
73                   if (buffer != NULL)
74                   {
75                       PUT32LE(buffer, na);
76                       buffer += sizeof(unsigned int);
77                   }
78 
79                   blen += sizeof(unsigned int);
80 
81                   break;
82               }
83 
84               case 'h':
85               case 'H':
86               {
87                   /* 16bit value */
88                   unsigned int na = va_arg(args, unsigned int);
89 
90                   if (buffer != NULL)
91                   {
92                       PUT16LE(buffer, na);
93                       buffer += sizeof(unsigned short);
94                   }
95 
96                   blen += sizeof(unsigned short);
97 
98                   break;
99               }
100 
101               case 'c':
102               case 'C':
103               case 'b':
104               case 'B':
105                   /* 8bit character / 8bit byte */
106                   ch = va_arg(args, int);
107 
108                   /*
109                    * XXX
110                    *
111                    * fall through to the normal character processing
112                    */
113 
114               case '%':
115               default:
116                   /* normal '%' character, or a different normal character */
117                   if (buffer != NULL)
118                       *buffer++ = (unsigned char)ch;
119 
120                   blen++;
121                   break;
122             }
123         }
124     }
125 
126     return blen;
127 }
128 
129 /*
130  * msgbuild
131  * --------
132  * Simple routine to aid in construction of Angel messages. See the
133  * "msgbuild.h" header file for a detailed description of the operation
134  * of this routine.
135  */
136 unsigned int msgbuild(unsigned char *buffer, const char *format, ...)
137 {
138     va_list args;
139     unsigned int blen;
140 
141     va_start(args, format);
142     blen = vmsgbuild(buffer, format, args);
143     va_end(args);
144 
145     return blen;
146 }
147 
148 #if !defined(JTAG_ADP_SUPPORTED) && !defined(MSG_UTILS_ONLY)
149 /*
150  * This routine allocates a buffer, puts the data supplied as
151  * parameters into the buffer and sends the message. It does *NOT*
152  * wait for a reply.
153  */
154 extern int msgsend(ChannelID chan, const char *format,...)
155 {
156     unsigned int length;
157     p_Buffer buffer;
158     va_list args;
159 # ifndef TARGET
160     Packet *packet;
161 
162     packet = DevSW_AllocatePacket(Armsd_BufferSize);
163     buffer = packet->pk_buffer;
164 # else
165     buffer = angel_ChannelAllocBuffer(Angel_ChanBuffSize);
166 # endif
167 
168     if (buffer != NULL)
169     {
170         va_start(args, format);
171 
172         length = vmsgbuild(BUFFERDATA(buffer), format, args);
173 
174 # ifdef TARGET
175         angel_ChannelSend(CH_DEFAULT_DEV, chan, buffer, length);
176 # else
177         packet->pk_length = length;
178         Adp_ChannelWrite(chan, packet);
179 # endif
180 
181         va_end(args);
182         return 0;
183     }
184     else
185         return -1;
186 }
187 
188 #endif /* ndef JTAG_ADP_SUPPORTED && ndef MSG_UTILS_ONLY */
189 
190 /*
191  * unpack_message
192  * --------------
193  */
194 extern unsigned int unpack_message(unsigned char *buffer, const char *format, ...)
195 {
196     va_list args;
197     unsigned int blen = 0;
198     int ch;
199     char *chp = NULL;
200 
201     va_start(args, format);
202 
203     /* Step through the format string. */
204     while ((ch = *format++) != '\0')
205     {
206         if (ch != '%')
207         {
208             if (buffer != NULL)
209                 ch = (unsigned char)*buffer++;
210 
211             blen++;
212         }
213         else
214         {
215             switch (ch = *format++)
216             {
217               case 'w':
218               case 'W':
219               {
220                   /* 32bit word. */
221                   unsigned int *nap = va_arg(args, unsigned int*);
222 
223                   if (buffer != NULL)
224                   {
225                       *nap = PREAD32(LE, buffer);
226                       buffer += sizeof(unsigned int);
227                   }
228 
229                   blen += sizeof(unsigned int);
230 
231                   break;
232               }
233 
234               case 'h':
235               case 'H':
236               {
237                   /* 16bit value. */
238                   unsigned int *nap = va_arg(args, unsigned int*);
239 
240                   if (buffer != NULL)
241                   {
242                       *nap = PREAD16(LE,buffer);
243                       buffer += sizeof(unsigned short);
244                   }
245 
246                   blen += sizeof(unsigned short);
247 
248                   break;
249               }
250 
251               case 'c':
252               case 'C':
253               case 'b':
254               case 'B':
255                   /* 8-bit character, or 8-bit byte */
256                   chp = va_arg(args, char*);
257 
258                   /*
259                    * XXX
260                    *
261                    * fall through to the normal character processing.
262                    */
263 
264               case '%':
265               default:
266                   /* normal '%' character, or a different normal character */
267                   if (buffer != NULL)
268                       *chp = (unsigned char)*buffer++;
269 
270                   blen++;
271 
272                   break;
273             }
274         }
275     }
276 
277     va_end(args);
278     return(blen);
279 }
280 
281 
282 /* EOF msgbuild.c */
283