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