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