1 /*
2 * CDDL HEADER START
3 *
4 * The contents of this file are subject to the terms of the
5 * Common Development and Distribution License, Version 1.0 only
6 * (the "License"). You may not use this file except in compliance
7 * with the License.
8 *
9 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
10 * or http://www.opensolaris.org/os/licensing.
11 * See the License for the specific language governing permissions
12 * and limitations under the License.
13 *
14 * When distributing Covered Code, include this CDDL HEADER in each
15 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
16 * If applicable, add the following below this CDDL HEADER, with the
17 * fields enclosed by brackets "[]" replaced with your own identifying
18 * information: Portions Copyright [yyyy] [name of copyright owner]
19 *
20 * CDDL HEADER END
21 */
22 /*
23 * Copyright 2004 Sun Microsystems, Inc. All rights reserved.
24 * Use is subject to license terms.
25 */
26
27 #pragma ident "%Z%%M% %I% %E% SMI"
28
29 /*
30 * Descriptor parsing functions
31 */
32
33 #include <sys/types.h>
34 #include <sys/systm.h>
35 #include <sys/inttypes.h>
36 #include <sys/ib/mgt/ibmf/ibmf_utils.h>
37 #include <sys/debug.h>
38
39 #define INCREMENT_BUF(buf) \
40 if ((buf)[0] == 0) { \
41 break; \
42 } else { \
43 (buf) += (buf)[0]; \
44 }
45 #define isdigit(ch) ((ch >= '0') && (ch <= '9'))
46
47 /*
48 * ibmf_utils_unpack_data:
49 *
50 * parser function which takes a format string, a void pointer, and a character
51 * buffer and parses the buffer according to the identifiers in the format
52 * string. Copies the data from the buffer and places into the structure,
53 * taking care of byte swapping and any padding due to 64-bit Solaris. Modified
54 * from /ws/on81-gate/usr/src/uts/common/io/usb/usba/parser.c.
55 *
56 * The data and structure length parameters can be larger than the number of
57 * bytes specified in the format. unpack_data will use the smallest of the
58 * three values, stopping when it finishes parsing the format string or reaches
59 * the end of one of the two buffers.
60 */
61 void
ibmf_utils_unpack_data(char * format,uchar_t * data,size_t datalen,void * structure,size_t structlen)62 ibmf_utils_unpack_data(char *format,
63 uchar_t *data,
64 size_t datalen,
65 void *structure,
66 size_t structlen)
67 {
68 int fmt;
69 int multiplier = 0;
70 uchar_t *dataend = data + datalen;
71 char *structstart = (char *)structure;
72 void *structend = (void *)((intptr_t)structstart + structlen);
73
74 while ((fmt = *format) != '\0') {
75
76 if (fmt == 'c') {
77 uint8_t *cp = (uint8_t *)structure;
78
79 /*
80 * account for possible hole in structure
81 * due to unaligned data
82 */
83 cp = (uint8_t *)
84 (((uintptr_t)cp + _CHAR_ALIGNMENT - 1) &
85 ~(_CHAR_ALIGNMENT - 1));
86
87 if (((data + 1) > dataend) ||
88 ((cp + 1) > (uint8_t *)structend))
89 break;
90
91 *cp++ = *data++;
92 structure = (void *)cp;
93 if (multiplier) {
94 multiplier--;
95 }
96 if (multiplier == 0) {
97 format++;
98 }
99 } else if (fmt == 's') {
100 uint16_t *sp = (uint16_t *)structure;
101
102 /*
103 * account for possible hole in structure
104 * due to unaligned data
105 */
106 sp = (uint16_t *)
107 (((uintptr_t)sp + _SHORT_ALIGNMENT - 1) &
108 ~(_SHORT_ALIGNMENT - 1));
109
110 if (((data + 2) > dataend) ||
111 ((sp + 1) > (uint16_t *)structend))
112 break;
113
114 *sp++ = (data[0] << 8) + data[1];
115 data += 2;
116 structure = (void *)sp;
117 if (multiplier) {
118 multiplier--;
119 }
120 if (multiplier == 0) {
121 format++;
122 }
123 } else if (fmt == 'l') {
124 uint32_t *lp = (uint32_t *)structure;
125
126 /*
127 * account for possible hole in structure
128 * due to unaligned data
129 */
130 lp = (uint32_t *)
131 (((uintptr_t)lp + _INT_ALIGNMENT - 1) &
132 ~(_INT_ALIGNMENT - 1));
133
134 if (((data + 4) > dataend) ||
135 ((lp + 1) > (uint32_t *)structend))
136 break;
137
138 *lp++ = ((((((uint32_t)data[0] << 8) | data[1]) << 8)
139 | data[2]) << 8) | data[3];
140
141 data += 4;
142 structure = (void *)lp;
143 if (multiplier) {
144 multiplier--;
145 }
146 if (multiplier == 0) {
147 format++;
148 }
149 } else if (fmt == 'L') {
150 uint64_t *llp = (uint64_t *)structure;
151
152 /*
153 * account for possible hole in structure
154 * due to unaligned data
155 */
156 llp = (uint64_t *)
157 (((uintptr_t)llp + _LONG_LONG_ALIGNMENT - 1) &
158 ~(_LONG_LONG_ALIGNMENT - 1));
159
160 if (((data + 8) > dataend) ||
161 ((llp + 1) > (uint64_t *)structend))
162 break;
163 /*
164 * note: data[0] is cast to uint64_t so that the
165 * compiler wouldn't treat the results of the shifts
166 * as a 32bit quantity; we really want to get 64bits
167 * out of this.
168 */
169 *llp++ = ((((((((((((((uint64_t)data[0] << 8) |
170 data[1]) << 8) | data[2]) << 8) |
171 data[3]) << 8) | data[4]) << 8) |
172 data[5]) << 8) | data[6]) << 8) |
173 data[7];
174
175 data += 8;
176 structure = (void *)llp;
177 if (multiplier) {
178 multiplier--;
179 }
180 if (multiplier == 0) {
181 format++;
182 }
183 } else if (isdigit(fmt)) {
184 multiplier = (multiplier * 10) + (fmt - '0');
185 format++;
186 } else {
187 multiplier = 0;
188 break;
189 }
190 }
191 }
192
193 /*
194 * ibmf_utils_pack_data:
195 *
196 * parser function which takes a format string, a void pointer, and a character
197 * buffer and parses the structure according to the identifiers in the format
198 * string. Copies the data from the structure and places in the buffer, taking
199 * care of byte swapping and any padding due to 64-bit Solaris. Modified from
200 * /ws/on81-gate/usr/src/uts/common/io/usb/usba/parser.c.
201 *
202 */
203 void
ibmf_utils_pack_data(char * format,void * structure,size_t structlen,uchar_t * data,size_t datalen)204 ibmf_utils_pack_data(char *format, void *structure,
205 size_t structlen, uchar_t *data, size_t datalen)
206 {
207 int fmt;
208 int multiplier = 0;
209 uchar_t *dataend = data + datalen;
210 char *structend = (void *)((uchar_t *)structure + structlen);
211
212 while ((fmt = *format) != '\0') {
213 if (fmt == 'c') {
214 uint8_t *cp = (uint8_t *)structure;
215
216 /*
217 * account for possible hole in structure
218 * due to unaligned data
219 */
220 cp = (uint8_t *)
221 (((uintptr_t)cp + _CHAR_ALIGNMENT - 1) &
222 ~(_CHAR_ALIGNMENT - 1));
223
224 if (((data + 1) > dataend) ||
225 ((cp + 1) > (uint8_t *)structend)) {
226 break;
227 }
228
229 *data++ = *cp++;
230 structure = (void *)cp;
231 if (multiplier) {
232 multiplier--;
233 }
234 if (multiplier == 0) {
235 format++;
236 }
237 } else if (fmt == 's') {
238 uint16_t *sp = (uint16_t *)structure;
239
240 /*
241 * account for possible hole in structure
242 * due to unaligned data
243 */
244 sp = (uint16_t *)
245 (((uintptr_t)sp + _SHORT_ALIGNMENT - 1) &
246 ~(_SHORT_ALIGNMENT - 1));
247
248 if (((data + 2) > dataend) ||
249 ((sp + 1) > (uint16_t *)structend))
250 break;
251
252 /* do an endian-independent copy */
253 data[0] = (uchar_t)(*sp >> 8);
254 data[1] = (uchar_t)(*sp);
255
256 sp++;
257 data += 2;
258
259 structure = (void *)sp;
260 if (multiplier) {
261 multiplier--;
262 }
263 if (multiplier == 0) {
264 format++;
265 }
266 } else if (fmt == 'l') {
267 uint32_t *lp = (uint32_t *)structure;
268
269 /*
270 * account for possible hole in structure
271 * due to unaligned data
272 */
273 lp = (uint32_t *)
274 (((uintptr_t)lp + _INT_ALIGNMENT - 1) &
275 ~(_INT_ALIGNMENT - 1));
276
277 if (((data + 4) > dataend) ||
278 ((lp + 1) > (uint32_t *)structend))
279 break;
280
281 /* do an endian-independent copy */
282 data[0] = (uchar_t)(*lp >> 24);
283 data[1] = (uchar_t)(*lp >> 16);
284 data[2] = (uchar_t)(*lp >> 8);
285 data[3] = (uchar_t)(*lp);
286
287 lp++;
288 data += 4;
289
290 structure = (void *)lp;
291 if (multiplier) {
292 multiplier--;
293 }
294 if (multiplier == 0) {
295 format++;
296 }
297 } else if (fmt == 'L') {
298 uint64_t *llp = (uint64_t *)structure;
299
300 /*
301 * account for possible hole in structure
302 * due to unaligned data
303 */
304 llp = (uint64_t *)
305 (((uintptr_t)llp + _LONG_LONG_ALIGNMENT - 1) &
306 ~(_LONG_LONG_ALIGNMENT - 1));
307
308 if (((data + 8) > dataend) ||
309 ((llp + 1) > (uint64_t *)structend))
310 break;
311
312 /* do an endian-independent copy */
313 data[0] = (uchar_t)(*llp >> 56);
314 data[1] = (uchar_t)(*llp >> 48);
315 data[2] = (uchar_t)(*llp >> 40);
316 data[3] = (uchar_t)(*llp >> 32);
317 data[4] = (uchar_t)(*llp >> 24);
318 data[5] = (uchar_t)(*llp >> 16);
319 data[6] = (uchar_t)(*llp >> 8);
320 data[7] = (uchar_t)(*llp);
321 llp++;
322 data += 8;
323
324 structure = (void *)llp;
325 if (multiplier) {
326 multiplier--;
327 }
328 if (multiplier == 0) {
329 format++;
330 }
331 } else if (isdigit(fmt)) {
332 multiplier = (multiplier * 10) + (fmt - '0');
333 format++;
334 } else {
335 multiplier = 0;
336 break;
337 }
338 }
339 }
340