xref: /titanic_51/usr/src/uts/common/io/ib/mgt/ibmf/ibmf_utils.c (revision 7c478bd95313f5f23a4c958a745db2134aa03244)
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
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
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