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 */
61void
62ibmf_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 */
203void
204ibmf_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