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 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 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