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
vmsgbuild(unsigned char * buffer,const char * format,va_list args)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 */
msgbuild(unsigned char * buffer,const char * format,...)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 */
msgsend(ChannelID chan,const char * format,...)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 */
unpack_message(unsigned char * buffer,const char * format,...)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