1184610Salfred/* $FreeBSD: stable/11/lib/libusb/libusb20_desc.c 368825 2020-12-30 01:11:05Z hselasky $ */ 2184610Salfred/*- 3184610Salfred * Copyright (c) 2008 Hans Petter Selasky. All rights reserved. 4184610Salfred * 5184610Salfred * Redistribution and use in source and binary forms, with or without 6184610Salfred * modification, are permitted provided that the following conditions 7184610Salfred * are met: 8184610Salfred * 1. Redistributions of source code must retain the above copyright 9184610Salfred * notice, this list of conditions and the following disclaimer. 10184610Salfred * 2. Redistributions in binary form must reproduce the above copyright 11184610Salfred * notice, this list of conditions and the following disclaimer in the 12184610Salfred * documentation and/or other materials provided with the distribution. 13184610Salfred * 14184610Salfred * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15184610Salfred * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16184610Salfred * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17184610Salfred * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18184610Salfred * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19184610Salfred * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20184610Salfred * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21184610Salfred * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22184610Salfred * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23184610Salfred * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24184610Salfred * SUCH DAMAGE. 25184610Salfred */ 26184610Salfred 27248236Shselasky#ifdef LIBUSB_GLOBAL_INCLUDE_FILE 28248236Shselasky#include LIBUSB_GLOBAL_INCLUDE_FILE 29248236Shselasky#else 30184610Salfred#include <stdio.h> 31184610Salfred#include <stdlib.h> 32184610Salfred#include <string.h> 33248236Shselasky#include <time.h> 34248236Shselasky#include <sys/queue.h> 35248236Shselasky#endif 36184610Salfred 37184610Salfred#include "libusb20.h" 38184610Salfred#include "libusb20_desc.h" 39184610Salfred#include "libusb20_int.h" 40184610Salfred 41184610Salfredstatic const uint32_t libusb20_me_encode_empty[2]; /* dummy */ 42184610Salfred 43184610SalfredLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_DEVICE_DESC); 44184610SalfredLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_ENDPOINT_DESC); 45184610SalfredLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_INTERFACE_DESC); 46184610SalfredLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONFIG_DESC); 47184610SalfredLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONTROL_SETUP); 48227404ShselaskyLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_ENDPT_COMP_DESC); 49227404ShselaskyLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_USB_20_DEVCAP_DESC); 50227404ShselaskyLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_USB_DEVCAP_DESC); 51227404ShselaskyLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_BOS_DESCRIPTOR); 52184610Salfred 53184610Salfred/*------------------------------------------------------------------------* 54184610Salfred * libusb20_parse_config_desc 55184610Salfred * 56184610Salfred * Return values: 57184610Salfred * NULL: Out of memory. 58184610Salfred * Else: A valid config structure pointer which must be passed to "free()" 59184610Salfred *------------------------------------------------------------------------*/ 60184610Salfredstruct libusb20_config * 61184610Salfredlibusb20_parse_config_desc(const void *config_desc) 62184610Salfred{ 63184610Salfred struct libusb20_config *lub_config; 64184610Salfred struct libusb20_interface *lub_interface; 65184610Salfred struct libusb20_interface *lub_alt_interface; 66184610Salfred struct libusb20_interface *last_if; 67184610Salfred struct libusb20_endpoint *lub_endpoint; 68184610Salfred struct libusb20_endpoint *last_ep; 69184610Salfred 70184610Salfred struct libusb20_me_struct pcdesc; 71184610Salfred const uint8_t *ptr; 72184610Salfred uint32_t size; 73184610Salfred uint16_t niface_no_alt; 74184610Salfred uint16_t niface; 75184610Salfred uint16_t nendpoint; 76234491Shselasky uint16_t iface_no; 77184610Salfred 78184610Salfred ptr = config_desc; 79184610Salfred if (ptr[1] != LIBUSB20_DT_CONFIG) { 80184610Salfred return (NULL); /* not config descriptor */ 81184610Salfred } 82368825Shselasky 83184610Salfred /* 84368825Shselasky * The first "bInterfaceNumber" cannot start at 0xFFFF 85368825Shselasky * because the field is 8-bit. 86184610Salfred */ 87184610Salfred niface_no_alt = 0; 88184610Salfred nendpoint = 0; 89184610Salfred niface = 0; 90234491Shselasky iface_no = 0xFFFF; 91184610Salfred ptr = NULL; 92184610Salfred 93184610Salfred /* get "wTotalLength" and setup "pcdesc" */ 94184610Salfred pcdesc.ptr = LIBUSB20_ADD_BYTES(config_desc, 0); 95184610Salfred pcdesc.len = 96185087Salfred ((const uint8_t *)config_desc)[2] | 97185087Salfred (((const uint8_t *)config_desc)[3] << 8); 98184610Salfred pcdesc.type = LIBUSB20_ME_IS_RAW; 99184610Salfred 100184610Salfred /* descriptor pre-scan */ 101184610Salfred while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) { 102184610Salfred if (ptr[1] == LIBUSB20_DT_ENDPOINT) { 103184610Salfred nendpoint++; 104184610Salfred } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) { 105184610Salfred niface++; 106184610Salfred /* check "bInterfaceNumber" */ 107184610Salfred if (ptr[2] != iface_no) { 108184610Salfred iface_no = ptr[2]; 109184610Salfred niface_no_alt++; 110184610Salfred } 111184610Salfred } 112184610Salfred } 113184610Salfred 114184610Salfred /* sanity checking */ 115184610Salfred if (niface >= 256) { 116184610Salfred return (NULL); /* corrupt */ 117184610Salfred } 118184610Salfred if (nendpoint >= 256) { 119184610Salfred return (NULL); /* corrupt */ 120184610Salfred } 121184610Salfred size = sizeof(*lub_config) + 122184610Salfred (niface * sizeof(*lub_interface)) + 123184610Salfred (nendpoint * sizeof(*lub_endpoint)) + 124184610Salfred pcdesc.len; 125184610Salfred 126184610Salfred lub_config = malloc(size); 127184610Salfred if (lub_config == NULL) { 128184610Salfred return (NULL); /* out of memory */ 129184610Salfred } 130199055Sthompsa /* make sure memory is initialised */ 131199055Sthompsa memset(lub_config, 0, size); 132199055Sthompsa 133184610Salfred lub_interface = (void *)(lub_config + 1); 134184610Salfred lub_alt_interface = (void *)(lub_interface + niface_no_alt); 135184610Salfred lub_endpoint = (void *)(lub_interface + niface); 136184610Salfred 137184610Salfred /* 138184610Salfred * Make a copy of the config descriptor, so that the caller can free 139298896Spfg * the initial config descriptor pointer! 140184610Salfred */ 141285720Spfg memcpy((void *)(lub_endpoint + nendpoint), config_desc, pcdesc.len); 142285720Spfg 143285720Spfg ptr = (const void *)(lub_endpoint + nendpoint); 144184610Salfred pcdesc.ptr = LIBUSB20_ADD_BYTES(ptr, 0); 145184610Salfred 146184610Salfred /* init config structure */ 147184610Salfred 148184610Salfred LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc); 149184610Salfred 150184610Salfred if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) { 151184610Salfred /* ignore */ 152184610Salfred } 153184610Salfred lub_config->num_interface = 0; 154184610Salfred lub_config->interface = lub_interface; 155184610Salfred lub_config->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 156184610Salfred lub_config->extra.len = -ptr[0]; 157184610Salfred lub_config->extra.type = LIBUSB20_ME_IS_RAW; 158184610Salfred 159184610Salfred /* reset states */ 160184610Salfred niface = 0; 161234491Shselasky iface_no = 0xFFFF; 162184610Salfred ptr = NULL; 163184610Salfred lub_interface--; 164184610Salfred lub_endpoint--; 165184610Salfred last_if = NULL; 166184610Salfred last_ep = NULL; 167184610Salfred 168184610Salfred /* descriptor pre-scan */ 169184610Salfred while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) { 170184610Salfred if (ptr[1] == LIBUSB20_DT_ENDPOINT) { 171184610Salfred if (last_if) { 172184610Salfred lub_endpoint++; 173184610Salfred last_ep = lub_endpoint; 174184610Salfred last_if->num_endpoints++; 175184610Salfred 176184610Salfred LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC, &last_ep->desc); 177184610Salfred 178184610Salfred if (libusb20_me_decode(ptr, ptr[0], &last_ep->desc)) { 179184610Salfred /* ignore */ 180184610Salfred } 181184610Salfred last_ep->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 182184610Salfred last_ep->extra.len = 0; 183184610Salfred last_ep->extra.type = LIBUSB20_ME_IS_RAW; 184184610Salfred } else { 185184610Salfred lub_config->extra.len += ptr[0]; 186184610Salfred } 187184610Salfred 188184610Salfred } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) { 189184610Salfred if (ptr[2] != iface_no) { 190184610Salfred /* new interface */ 191184610Salfred iface_no = ptr[2]; 192184610Salfred lub_interface++; 193184610Salfred lub_config->num_interface++; 194184610Salfred last_if = lub_interface; 195184610Salfred niface++; 196184610Salfred } else { 197184610Salfred /* one more alternate setting */ 198184610Salfred lub_interface->num_altsetting++; 199184610Salfred last_if = lub_alt_interface; 200184610Salfred lub_alt_interface++; 201184610Salfred } 202184610Salfred 203184610Salfred LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC, &last_if->desc); 204184610Salfred 205184610Salfred if (libusb20_me_decode(ptr, ptr[0], &last_if->desc)) { 206184610Salfred /* ignore */ 207184610Salfred } 208368825Shselasky 209368825Shselasky /* detect broken USB descriptors when USB debugging is enabled */ 210368825Shselasky if (last_if->desc.bInterfaceNumber != (uint8_t)(niface - 1)) { 211368825Shselasky const char *str = getenv("LIBUSB_DEBUG"); 212368825Shselasky if (str != NULL && str[0] != '\0' && str[0] != '0') { 213368825Shselasky printf("LIBUSB_DEBUG: bInterfaceNumber(%u) is not sequential(%u)\n", 214368825Shselasky last_if->desc.bInterfaceNumber, niface - 1); 215368825Shselasky } 216368825Shselasky } 217184610Salfred last_if->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 218184610Salfred last_if->extra.len = 0; 219184610Salfred last_if->extra.type = LIBUSB20_ME_IS_RAW; 220184610Salfred last_if->endpoints = lub_endpoint + 1; 221184610Salfred last_if->altsetting = lub_alt_interface; 222184610Salfred last_if->num_altsetting = 0; 223184610Salfred last_if->num_endpoints = 0; 224184610Salfred last_ep = NULL; 225184610Salfred } else { 226184610Salfred /* unknown descriptor */ 227184610Salfred if (last_if) { 228184610Salfred if (last_ep) { 229184610Salfred last_ep->extra.len += ptr[0]; 230184610Salfred } else { 231184610Salfred last_if->extra.len += ptr[0]; 232184610Salfred } 233184610Salfred } else { 234184610Salfred lub_config->extra.len += ptr[0]; 235184610Salfred } 236184610Salfred } 237184610Salfred } 238184610Salfred return (lub_config); 239184610Salfred} 240184610Salfred 241184610Salfred/*------------------------------------------------------------------------* 242184610Salfred * libusb20_desc_foreach 243184610Salfred * 244184610Salfred * Safe traversal of USB descriptors. 245184610Salfred * 246184610Salfred * Return values: 247184610Salfred * NULL: End of descriptors 248184610Salfred * Else: Pointer to next descriptor 249184610Salfred *------------------------------------------------------------------------*/ 250184610Salfredconst uint8_t * 251184610Salfredlibusb20_desc_foreach(const struct libusb20_me_struct *pdesc, 252184610Salfred const uint8_t *psubdesc) 253184610Salfred{ 254186730Salfred const uint8_t *start; 255186730Salfred const uint8_t *end; 256186730Salfred const uint8_t *desc_next; 257184610Salfred 258186730Salfred /* be NULL safe */ 259186730Salfred if (pdesc == NULL) 260184610Salfred return (NULL); 261184610Salfred 262186730Salfred start = (const uint8_t *)pdesc->ptr; 263186730Salfred end = LIBUSB20_ADD_BYTES(start, pdesc->len); 264186730Salfred 265186730Salfred /* get start of next descriptor */ 266186730Salfred if (psubdesc == NULL) 267186730Salfred psubdesc = start; 268186730Salfred else 269186730Salfred psubdesc = psubdesc + psubdesc[0]; 270186730Salfred 271186730Salfred /* check that the next USB descriptor is within the range */ 272186730Salfred if ((psubdesc < start) || (psubdesc >= end)) 273186730Salfred return (NULL); /* out of range, or EOD */ 274186730Salfred 275186730Salfred /* check start of the second next USB descriptor, if any */ 276186730Salfred desc_next = psubdesc + psubdesc[0]; 277186730Salfred if ((desc_next < start) || (desc_next > end)) 278186730Salfred return (NULL); /* out of range */ 279186730Salfred 280186730Salfred /* check minimum descriptor length */ 281186730Salfred if (psubdesc[0] < 3) 282186730Salfred return (NULL); /* too short descriptor */ 283186730Salfred 284186730Salfred return (psubdesc); /* return start of next descriptor */ 285184610Salfred} 286184610Salfred 287184610Salfred/*------------------------------------------------------------------------* 288184610Salfred * libusb20_me_get_1 - safety wrapper to read out one byte 289184610Salfred *------------------------------------------------------------------------*/ 290184610Salfreduint8_t 291184610Salfredlibusb20_me_get_1(const struct libusb20_me_struct *ie, uint16_t offset) 292184610Salfred{ 293184610Salfred if (offset < ie->len) { 294184610Salfred return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie->ptr, offset))); 295184610Salfred } 296184610Salfred return (0); 297184610Salfred} 298184610Salfred 299184610Salfred/*------------------------------------------------------------------------* 300184610Salfred * libusb20_me_get_2 - safety wrapper to read out one word 301184610Salfred *------------------------------------------------------------------------*/ 302184610Salfreduint16_t 303184610Salfredlibusb20_me_get_2(const struct libusb20_me_struct *ie, uint16_t offset) 304184610Salfred{ 305184610Salfred return (libusb20_me_get_1(ie, offset) | 306184610Salfred (libusb20_me_get_1(ie, offset + 1) << 8)); 307184610Salfred} 308184610Salfred 309184610Salfred/*------------------------------------------------------------------------* 310184610Salfred * libusb20_me_encode - encode a message structure 311184610Salfred * 312184610Salfred * Description of parameters: 313184610Salfred * "len" - maximum length of output buffer 314184610Salfred * "ptr" - pointer to output buffer. If NULL, no data will be written 315184610Salfred * "pd" - source structure 316184610Salfred * 317184610Salfred * Return values: 318184610Salfred * 0..65535 - Number of bytes used, limited by the "len" input parameter. 319184610Salfred *------------------------------------------------------------------------*/ 320184610Salfreduint16_t 321184610Salfredlibusb20_me_encode(void *ptr, uint16_t len, const void *pd) 322184610Salfred{ 323184610Salfred const uint8_t *pf; /* pointer to format data */ 324184610Salfred uint8_t *buf; /* pointer to output buffer */ 325184610Salfred 326184610Salfred uint32_t pd_offset; /* decoded structure offset */ 327184610Salfred uint16_t len_old; /* old length */ 328184610Salfred uint16_t pd_count; /* decoded element count */ 329184610Salfred uint8_t me; /* message element */ 330184610Salfred 331184610Salfred /* initialise */ 332184610Salfred 333184610Salfred len_old = len; 334184610Salfred buf = ptr; 335184610Salfred pd_offset = sizeof(void *); 336185087Salfred pf = (*((struct libusb20_me_format *const *)pd))->format; 337184610Salfred 338184610Salfred /* scan */ 339184610Salfred 340184610Salfred while (1) { 341184610Salfred 342184610Salfred /* get information element */ 343184610Salfred 344184610Salfred me = (pf[0]) & LIBUSB20_ME_MASK; 345184610Salfred pd_count = pf[1] | (pf[2] << 8); 346184610Salfred pf += 3; 347184610Salfred 348184610Salfred /* encode the message element */ 349184610Salfred 350184610Salfred switch (me) { 351184610Salfred case LIBUSB20_ME_INT8: 352184610Salfred while (pd_count--) { 353184610Salfred uint8_t temp; 354184610Salfred 355184610Salfred if (len < 1) /* overflow */ 356184610Salfred goto done; 357184610Salfred if (buf) { 358184610Salfred temp = *((const uint8_t *) 359184610Salfred LIBUSB20_ADD_BYTES(pd, pd_offset)); 360184610Salfred buf[0] = temp; 361184610Salfred buf += 1; 362184610Salfred } 363184610Salfred pd_offset += 1; 364184610Salfred len -= 1; 365184610Salfred } 366184610Salfred break; 367184610Salfred 368184610Salfred case LIBUSB20_ME_INT16: 369184610Salfred pd_offset = -((-pd_offset) & ~1); /* align */ 370184610Salfred while (pd_count--) { 371184610Salfred uint16_t temp; 372184610Salfred 373184610Salfred if (len < 2) /* overflow */ 374184610Salfred goto done; 375184610Salfred 376184610Salfred if (buf) { 377184610Salfred temp = *((const uint16_t *) 378184610Salfred LIBUSB20_ADD_BYTES(pd, pd_offset)); 379184610Salfred buf[1] = (temp >> 8) & 0xFF; 380184610Salfred buf[0] = temp & 0xFF; 381184610Salfred buf += 2; 382184610Salfred } 383184610Salfred pd_offset += 2; 384184610Salfred len -= 2; 385184610Salfred } 386184610Salfred break; 387184610Salfred 388184610Salfred case LIBUSB20_ME_INT32: 389184610Salfred pd_offset = -((-pd_offset) & ~3); /* align */ 390184610Salfred while (pd_count--) { 391184610Salfred uint32_t temp; 392184610Salfred 393184610Salfred if (len < 4) /* overflow */ 394184610Salfred goto done; 395184610Salfred if (buf) { 396184610Salfred temp = *((const uint32_t *) 397184610Salfred LIBUSB20_ADD_BYTES(pd, pd_offset)); 398184610Salfred buf[3] = (temp >> 24) & 0xFF; 399184610Salfred buf[2] = (temp >> 16) & 0xFF; 400184610Salfred buf[1] = (temp >> 8) & 0xFF; 401184610Salfred buf[0] = temp & 0xFF; 402184610Salfred buf += 4; 403184610Salfred } 404184610Salfred pd_offset += 4; 405184610Salfred len -= 4; 406184610Salfred } 407184610Salfred break; 408184610Salfred 409184610Salfred case LIBUSB20_ME_INT64: 410184610Salfred pd_offset = -((-pd_offset) & ~7); /* align */ 411184610Salfred while (pd_count--) { 412184610Salfred uint64_t temp; 413184610Salfred 414184610Salfred if (len < 8) /* overflow */ 415184610Salfred goto done; 416184610Salfred if (buf) { 417184610Salfred 418184610Salfred temp = *((const uint64_t *) 419184610Salfred LIBUSB20_ADD_BYTES(pd, pd_offset)); 420184610Salfred buf[7] = (temp >> 56) & 0xFF; 421184610Salfred buf[6] = (temp >> 48) & 0xFF; 422184610Salfred buf[5] = (temp >> 40) & 0xFF; 423184610Salfred buf[4] = (temp >> 32) & 0xFF; 424184610Salfred buf[3] = (temp >> 24) & 0xFF; 425184610Salfred buf[2] = (temp >> 16) & 0xFF; 426184610Salfred buf[1] = (temp >> 8) & 0xFF; 427184610Salfred buf[0] = temp & 0xFF; 428184610Salfred buf += 8; 429184610Salfred } 430184610Salfred pd_offset += 8; 431184610Salfred len -= 8; 432184610Salfred } 433184610Salfred break; 434184610Salfred 435184610Salfred case LIBUSB20_ME_STRUCT: 436184610Salfred pd_offset = -((-pd_offset) & 437184610Salfred ~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */ 438184610Salfred while (pd_count--) { 439184610Salfred void *src_ptr; 440184610Salfred uint16_t src_len; 441184610Salfred struct libusb20_me_struct *ps; 442184610Salfred 443184610Salfred ps = LIBUSB20_ADD_BYTES(pd, pd_offset); 444184610Salfred 445184610Salfred switch (ps->type) { 446184610Salfred case LIBUSB20_ME_IS_RAW: 447184610Salfred src_len = ps->len; 448184610Salfred src_ptr = ps->ptr; 449184610Salfred break; 450184610Salfred 451184610Salfred case LIBUSB20_ME_IS_ENCODED: 452184610Salfred if (ps->len == 0) { 453184610Salfred /* 454184610Salfred * Length is encoded 455184610Salfred * in the data itself 456184610Salfred * and should be 457184610Salfred * correct: 458184610Salfred */ 459234491Shselasky ps->len = 0xFFFF; 460184610Salfred } 461184610Salfred src_len = libusb20_me_get_1(pd, 0); 462184610Salfred src_ptr = LIBUSB20_ADD_BYTES(ps->ptr, 1); 463184610Salfred if (src_len == 0xFF) { 464184610Salfred /* length is escaped */ 465184610Salfred src_len = libusb20_me_get_2(pd, 1); 466184610Salfred src_ptr = 467184610Salfred LIBUSB20_ADD_BYTES(ps->ptr, 3); 468184610Salfred } 469184610Salfred break; 470184610Salfred 471184610Salfred case LIBUSB20_ME_IS_DECODED: 472184610Salfred /* reserve 3 length bytes */ 473184610Salfred src_len = libusb20_me_encode(NULL, 474234491Shselasky 0xFFFF - 3, ps->ptr); 475184610Salfred src_ptr = NULL; 476184610Salfred break; 477184610Salfred 478184610Salfred default: /* empty structure */ 479184610Salfred src_len = 0; 480184610Salfred src_ptr = NULL; 481184610Salfred break; 482184610Salfred } 483184610Salfred 484184610Salfred if (src_len > 0xFE) { 485234491Shselasky if (src_len > (0xFFFF - 3)) 486184610Salfred /* overflow */ 487184610Salfred goto done; 488184610Salfred 489184610Salfred if (len < (src_len + 3)) 490184610Salfred /* overflow */ 491184610Salfred goto done; 492184610Salfred 493184610Salfred if (buf) { 494184610Salfred buf[0] = 0xFF; 495184610Salfred buf[1] = (src_len & 0xFF); 496184610Salfred buf[2] = (src_len >> 8) & 0xFF; 497184610Salfred buf += 3; 498184610Salfred } 499184610Salfred len -= (src_len + 3); 500184610Salfred } else { 501184610Salfred if (len < (src_len + 1)) 502184610Salfred /* overflow */ 503184610Salfred goto done; 504184610Salfred 505184610Salfred if (buf) { 506184610Salfred buf[0] = (src_len & 0xFF); 507184610Salfred buf += 1; 508184610Salfred } 509184610Salfred len -= (src_len + 1); 510184610Salfred } 511184610Salfred 512184610Salfred /* check for buffer and non-zero length */ 513184610Salfred 514184610Salfred if (buf && src_len) { 515184610Salfred if (ps->type == LIBUSB20_ME_IS_DECODED) { 516184610Salfred /* 517184610Salfred * Repeat encode 518184610Salfred * procedure - we have 519184610Salfred * room for the 520184610Salfred * complete structure: 521184610Salfred */ 522284744Saraujo (void) libusb20_me_encode(buf, 523234491Shselasky 0xFFFF - 3, ps->ptr); 524184610Salfred } else { 525184610Salfred bcopy(src_ptr, buf, src_len); 526184610Salfred } 527184610Salfred buf += src_len; 528184610Salfred } 529184610Salfred pd_offset += sizeof(struct libusb20_me_struct); 530184610Salfred } 531184610Salfred break; 532184610Salfred 533184610Salfred default: 534184610Salfred goto done; 535184610Salfred } 536184610Salfred } 537184610Salfreddone: 538184610Salfred return (len_old - len); 539184610Salfred} 540184610Salfred 541184610Salfred/*------------------------------------------------------------------------* 542184610Salfred * libusb20_me_decode - decode a message into a decoded structure 543184610Salfred * 544184610Salfred * Description of parameters: 545184610Salfred * "ptr" - message pointer 546184610Salfred * "len" - message length 547184610Salfred * "pd" - pointer to decoded structure 548184610Salfred * 549184610Salfred * Returns: 550184610Salfred * "0..65535" - number of bytes decoded, limited by "len" 551184610Salfred *------------------------------------------------------------------------*/ 552184610Salfreduint16_t 553184610Salfredlibusb20_me_decode(const void *ptr, uint16_t len, void *pd) 554184610Salfred{ 555184610Salfred const uint8_t *pf; /* pointer to format data */ 556184610Salfred const uint8_t *buf; /* pointer to input buffer */ 557184610Salfred 558184610Salfred uint32_t pd_offset; /* decoded structure offset */ 559184610Salfred uint16_t len_old; /* old length */ 560184610Salfred uint16_t pd_count; /* decoded element count */ 561184610Salfred uint8_t me; /* message element */ 562184610Salfred 563184610Salfred /* initialise */ 564184610Salfred 565184610Salfred len_old = len; 566184610Salfred buf = ptr; 567184610Salfred pd_offset = sizeof(void *); 568184610Salfred pf = (*((struct libusb20_me_format **)pd))->format; 569184610Salfred 570184610Salfred /* scan */ 571184610Salfred 572184610Salfred while (1) { 573184610Salfred 574184610Salfred /* get information element */ 575184610Salfred 576184610Salfred me = (pf[0]) & LIBUSB20_ME_MASK; 577184610Salfred pd_count = pf[1] | (pf[2] << 8); 578184610Salfred pf += 3; 579184610Salfred 580184610Salfred /* decode the message element by type */ 581184610Salfred 582184610Salfred switch (me) { 583184610Salfred case LIBUSB20_ME_INT8: 584184610Salfred while (pd_count--) { 585184610Salfred uint8_t temp; 586184610Salfred 587184610Salfred if (len < 1) { 588184610Salfred len = 0; 589184610Salfred temp = 0; 590184610Salfred } else { 591184610Salfred len -= 1; 592184610Salfred temp = buf[0]; 593184610Salfred buf++; 594184610Salfred } 595184610Salfred *((uint8_t *)LIBUSB20_ADD_BYTES(pd, 596184610Salfred pd_offset)) = temp; 597184610Salfred pd_offset += 1; 598184610Salfred } 599184610Salfred break; 600184610Salfred 601184610Salfred case LIBUSB20_ME_INT16: 602184610Salfred pd_offset = -((-pd_offset) & ~1); /* align */ 603184610Salfred while (pd_count--) { 604184610Salfred uint16_t temp; 605184610Salfred 606184610Salfred if (len < 2) { 607184610Salfred len = 0; 608184610Salfred temp = 0; 609184610Salfred } else { 610184610Salfred len -= 2; 611184610Salfred temp = buf[1] << 8; 612184610Salfred temp |= buf[0]; 613184610Salfred buf += 2; 614184610Salfred } 615184610Salfred *((uint16_t *)LIBUSB20_ADD_BYTES(pd, 616184610Salfred pd_offset)) = temp; 617184610Salfred pd_offset += 2; 618184610Salfred } 619184610Salfred break; 620184610Salfred 621184610Salfred case LIBUSB20_ME_INT32: 622184610Salfred pd_offset = -((-pd_offset) & ~3); /* align */ 623184610Salfred while (pd_count--) { 624184610Salfred uint32_t temp; 625184610Salfred 626184610Salfred if (len < 4) { 627184610Salfred len = 0; 628184610Salfred temp = 0; 629184610Salfred } else { 630184610Salfred len -= 4; 631184610Salfred temp = buf[3] << 24; 632184610Salfred temp |= buf[2] << 16; 633184610Salfred temp |= buf[1] << 8; 634184610Salfred temp |= buf[0]; 635184610Salfred buf += 4; 636184610Salfred } 637184610Salfred 638184610Salfred *((uint32_t *)LIBUSB20_ADD_BYTES(pd, 639184610Salfred pd_offset)) = temp; 640184610Salfred pd_offset += 4; 641184610Salfred } 642184610Salfred break; 643184610Salfred 644184610Salfred case LIBUSB20_ME_INT64: 645184610Salfred pd_offset = -((-pd_offset) & ~7); /* align */ 646184610Salfred while (pd_count--) { 647184610Salfred uint64_t temp; 648184610Salfred 649184610Salfred if (len < 8) { 650184610Salfred len = 0; 651184610Salfred temp = 0; 652184610Salfred } else { 653184610Salfred len -= 8; 654184610Salfred temp = ((uint64_t)buf[7]) << 56; 655184610Salfred temp |= ((uint64_t)buf[6]) << 48; 656184610Salfred temp |= ((uint64_t)buf[5]) << 40; 657184610Salfred temp |= ((uint64_t)buf[4]) << 32; 658184610Salfred temp |= buf[3] << 24; 659184610Salfred temp |= buf[2] << 16; 660184610Salfred temp |= buf[1] << 8; 661184610Salfred temp |= buf[0]; 662184610Salfred buf += 8; 663184610Salfred } 664184610Salfred 665184610Salfred *((uint64_t *)LIBUSB20_ADD_BYTES(pd, 666184610Salfred pd_offset)) = temp; 667184610Salfred pd_offset += 8; 668184610Salfred } 669184610Salfred break; 670184610Salfred 671184610Salfred case LIBUSB20_ME_STRUCT: 672184610Salfred pd_offset = -((-pd_offset) & 673184610Salfred ~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */ 674184610Salfred while (pd_count--) { 675184610Salfred uint16_t temp; 676184610Salfred struct libusb20_me_struct *ps; 677184610Salfred 678184610Salfred ps = LIBUSB20_ADD_BYTES(pd, pd_offset); 679184610Salfred 680184610Salfred if (ps->type == LIBUSB20_ME_IS_ENCODED) { 681184610Salfred /* 682184610Salfred * Pre-store a de-constified 683184610Salfred * pointer to the raw 684184610Salfred * structure: 685184610Salfred */ 686184610Salfred ps->ptr = LIBUSB20_ADD_BYTES(buf, 0); 687184610Salfred 688184610Salfred /* 689184610Salfred * Get the correct number of 690184610Salfred * length bytes: 691184610Salfred */ 692184610Salfred if (len != 0) { 693184610Salfred if (buf[0] == 0xFF) { 694184610Salfred ps->len = 3; 695184610Salfred } else { 696184610Salfred ps->len = 1; 697184610Salfred } 698184610Salfred } else { 699184610Salfred ps->len = 0; 700184610Salfred } 701184610Salfred } 702184610Salfred /* get the structure length */ 703184610Salfred 704184610Salfred if (len != 0) { 705184610Salfred if (buf[0] == 0xFF) { 706184610Salfred if (len < 3) { 707184610Salfred len = 0; 708184610Salfred temp = 0; 709184610Salfred } else { 710184610Salfred len -= 3; 711184610Salfred temp = buf[1] | 712184610Salfred (buf[2] << 8); 713184610Salfred buf += 3; 714184610Salfred } 715184610Salfred } else { 716184610Salfred len -= 1; 717184610Salfred temp = buf[0]; 718184610Salfred buf += 1; 719184610Salfred } 720184610Salfred } else { 721184610Salfred len = 0; 722184610Salfred temp = 0; 723184610Salfred } 724184610Salfred /* check for invalid length */ 725184610Salfred 726184610Salfred if (temp > len) { 727184610Salfred len = 0; 728184610Salfred temp = 0; 729184610Salfred } 730184610Salfred /* check wanted structure type */ 731184610Salfred 732184610Salfred switch (ps->type) { 733184610Salfred case LIBUSB20_ME_IS_ENCODED: 734184610Salfred /* check for zero length */ 735184610Salfred if (temp == 0) { 736184610Salfred /* 737184610Salfred * The pointer must 738184610Salfred * be valid: 739184610Salfred */ 740184610Salfred ps->ptr = LIBUSB20_ADD_BYTES( 741184610Salfred libusb20_me_encode_empty, 0); 742184610Salfred ps->len = 1; 743184610Salfred } else { 744184610Salfred ps->len += temp; 745184610Salfred } 746184610Salfred break; 747184610Salfred 748184610Salfred case LIBUSB20_ME_IS_RAW: 749184610Salfred /* update length and pointer */ 750184610Salfred ps->len = temp; 751184610Salfred ps->ptr = LIBUSB20_ADD_BYTES(buf, 0); 752184610Salfred break; 753184610Salfred 754184610Salfred case LIBUSB20_ME_IS_EMPTY: 755184610Salfred case LIBUSB20_ME_IS_DECODED: 756184610Salfred /* check for non-zero length */ 757184610Salfred if (temp != 0) { 758184610Salfred /* update type */ 759184610Salfred ps->type = LIBUSB20_ME_IS_DECODED; 760184610Salfred ps->len = 0; 761184610Salfred /* 762184610Salfred * Recursivly decode 763184610Salfred * the next structure 764184610Salfred */ 765284744Saraujo (void) libusb20_me_decode(buf, 766184610Salfred temp, ps->ptr); 767184610Salfred } else { 768184610Salfred /* update type */ 769184610Salfred ps->type = LIBUSB20_ME_IS_EMPTY; 770184610Salfred ps->len = 0; 771184610Salfred } 772184610Salfred break; 773184610Salfred 774184610Salfred default: 775184610Salfred /* 776184610Salfred * nothing to do - should 777184610Salfred * not happen 778184610Salfred */ 779184610Salfred ps->ptr = NULL; 780184610Salfred ps->len = 0; 781184610Salfred break; 782184610Salfred } 783184610Salfred buf += temp; 784184610Salfred len -= temp; 785184610Salfred pd_offset += sizeof(struct libusb20_me_struct); 786184610Salfred } 787184610Salfred break; 788184610Salfred 789184610Salfred default: 790184610Salfred goto done; 791184610Salfred } 792184610Salfred } 793184610Salfreddone: 794184610Salfred return (len_old - len); 795184610Salfred} 796