Commit | Line | Data |
---|---|---|
c906108c SS |
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$ | |
12 | * $Date$ | |
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" | |
5c44784c | 31 | #include "angel_endian.h" /* Endianness support macros */ |
c906108c SS |
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 */ |