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