libusb20_desc.c revision 285830
118334Speter/* $FreeBSD: releng/10.2/lib/libusb/libusb20_desc.c 248236 2013-03-13 12:23:14Z hselasky $ */ 290289Sobrien/*- 390289Sobrien * Copyright (c) 2008 Hans Petter Selasky. All rights reserved. 418334Speter * 518334Speter * Redistribution and use in source and binary forms, with or without 618334Speter * modification, are permitted provided that the following conditions 718334Speter * are met: 818334Speter * 1. Redistributions of source code must retain the above copyright 918334Speter * notice, this list of conditions and the following disclaimer. 1018334Speter * 2. Redistributions in binary form must reproduce the above copyright 1118334Speter * notice, this list of conditions and the following disclaimer in the 1218334Speter * documentation and/or other materials provided with the distribution. 1318334Speter * 1418334Speter * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 1518334Speter * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1618334Speter * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 1718334Speter * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 1818334Speter * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 1918334Speter * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 2018334Speter * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 2118334Speter * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 2218334Speter * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 2318334Speter * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 2418334Speter * SUCH DAMAGE. 2518334Speter */ 2618334Speter 2750643Sobrien#ifdef LIBUSB_GLOBAL_INCLUDE_FILE 2818334Speter#include LIBUSB_GLOBAL_INCLUDE_FILE 2918334Speter#else 3090289Sobrien#include <stdio.h> 3190289Sobrien#include <stdlib.h> 3218334Speter#include <string.h> 3318334Speter#include <time.h> 3418334Speter#include <sys/queue.h> 3550643Sobrien#endif 3650643Sobrien 3750643Sobrien#include "libusb20.h" 3818334Speter#include "libusb20_desc.h" 3990289Sobrien#include "libusb20_int.h" 4090289Sobrien 4190289Sobrienstatic const uint32_t libusb20_me_encode_empty[2]; /* dummy */ 4290289Sobrien 4390289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_DEVICE_DESC); 4490289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_ENDPOINT_DESC); 4590289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_INTERFACE_DESC); 4690289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONFIG_DESC); 4790289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONTROL_SETUP); 4890289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_ENDPT_COMP_DESC); 4990289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_USB_20_DEVCAP_DESC); 5090289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_USB_DEVCAP_DESC); 5190289SobrienLIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_BOS_DESCRIPTOR); 5290289Sobrien 5390289Sobrien/*------------------------------------------------------------------------* 5418334Speter * libusb20_parse_config_desc 5518334Speter * 5618334Speter * Return values: 5718334Speter * NULL: Out of memory. 5890289Sobrien * Else: A valid config structure pointer which must be passed to "free()" 5990289Sobrien *------------------------------------------------------------------------*/ 6018334Speterstruct libusb20_config * 6118334Speterlibusb20_parse_config_desc(const void *config_desc) 6250643Sobrien{ 6318334Speter struct libusb20_config *lub_config; 6490289Sobrien struct libusb20_interface *lub_interface; 6518334Speter struct libusb20_interface *lub_alt_interface; 6690289Sobrien struct libusb20_interface *last_if; 6790289Sobrien struct libusb20_endpoint *lub_endpoint; 6890289Sobrien struct libusb20_endpoint *last_ep; 6990289Sobrien 7090289Sobrien struct libusb20_me_struct pcdesc; 7190289Sobrien const uint8_t *ptr; 7290289Sobrien uint32_t size; 7318334Speter uint16_t niface_no_alt; 7490289Sobrien uint16_t niface; 7590289Sobrien uint16_t nendpoint; 7690289Sobrien uint16_t iface_no; 7790289Sobrien 7890289Sobrien ptr = config_desc; 7918334Speter if (ptr[1] != LIBUSB20_DT_CONFIG) { 8090289Sobrien return (NULL); /* not config descriptor */ 8190289Sobrien } 8290289Sobrien /* 8318334Speter * The first "bInterfaceNumber" should never have the value 0xff. 8490289Sobrien * Then it is corrupt. 8590289Sobrien */ 8618334Speter niface_no_alt = 0; 8718334Speter nendpoint = 0; 8890289Sobrien niface = 0; 8990289Sobrien iface_no = 0xFFFF; 9018334Speter ptr = NULL; 9150643Sobrien 9290289Sobrien /* get "wTotalLength" and setup "pcdesc" */ 9318334Speter pcdesc.ptr = LIBUSB20_ADD_BYTES(config_desc, 0); 9490289Sobrien pcdesc.len = 9518334Speter ((const uint8_t *)config_desc)[2] | 9690289Sobrien (((const uint8_t *)config_desc)[3] << 8); 9790289Sobrien pcdesc.type = LIBUSB20_ME_IS_RAW; 9890289Sobrien 9990289Sobrien /* descriptor pre-scan */ 10018334Speter while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) { 10150643Sobrien if (ptr[1] == LIBUSB20_DT_ENDPOINT) { 10290289Sobrien nendpoint++; 10350643Sobrien } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) { 10418334Speter niface++; 10590289Sobrien /* check "bInterfaceNumber" */ 10690289Sobrien if (ptr[2] != iface_no) { 10750643Sobrien iface_no = ptr[2]; 10850643Sobrien niface_no_alt++; 10918334Speter } 11050643Sobrien } 11150643Sobrien } 11250643Sobrien 11318334Speter /* sanity checking */ 11450643Sobrien if (niface >= 256) { 11550643Sobrien return (NULL); /* corrupt */ 11618334Speter } 11790289Sobrien if (nendpoint >= 256) { 11818334Speter return (NULL); /* corrupt */ 11918334Speter } 12090289Sobrien size = sizeof(*lub_config) + 12190289Sobrien (niface * sizeof(*lub_interface)) + 12218334Speter (nendpoint * sizeof(*lub_endpoint)) + 12318334Speter pcdesc.len; 12490289Sobrien 12518334Speter lub_config = malloc(size); 12618334Speter if (lub_config == NULL) { 12718334Speter return (NULL); /* out of memory */ 12818334Speter } 12990289Sobrien /* make sure memory is initialised */ 13090289Sobrien memset(lub_config, 0, size); 13118334Speter 13290289Sobrien lub_interface = (void *)(lub_config + 1); 13390289Sobrien lub_alt_interface = (void *)(lub_interface + niface_no_alt); 13490289Sobrien lub_endpoint = (void *)(lub_interface + niface); 13590289Sobrien 13618334Speter /* 13790289Sobrien * Make a copy of the config descriptor, so that the caller can free 13890289Sobrien * the inital config descriptor pointer! 13918334Speter */ 14090289Sobrien ptr = (void *)(lub_endpoint + nendpoint); 14190289Sobrien memcpy(LIBUSB20_ADD_BYTES(ptr, 0), config_desc, pcdesc.len); 14250643Sobrien pcdesc.ptr = LIBUSB20_ADD_BYTES(ptr, 0); 14390289Sobrien config_desc = LIBUSB20_ADD_BYTES(ptr, 0); 14490289Sobrien 14590289Sobrien /* init config structure */ 14690289Sobrien 14718334Speter ptr = config_desc; 14818334Speter 14990289Sobrien LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc); 15090289Sobrien 15150643Sobrien if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) { 15218334Speter /* ignore */ 15390289Sobrien } 15418334Speter lub_config->num_interface = 0; 15590289Sobrien lub_config->interface = lub_interface; 15690289Sobrien lub_config->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 15790289Sobrien lub_config->extra.len = -ptr[0]; 15890289Sobrien lub_config->extra.type = LIBUSB20_ME_IS_RAW; 15990289Sobrien 16090289Sobrien /* reset states */ 16118334Speter niface = 0; 16290289Sobrien iface_no = 0xFFFF; 16390289Sobrien ptr = NULL; 16490289Sobrien lub_interface--; 16518334Speter lub_endpoint--; 16690289Sobrien last_if = NULL; 16790289Sobrien last_ep = NULL; 16890289Sobrien 16918334Speter /* descriptor pre-scan */ 17018334Speter while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) { 17190289Sobrien if (ptr[1] == LIBUSB20_DT_ENDPOINT) { 17290289Sobrien if (last_if) { 17390289Sobrien lub_endpoint++; 17490289Sobrien last_ep = lub_endpoint; 17590289Sobrien last_if->num_endpoints++; 17690289Sobrien 17750643Sobrien LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC, &last_ep->desc); 17890289Sobrien 17990289Sobrien if (libusb20_me_decode(ptr, ptr[0], &last_ep->desc)) { 18090289Sobrien /* ignore */ 18190289Sobrien } 18290289Sobrien last_ep->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 18390289Sobrien last_ep->extra.len = 0; 18490289Sobrien last_ep->extra.type = LIBUSB20_ME_IS_RAW; 18590289Sobrien } else { 18690289Sobrien lub_config->extra.len += ptr[0]; 18790289Sobrien } 18890289Sobrien 18950643Sobrien } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) { 19050643Sobrien if (ptr[2] != iface_no) { 19190289Sobrien /* new interface */ 19290289Sobrien iface_no = ptr[2]; 19350643Sobrien lub_interface++; 19450643Sobrien lub_config->num_interface++; 19590289Sobrien last_if = lub_interface; 19690289Sobrien niface++; 19750643Sobrien } else { 19850643Sobrien /* one more alternate setting */ 19990289Sobrien lub_interface->num_altsetting++; 20090289Sobrien last_if = lub_alt_interface; 20150643Sobrien lub_alt_interface++; 20250643Sobrien } 20350643Sobrien 20450643Sobrien LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC, &last_if->desc); 20590289Sobrien 20690289Sobrien if (libusb20_me_decode(ptr, ptr[0], &last_if->desc)) { 20790289Sobrien /* ignore */ 20890289Sobrien } 20950643Sobrien /* 21050643Sobrien * Sometimes USB devices have corrupt interface 21190289Sobrien * descriptors and we need to overwrite the provided 21290289Sobrien * interface number! 21390289Sobrien */ 21450643Sobrien last_if->desc.bInterfaceNumber = niface - 1; 21550643Sobrien last_if->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]); 21650643Sobrien last_if->extra.len = 0; 21750643Sobrien last_if->extra.type = LIBUSB20_ME_IS_RAW; 21850643Sobrien last_if->endpoints = lub_endpoint + 1; 21950643Sobrien last_if->altsetting = lub_alt_interface; 22090289Sobrien last_if->num_altsetting = 0; 22190289Sobrien last_if->num_endpoints = 0; 22250643Sobrien last_ep = NULL; 22390289Sobrien } else { 22450643Sobrien /* unknown descriptor */ 22550643Sobrien if (last_if) { 22690289Sobrien if (last_ep) { 22790289Sobrien last_ep->extra.len += ptr[0]; 22850643Sobrien } else { 22990289Sobrien last_if->extra.len += ptr[0]; 23090289Sobrien } 23190289Sobrien } else { 23290289Sobrien lub_config->extra.len += ptr[0]; 23350643Sobrien } 23490289Sobrien } 23590289Sobrien } 23690289Sobrien return (lub_config); 23790289Sobrien} 23890289Sobrien 23990289Sobrien/*------------------------------------------------------------------------* 24090289Sobrien * libusb20_desc_foreach 24190289Sobrien * 24290289Sobrien * Safe traversal of USB descriptors. 24390289Sobrien * 24490289Sobrien * Return values: 24590289Sobrien * NULL: End of descriptors 24690289Sobrien * Else: Pointer to next descriptor 24790289Sobrien *------------------------------------------------------------------------*/ 24890289Sobrienconst uint8_t * 24990289Sobrienlibusb20_desc_foreach(const struct libusb20_me_struct *pdesc, 25090289Sobrien const uint8_t *psubdesc) 25190289Sobrien{ 25290289Sobrien const uint8_t *start; 25390289Sobrien const uint8_t *end; 25490289Sobrien const uint8_t *desc_next; 25590289Sobrien 25690289Sobrien /* be NULL safe */ 25790289Sobrien if (pdesc == NULL) 25890289Sobrien return (NULL); 25990289Sobrien 26090289Sobrien start = (const uint8_t *)pdesc->ptr; 26190289Sobrien end = LIBUSB20_ADD_BYTES(start, pdesc->len); 26290289Sobrien 26390289Sobrien /* get start of next descriptor */ 26490289Sobrien if (psubdesc == NULL) 26590289Sobrien psubdesc = start; 26690289Sobrien else 26790289Sobrien psubdesc = psubdesc + psubdesc[0]; 26890289Sobrien 26990289Sobrien /* check that the next USB descriptor is within the range */ 27050643Sobrien if ((psubdesc < start) || (psubdesc >= end)) 27150643Sobrien return (NULL); /* out of range, or EOD */ 27290289Sobrien 27390289Sobrien /* check start of the second next USB descriptor, if any */ 27490289Sobrien desc_next = psubdesc + psubdesc[0]; 27590289Sobrien if ((desc_next < start) || (desc_next > end)) 27690289Sobrien return (NULL); /* out of range */ 27790289Sobrien 27818334Speter /* check minimum descriptor length */ 27990289Sobrien if (psubdesc[0] < 3) 28090289Sobrien return (NULL); /* too short descriptor */ 28118334Speter 28290289Sobrien return (psubdesc); /* return start of next descriptor */ 28390289Sobrien} 28490289Sobrien 28590289Sobrien/*------------------------------------------------------------------------* 28690289Sobrien * libusb20_me_get_1 - safety wrapper to read out one byte 28790289Sobrien *------------------------------------------------------------------------*/ 28850643Sobrienuint8_t 28990289Sobrienlibusb20_me_get_1(const struct libusb20_me_struct *ie, uint16_t offset) 29050643Sobrien{ 29190289Sobrien if (offset < ie->len) { 29250643Sobrien return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie->ptr, offset))); 29390289Sobrien } 29490289Sobrien return (0); 29590289Sobrien} 29690289Sobrien 29790289Sobrien/*------------------------------------------------------------------------* 29890289Sobrien * libusb20_me_get_2 - safety wrapper to read out one word 29990289Sobrien *------------------------------------------------------------------------*/ 30090289Sobrienuint16_t 30190289Sobrienlibusb20_me_get_2(const struct libusb20_me_struct *ie, uint16_t offset) 30290289Sobrien{ 30390289Sobrien return (libusb20_me_get_1(ie, offset) | 30490289Sobrien (libusb20_me_get_1(ie, offset + 1) << 8)); 30590289Sobrien} 30650643Sobrien 30750643Sobrien/*------------------------------------------------------------------------* 30890289Sobrien * libusb20_me_encode - encode a message structure 30990289Sobrien * 31090289Sobrien * Description of parameters: 31190289Sobrien * "len" - maximum length of output buffer 31290289Sobrien * "ptr" - pointer to output buffer. If NULL, no data will be written 31350643Sobrien * "pd" - source structure 31490289Sobrien * 31590289Sobrien * Return values: 31690289Sobrien * 0..65535 - Number of bytes used, limited by the "len" input parameter. 31790289Sobrien *------------------------------------------------------------------------*/ 31890289Sobrienuint16_t 31990289Sobrienlibusb20_me_encode(void *ptr, uint16_t len, const void *pd) 32090289Sobrien{ 32190289Sobrien const uint8_t *pf; /* pointer to format data */ 32290289Sobrien uint8_t *buf; /* pointer to output buffer */ 32390289Sobrien 32490289Sobrien uint32_t pd_offset; /* decoded structure offset */ 32590289Sobrien uint16_t len_old; /* old length */ 32690289Sobrien uint16_t pd_count; /* decoded element count */ 32790289Sobrien uint8_t me; /* message element */ 32890289Sobrien 32950643Sobrien /* initialise */ 33050643Sobrien 33190289Sobrien len_old = len; 33250643Sobrien buf = ptr; 33350643Sobrien pd_offset = sizeof(void *); 33490289Sobrien pf = (*((struct libusb20_me_format *const *)pd))->format; 33590289Sobrien 33690289Sobrien /* scan */ 33718334Speter 33850643Sobrien while (1) { 33990289Sobrien 34018334Speter /* get information element */ 34190289Sobrien 34290289Sobrien me = (pf[0]) & LIBUSB20_ME_MASK; 34390289Sobrien pd_count = pf[1] | (pf[2] << 8); 34418334Speter pf += 3; 34590289Sobrien 34690289Sobrien /* encode the message element */ 34790289Sobrien 34890289Sobrien switch (me) { 34990289Sobrien case LIBUSB20_ME_INT8: 35090289Sobrien while (pd_count--) { 35190289Sobrien uint8_t temp; 35218334Speter 35390289Sobrien if (len < 1) /* overflow */ 35490289Sobrien goto done; 35518334Speter if (buf) { 35690289Sobrien temp = *((const uint8_t *) 35790289Sobrien LIBUSB20_ADD_BYTES(pd, pd_offset)); 35890289Sobrien buf[0] = temp; 35990289Sobrien buf += 1; 36090289Sobrien } 36118334Speter pd_offset += 1; 36290289Sobrien len -= 1; 36318334Speter } 36490289Sobrien break; 36518334Speter 36690289Sobrien case LIBUSB20_ME_INT16: 36790289Sobrien pd_offset = -((-pd_offset) & ~1); /* align */ 36890289Sobrien while (pd_count--) { 36950643Sobrien uint16_t temp; 37090289Sobrien 37190289Sobrien if (len < 2) /* overflow */ 37290289Sobrien goto done; 37390289Sobrien 37490289Sobrien if (buf) { 37590289Sobrien temp = *((const uint16_t *) 37618334Speter LIBUSB20_ADD_BYTES(pd, pd_offset)); 37790289Sobrien buf[1] = (temp >> 8) & 0xFF; 37890289Sobrien buf[0] = temp & 0xFF; 37990289Sobrien buf += 2; 38018334Speter } 38190289Sobrien pd_offset += 2; 38290289Sobrien len -= 2; 38390289Sobrien } 38490289Sobrien break; 38518334Speter 38690289Sobrien case LIBUSB20_ME_INT32: 38718334Speter pd_offset = -((-pd_offset) & ~3); /* align */ 38890289Sobrien while (pd_count--) { 38990289Sobrien uint32_t temp; 39090289Sobrien 39190289Sobrien if (len < 4) /* overflow */ 39290289Sobrien goto done; 39390289Sobrien if (buf) { 39490289Sobrien temp = *((const uint32_t *) 39518334Speter LIBUSB20_ADD_BYTES(pd, pd_offset)); 39690289Sobrien buf[3] = (temp >> 24) & 0xFF; 39790289Sobrien buf[2] = (temp >> 16) & 0xFF; 39818334Speter buf[1] = (temp >> 8) & 0xFF; 39990289Sobrien buf[0] = temp & 0xFF; 40090289Sobrien buf += 4; 40190289Sobrien } 40218334Speter pd_offset += 4; 40390289Sobrien len -= 4; 40490289Sobrien } 40590289Sobrien break; 40690289Sobrien 40718334Speter case LIBUSB20_ME_INT64: 40890289Sobrien pd_offset = -((-pd_offset) & ~7); /* align */ 40990289Sobrien while (pd_count--) { 41090289Sobrien uint64_t temp; 41190289Sobrien 41218334Speter if (len < 8) /* overflow */ 41390289Sobrien goto done; 41450643Sobrien if (buf) { 41590289Sobrien 41690289Sobrien temp = *((const uint64_t *) 41790289Sobrien LIBUSB20_ADD_BYTES(pd, pd_offset)); 41890289Sobrien buf[7] = (temp >> 56) & 0xFF; 41990289Sobrien buf[6] = (temp >> 48) & 0xFF; 42090289Sobrien buf[5] = (temp >> 40) & 0xFF; 42190289Sobrien buf[4] = (temp >> 32) & 0xFF; 42290289Sobrien buf[3] = (temp >> 24) & 0xFF; 42390289Sobrien buf[2] = (temp >> 16) & 0xFF; 42450643Sobrien buf[1] = (temp >> 8) & 0xFF; 42590289Sobrien buf[0] = temp & 0xFF; 42690289Sobrien buf += 8; 42790289Sobrien } 42890289Sobrien pd_offset += 8; 42990289Sobrien len -= 8; 43090289Sobrien } 43190289Sobrien break; 43290289Sobrien 43390289Sobrien case LIBUSB20_ME_STRUCT: 43490289Sobrien pd_offset = -((-pd_offset) & 43590289Sobrien ~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */ 43650643Sobrien while (pd_count--) { 43750643Sobrien void *src_ptr; 43890289Sobrien uint16_t src_len; 43918334Speter struct libusb20_me_struct *ps; 44090289Sobrien 44190289Sobrien ps = LIBUSB20_ADD_BYTES(pd, pd_offset); 44290289Sobrien 44390289Sobrien switch (ps->type) { 44418334Speter case LIBUSB20_ME_IS_RAW: 44590289Sobrien src_len = ps->len; 44690289Sobrien src_ptr = ps->ptr; 44718334Speter break; 44890289Sobrien 44918334Speter case LIBUSB20_ME_IS_ENCODED: 45018334Speter if (ps->len == 0) { 45118334Speter /* 45250643Sobrien * Length is encoded 45350643Sobrien * in the data itself 45450643Sobrien * and should be 45518334Speter * correct: 45618334Speter */ 45750643Sobrien ps->len = 0xFFFF; 45818334Speter } 45950643Sobrien src_len = libusb20_me_get_1(pd, 0); 46018334Speter src_ptr = LIBUSB20_ADD_BYTES(ps->ptr, 1); 46118334Speter if (src_len == 0xFF) { 46290289Sobrien /* length is escaped */ 46390289Sobrien src_len = libusb20_me_get_2(pd, 1); 46490289Sobrien src_ptr = 46590289Sobrien LIBUSB20_ADD_BYTES(ps->ptr, 3); 46690289Sobrien } 46790289Sobrien break; 46890289Sobrien 46918334Speter case LIBUSB20_ME_IS_DECODED: 47090289Sobrien /* reserve 3 length bytes */ 47190289Sobrien src_len = libusb20_me_encode(NULL, 47290289Sobrien 0xFFFF - 3, ps->ptr); 47390289Sobrien src_ptr = NULL; 47490289Sobrien break; 47590289Sobrien 47650643Sobrien default: /* empty structure */ 47718334Speter src_len = 0; 47890289Sobrien src_ptr = NULL; 47990289Sobrien break; 48090289Sobrien } 48190289Sobrien 48290289Sobrien if (src_len > 0xFE) { 48390289Sobrien if (src_len > (0xFFFF - 3)) 48418334Speter /* overflow */ 48590289Sobrien goto done; 48618334Speter 48790289Sobrien if (len < (src_len + 3)) 48890289Sobrien /* overflow */ 48990289Sobrien goto done; 49090289Sobrien 49190289Sobrien if (buf) { 49290289Sobrien buf[0] = 0xFF; 49318334Speter buf[1] = (src_len & 0xFF); 49490289Sobrien buf[2] = (src_len >> 8) & 0xFF; 49518334Speter buf += 3; 49618334Speter } 49790289Sobrien len -= (src_len + 3); 49890289Sobrien } else { 49990289Sobrien if (len < (src_len + 1)) 50090289Sobrien /* overflow */ 50190289Sobrien goto done; 50218334Speter 50390289Sobrien if (buf) { 50418334Speter buf[0] = (src_len & 0xFF); 50590289Sobrien buf += 1; 50690289Sobrien } 50790289Sobrien len -= (src_len + 1); 50890289Sobrien } 50950643Sobrien 51090289Sobrien /* check for buffer and non-zero length */ 511110621Skan 51290289Sobrien if (buf && src_len) { 51350643Sobrien if (ps->type == LIBUSB20_ME_IS_DECODED) { 51490289Sobrien /* 51590289Sobrien * Repeat encode 51690289Sobrien * procedure - we have 51790289Sobrien * room for the 51818334Speter * complete structure: 51990289Sobrien */ 52090289Sobrien uint16_t dummy; 52190289Sobrien 52250643Sobrien dummy = libusb20_me_encode(buf, 52390289Sobrien 0xFFFF - 3, ps->ptr); 52490289Sobrien } else { 52590289Sobrien bcopy(src_ptr, buf, src_len); 52690289Sobrien } 52790289Sobrien buf += src_len; 52850643Sobrien } 52990289Sobrien pd_offset += sizeof(struct libusb20_me_struct); 53050643Sobrien } 53150643Sobrien break; 53250643Sobrien 53318334Speter default: 53490289Sobrien goto done; 53590289Sobrien } 53690289Sobrien } 53750643Sobriendone: 53818334Speter return (len_old - len); 53990289Sobrien} 54018334Speter 54190289Sobrien/*------------------------------------------------------------------------* 54218334Speter * libusb20_me_decode - decode a message into a decoded structure 54390289Sobrien * 54450643Sobrien * Description of parameters: 54590289Sobrien * "ptr" - message pointer 54690289Sobrien * "len" - message length 54790289Sobrien * "pd" - pointer to decoded structure 54818334Speter * 54990289Sobrien * Returns: 55018334Speter * "0..65535" - number of bytes decoded, limited by "len" 55190289Sobrien *------------------------------------------------------------------------*/ 55290289Sobrienuint16_t 55390289Sobrienlibusb20_me_decode(const void *ptr, uint16_t len, void *pd) 55490289Sobrien{ 55590289Sobrien const uint8_t *pf; /* pointer to format data */ 55690289Sobrien const uint8_t *buf; /* pointer to input buffer */ 55790289Sobrien 55890289Sobrien uint32_t pd_offset; /* decoded structure offset */ 55990289Sobrien uint16_t len_old; /* old length */ 56090289Sobrien uint16_t pd_count; /* decoded element count */ 56150643Sobrien uint8_t me; /* message element */ 56290289Sobrien 56390289Sobrien /* initialise */ 56450643Sobrien 56518334Speter len_old = len; 56690289Sobrien buf = ptr; 56790289Sobrien pd_offset = sizeof(void *); 56890289Sobrien pf = (*((struct libusb20_me_format **)pd))->format; 56990289Sobrien 57050643Sobrien /* scan */ 57190289Sobrien 57290289Sobrien while (1) { 57390289Sobrien 57490289Sobrien /* get information element */ 57590289Sobrien 57690289Sobrien me = (pf[0]) & LIBUSB20_ME_MASK; 57790289Sobrien pd_count = pf[1] | (pf[2] << 8); 57890289Sobrien pf += 3; 57990289Sobrien 58090289Sobrien /* decode the message element by type */ 58118334Speter 58290289Sobrien switch (me) { 58390289Sobrien case LIBUSB20_ME_INT8: 58490289Sobrien while (pd_count--) { 58590289Sobrien uint8_t temp; 58690289Sobrien 58790289Sobrien if (len < 1) { 58890289Sobrien len = 0; 58990289Sobrien temp = 0; 59090289Sobrien } else { 59190289Sobrien len -= 1; 59218334Speter temp = buf[0]; 59390289Sobrien buf++; 59490289Sobrien } 59590289Sobrien *((uint8_t *)LIBUSB20_ADD_BYTES(pd, 59690289Sobrien pd_offset)) = temp; 59790289Sobrien pd_offset += 1; 59890289Sobrien } 59990289Sobrien break; 60090289Sobrien 60190289Sobrien case LIBUSB20_ME_INT16: 60290289Sobrien pd_offset = -((-pd_offset) & ~1); /* align */ 60390289Sobrien while (pd_count--) { 60490289Sobrien uint16_t temp; 60590289Sobrien 60618334Speter if (len < 2) { 60790289Sobrien len = 0; 60890289Sobrien temp = 0; 60990289Sobrien } else { 61090289Sobrien len -= 2; 61190289Sobrien temp = buf[1] << 8; 61290289Sobrien temp |= buf[0]; 61390289Sobrien buf += 2; 61490289Sobrien } 61590289Sobrien *((uint16_t *)LIBUSB20_ADD_BYTES(pd, 61618334Speter pd_offset)) = temp; 61790289Sobrien pd_offset += 2; 61818334Speter } 61990289Sobrien break; 62090289Sobrien 62190289Sobrien case LIBUSB20_ME_INT32: 62218334Speter pd_offset = -((-pd_offset) & ~3); /* align */ 62390289Sobrien while (pd_count--) { 62490289Sobrien uint32_t temp; 62590289Sobrien 62690289Sobrien if (len < 4) { 62790289Sobrien len = 0; 62818334Speter temp = 0; 62990289Sobrien } else { 63090289Sobrien len -= 4; 63190289Sobrien temp = buf[3] << 24; 63290289Sobrien temp |= buf[2] << 16; 63390289Sobrien temp |= buf[1] << 8; 63490289Sobrien temp |= buf[0]; 63518334Speter buf += 4; 63690289Sobrien } 63718334Speter 63890289Sobrien *((uint32_t *)LIBUSB20_ADD_BYTES(pd, 63990289Sobrien pd_offset)) = temp; 64090289Sobrien pd_offset += 4; 64190289Sobrien } 64290289Sobrien break; 64390289Sobrien 64490289Sobrien case LIBUSB20_ME_INT64: 64590289Sobrien pd_offset = -((-pd_offset) & ~7); /* align */ 64690289Sobrien while (pd_count--) { 64790289Sobrien uint64_t temp; 64890289Sobrien 64918334Speter if (len < 8) { 65090289Sobrien len = 0; 65190289Sobrien temp = 0; 65290289Sobrien } else { 65390289Sobrien len -= 8; 65490289Sobrien temp = ((uint64_t)buf[7]) << 56; 65590289Sobrien temp |= ((uint64_t)buf[6]) << 48; 65690289Sobrien temp |= ((uint64_t)buf[5]) << 40; 65718334Speter temp |= ((uint64_t)buf[4]) << 32; 65890289Sobrien temp |= buf[3] << 24; 65990289Sobrien temp |= buf[2] << 16; 66018334Speter temp |= buf[1] << 8; 66190289Sobrien temp |= buf[0]; 66290289Sobrien buf += 8; 66390289Sobrien } 66418334Speter 66590289Sobrien *((uint64_t *)LIBUSB20_ADD_BYTES(pd, 66690289Sobrien pd_offset)) = temp; 66718334Speter pd_offset += 8; 66890289Sobrien } 66918334Speter break; 67090289Sobrien 67190289Sobrien case LIBUSB20_ME_STRUCT: 67290289Sobrien pd_offset = -((-pd_offset) & 67390289Sobrien ~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */ 67490289Sobrien while (pd_count--) { 67590289Sobrien uint16_t temp; 67690289Sobrien uint16_t dummy; 67790289Sobrien struct libusb20_me_struct *ps; 67890289Sobrien 67990289Sobrien ps = LIBUSB20_ADD_BYTES(pd, pd_offset); 68090289Sobrien 68190289Sobrien if (ps->type == LIBUSB20_ME_IS_ENCODED) { 68290289Sobrien /* 68390289Sobrien * Pre-store a de-constified 68490289Sobrien * pointer to the raw 68550643Sobrien * structure: 68690289Sobrien */ 68790289Sobrien ps->ptr = LIBUSB20_ADD_BYTES(buf, 0); 68890289Sobrien 68990289Sobrien /* 69018334Speter * Get the correct number of 69190289Sobrien * length bytes: 69218334Speter */ 69390289Sobrien if (len != 0) { 69450643Sobrien if (buf[0] == 0xFF) { 69590289Sobrien ps->len = 3; 69690289Sobrien } else { 69750643Sobrien ps->len = 1; 69850643Sobrien } 69990289Sobrien } else { 70090289Sobrien ps->len = 0; 70190289Sobrien } 70290289Sobrien } 70390289Sobrien /* get the structure length */ 70490289Sobrien 70590289Sobrien if (len != 0) { 70690289Sobrien if (buf[0] == 0xFF) { 70790289Sobrien if (len < 3) { 70890289Sobrien len = 0; 70990289Sobrien temp = 0; 71090289Sobrien } else { 71190289Sobrien len -= 3; 71250643Sobrien temp = buf[1] | 71350643Sobrien (buf[2] << 8); 71490289Sobrien buf += 3; 71590289Sobrien } 71650643Sobrien } else { 71750643Sobrien len -= 1; 71850643Sobrien temp = buf[0]; 71990289Sobrien buf += 1; 72090289Sobrien } 72190289Sobrien } else { 72290289Sobrien len = 0; 72318334Speter temp = 0; 72490289Sobrien } 72590289Sobrien /* check for invalid length */ 72690289Sobrien 72790289Sobrien if (temp > len) { 72890289Sobrien len = 0; 72990289Sobrien temp = 0; 73090289Sobrien } 73190289Sobrien /* check wanted structure type */ 73290289Sobrien 73390289Sobrien switch (ps->type) { 73490289Sobrien case LIBUSB20_ME_IS_ENCODED: 73590289Sobrien /* check for zero length */ 73690289Sobrien if (temp == 0) { 73790289Sobrien /* 73890289Sobrien * The pointer must 73990289Sobrien * be valid: 74090289Sobrien */ 74190289Sobrien ps->ptr = LIBUSB20_ADD_BYTES( 74290289Sobrien libusb20_me_encode_empty, 0); 74390289Sobrien ps->len = 1; 74490289Sobrien } else { 74590289Sobrien ps->len += temp; 74690289Sobrien } 74790289Sobrien break; 74890289Sobrien 74950643Sobrien case LIBUSB20_ME_IS_RAW: 75090289Sobrien /* update length and pointer */ 75190289Sobrien ps->len = temp; 75290289Sobrien ps->ptr = LIBUSB20_ADD_BYTES(buf, 0); 75390289Sobrien break; 75490289Sobrien 75590289Sobrien case LIBUSB20_ME_IS_EMPTY: 75690289Sobrien case LIBUSB20_ME_IS_DECODED: 75718334Speter /* check for non-zero length */ 75890289Sobrien if (temp != 0) { 75918334Speter /* update type */ 76090289Sobrien ps->type = LIBUSB20_ME_IS_DECODED; 76190289Sobrien ps->len = 0; 76290289Sobrien /* 76390289Sobrien * Recursivly decode 76490289Sobrien * the next structure 76590289Sobrien */ 76690289Sobrien dummy = libusb20_me_decode(buf, 76790289Sobrien temp, ps->ptr); 76890289Sobrien } else { 76918334Speter /* update type */ 77090289Sobrien ps->type = LIBUSB20_ME_IS_EMPTY; 77190289Sobrien ps->len = 0; 77290289Sobrien } 77390289Sobrien break; 77418334Speter 77590289Sobrien default: 77690289Sobrien /* 77790289Sobrien * nothing to do - should 77818334Speter * not happen 77990289Sobrien */ 78090289Sobrien ps->ptr = NULL; 78118334Speter ps->len = 0; 78290289Sobrien break; 78390289Sobrien } 78450643Sobrien buf += temp; 78590289Sobrien len -= temp; 78690289Sobrien pd_offset += sizeof(struct libusb20_me_struct); 78790289Sobrien } 78890289Sobrien break; 78918334Speter 79090289Sobrien default: 79118334Speter goto done; 79290289Sobrien } 79390289Sobrien } 79490289Sobriendone: 79590289Sobrien return (len_old - len); 79690289Sobrien} 79718334Speter