mirror of
https://github.com/freebsd/freebsd-src
synced 2024-09-06 17:18:32 +00:00
797 lines
19 KiB
C
797 lines
19 KiB
C
/*-
|
|
* SPDX-License-Identifier: BSD-2-Clause
|
|
*
|
|
* Copyright (c) 2008 Hans Petter Selasky. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
|
|
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
|
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
* SUCH DAMAGE.
|
|
*/
|
|
|
|
#ifdef LIBUSB_GLOBAL_INCLUDE_FILE
|
|
#include LIBUSB_GLOBAL_INCLUDE_FILE
|
|
#else
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <sys/queue.h>
|
|
#endif
|
|
|
|
#include "libusb20.h"
|
|
#include "libusb20_desc.h"
|
|
#include "libusb20_int.h"
|
|
|
|
static const uint32_t libusb20_me_encode_empty[2]; /* dummy */
|
|
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_DEVICE_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_ENDPOINT_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_INTERFACE_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONFIG_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONTROL_SETUP);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_ENDPT_COMP_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_USB_20_DEVCAP_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_USB_DEVCAP_DESC);
|
|
LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_BOS_DESCRIPTOR);
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_parse_config_desc
|
|
*
|
|
* Return values:
|
|
* NULL: Out of memory.
|
|
* Else: A valid config structure pointer which must be passed to "free()"
|
|
*------------------------------------------------------------------------*/
|
|
struct libusb20_config *
|
|
libusb20_parse_config_desc(const void *config_desc)
|
|
{
|
|
struct libusb20_config *lub_config;
|
|
struct libusb20_interface *lub_interface;
|
|
struct libusb20_interface *lub_alt_interface;
|
|
struct libusb20_interface *last_if;
|
|
struct libusb20_endpoint *lub_endpoint;
|
|
struct libusb20_endpoint *last_ep;
|
|
|
|
struct libusb20_me_struct pcdesc;
|
|
const uint8_t *ptr;
|
|
uint32_t size;
|
|
uint16_t niface_no_alt;
|
|
uint16_t niface;
|
|
uint16_t nendpoint;
|
|
uint16_t iface_no;
|
|
|
|
ptr = config_desc;
|
|
if (ptr[1] != LIBUSB20_DT_CONFIG) {
|
|
return (NULL); /* not config descriptor */
|
|
}
|
|
|
|
/*
|
|
* The first "bInterfaceNumber" cannot start at 0xFFFF
|
|
* because the field is 8-bit.
|
|
*/
|
|
niface_no_alt = 0;
|
|
nendpoint = 0;
|
|
niface = 0;
|
|
iface_no = 0xFFFF;
|
|
ptr = NULL;
|
|
|
|
/* get "wTotalLength" and setup "pcdesc" */
|
|
pcdesc.ptr = LIBUSB20_ADD_BYTES(config_desc, 0);
|
|
pcdesc.len =
|
|
((const uint8_t *)config_desc)[2] |
|
|
(((const uint8_t *)config_desc)[3] << 8);
|
|
pcdesc.type = LIBUSB20_ME_IS_RAW;
|
|
|
|
/* descriptor pre-scan */
|
|
while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
|
|
if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
|
|
nendpoint++;
|
|
} else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
|
|
niface++;
|
|
/* check "bInterfaceNumber" */
|
|
if (ptr[2] != iface_no) {
|
|
iface_no = ptr[2];
|
|
niface_no_alt++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* sanity checking */
|
|
if (niface >= 256) {
|
|
return (NULL); /* corrupt */
|
|
}
|
|
if (nendpoint >= 256) {
|
|
return (NULL); /* corrupt */
|
|
}
|
|
size = sizeof(*lub_config) +
|
|
(niface * sizeof(*lub_interface)) +
|
|
(nendpoint * sizeof(*lub_endpoint)) +
|
|
pcdesc.len;
|
|
|
|
lub_config = malloc(size);
|
|
if (lub_config == NULL) {
|
|
return (NULL); /* out of memory */
|
|
}
|
|
/* make sure memory is initialised */
|
|
memset(lub_config, 0, size);
|
|
|
|
lub_interface = (void *)(lub_config + 1);
|
|
lub_alt_interface = (void *)(lub_interface + niface_no_alt);
|
|
lub_endpoint = (void *)(lub_interface + niface);
|
|
|
|
/*
|
|
* Make a copy of the config descriptor, so that the caller can free
|
|
* the initial config descriptor pointer!
|
|
*/
|
|
memcpy((void *)(lub_endpoint + nendpoint), config_desc, pcdesc.len);
|
|
|
|
ptr = (const void *)(lub_endpoint + nendpoint);
|
|
pcdesc.ptr = LIBUSB20_ADD_BYTES(ptr, 0);
|
|
|
|
/* init config structure */
|
|
|
|
LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc);
|
|
|
|
if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) {
|
|
/* ignore */
|
|
}
|
|
lub_config->num_interface = 0;
|
|
lub_config->interface = lub_interface;
|
|
lub_config->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
|
|
lub_config->extra.len = -ptr[0];
|
|
lub_config->extra.type = LIBUSB20_ME_IS_RAW;
|
|
|
|
/* reset states */
|
|
niface = 0;
|
|
iface_no = 0xFFFF;
|
|
ptr = NULL;
|
|
lub_interface--;
|
|
lub_endpoint--;
|
|
last_if = NULL;
|
|
last_ep = NULL;
|
|
|
|
/* descriptor pre-scan */
|
|
while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
|
|
if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
|
|
if (last_if) {
|
|
lub_endpoint++;
|
|
last_ep = lub_endpoint;
|
|
last_if->num_endpoints++;
|
|
|
|
LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC, &last_ep->desc);
|
|
|
|
if (libusb20_me_decode(ptr, ptr[0], &last_ep->desc)) {
|
|
/* ignore */
|
|
}
|
|
last_ep->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
|
|
last_ep->extra.len = 0;
|
|
last_ep->extra.type = LIBUSB20_ME_IS_RAW;
|
|
} else {
|
|
lub_config->extra.len += ptr[0];
|
|
}
|
|
|
|
} else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
|
|
if (ptr[2] != iface_no) {
|
|
/* new interface */
|
|
iface_no = ptr[2];
|
|
lub_interface++;
|
|
lub_config->num_interface++;
|
|
last_if = lub_interface;
|
|
niface++;
|
|
} else {
|
|
/* one more alternate setting */
|
|
lub_interface->num_altsetting++;
|
|
last_if = lub_alt_interface;
|
|
lub_alt_interface++;
|
|
}
|
|
|
|
LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC, &last_if->desc);
|
|
|
|
if (libusb20_me_decode(ptr, ptr[0], &last_if->desc)) {
|
|
/* ignore */
|
|
}
|
|
|
|
/* detect broken USB descriptors when USB debugging is enabled */
|
|
if (last_if->desc.bInterfaceNumber != (uint8_t)(niface - 1)) {
|
|
const char *str = getenv("LIBUSB_DEBUG");
|
|
if (str != NULL && str[0] != '\0' && str[0] != '0') {
|
|
printf("LIBUSB_DEBUG: bInterfaceNumber(%u) is not sequential(%u)\n",
|
|
last_if->desc.bInterfaceNumber, niface - 1);
|
|
}
|
|
}
|
|
last_if->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
|
|
last_if->extra.len = 0;
|
|
last_if->extra.type = LIBUSB20_ME_IS_RAW;
|
|
last_if->endpoints = lub_endpoint + 1;
|
|
last_if->altsetting = lub_alt_interface;
|
|
last_if->num_altsetting = 0;
|
|
last_if->num_endpoints = 0;
|
|
last_ep = NULL;
|
|
} else {
|
|
/* unknown descriptor */
|
|
if (last_if) {
|
|
if (last_ep) {
|
|
last_ep->extra.len += ptr[0];
|
|
} else {
|
|
last_if->extra.len += ptr[0];
|
|
}
|
|
} else {
|
|
lub_config->extra.len += ptr[0];
|
|
}
|
|
}
|
|
}
|
|
return (lub_config);
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_desc_foreach
|
|
*
|
|
* Safe traversal of USB descriptors.
|
|
*
|
|
* Return values:
|
|
* NULL: End of descriptors
|
|
* Else: Pointer to next descriptor
|
|
*------------------------------------------------------------------------*/
|
|
const uint8_t *
|
|
libusb20_desc_foreach(const struct libusb20_me_struct *pdesc,
|
|
const uint8_t *psubdesc)
|
|
{
|
|
const uint8_t *start;
|
|
const uint8_t *end;
|
|
const uint8_t *desc_next;
|
|
|
|
/* be NULL safe */
|
|
if (pdesc == NULL)
|
|
return (NULL);
|
|
|
|
start = (const uint8_t *)pdesc->ptr;
|
|
end = LIBUSB20_ADD_BYTES(start, pdesc->len);
|
|
|
|
/* get start of next descriptor */
|
|
if (psubdesc == NULL)
|
|
psubdesc = start;
|
|
else
|
|
psubdesc = psubdesc + psubdesc[0];
|
|
|
|
/* check that the next USB descriptor is within the range */
|
|
if ((psubdesc < start) || (psubdesc >= end))
|
|
return (NULL); /* out of range, or EOD */
|
|
|
|
/* check start of the second next USB descriptor, if any */
|
|
desc_next = psubdesc + psubdesc[0];
|
|
if ((desc_next < start) || (desc_next > end))
|
|
return (NULL); /* out of range */
|
|
|
|
/* check minimum descriptor length */
|
|
if (psubdesc[0] < 3)
|
|
return (NULL); /* too short descriptor */
|
|
|
|
return (psubdesc); /* return start of next descriptor */
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_me_get_1 - safety wrapper to read out one byte
|
|
*------------------------------------------------------------------------*/
|
|
uint8_t
|
|
libusb20_me_get_1(const struct libusb20_me_struct *ie, uint16_t offset)
|
|
{
|
|
if (offset < ie->len) {
|
|
return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie->ptr, offset)));
|
|
}
|
|
return (0);
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_me_get_2 - safety wrapper to read out one word
|
|
*------------------------------------------------------------------------*/
|
|
uint16_t
|
|
libusb20_me_get_2(const struct libusb20_me_struct *ie, uint16_t offset)
|
|
{
|
|
return (libusb20_me_get_1(ie, offset) |
|
|
(libusb20_me_get_1(ie, offset + 1) << 8));
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_me_encode - encode a message structure
|
|
*
|
|
* Description of parameters:
|
|
* "len" - maximum length of output buffer
|
|
* "ptr" - pointer to output buffer. If NULL, no data will be written
|
|
* "pd" - source structure
|
|
*
|
|
* Return values:
|
|
* 0..65535 - Number of bytes used, limited by the "len" input parameter.
|
|
*------------------------------------------------------------------------*/
|
|
uint16_t
|
|
libusb20_me_encode(void *ptr, uint16_t len, const void *pd)
|
|
{
|
|
const uint8_t *pf; /* pointer to format data */
|
|
uint8_t *buf; /* pointer to output buffer */
|
|
|
|
uint32_t pd_offset; /* decoded structure offset */
|
|
uint16_t len_old; /* old length */
|
|
uint16_t pd_count; /* decoded element count */
|
|
uint8_t me; /* message element */
|
|
|
|
/* initialise */
|
|
|
|
len_old = len;
|
|
buf = ptr;
|
|
pd_offset = sizeof(void *);
|
|
pf = (*((struct libusb20_me_format *const *)pd))->format;
|
|
|
|
/* scan */
|
|
|
|
while (1) {
|
|
|
|
/* get information element */
|
|
|
|
me = (pf[0]) & LIBUSB20_ME_MASK;
|
|
pd_count = pf[1] | (pf[2] << 8);
|
|
pf += 3;
|
|
|
|
/* encode the message element */
|
|
|
|
switch (me) {
|
|
case LIBUSB20_ME_INT8:
|
|
while (pd_count--) {
|
|
uint8_t temp;
|
|
|
|
if (len < 1) /* overflow */
|
|
goto done;
|
|
if (buf) {
|
|
temp = *((const uint8_t *)
|
|
LIBUSB20_ADD_BYTES(pd, pd_offset));
|
|
buf[0] = temp;
|
|
buf += 1;
|
|
}
|
|
pd_offset += 1;
|
|
len -= 1;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT16:
|
|
pd_offset = -((-pd_offset) & ~1); /* align */
|
|
while (pd_count--) {
|
|
uint16_t temp;
|
|
|
|
if (len < 2) /* overflow */
|
|
goto done;
|
|
|
|
if (buf) {
|
|
temp = *((const uint16_t *)
|
|
LIBUSB20_ADD_BYTES(pd, pd_offset));
|
|
buf[1] = (temp >> 8) & 0xFF;
|
|
buf[0] = temp & 0xFF;
|
|
buf += 2;
|
|
}
|
|
pd_offset += 2;
|
|
len -= 2;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT32:
|
|
pd_offset = -((-pd_offset) & ~3); /* align */
|
|
while (pd_count--) {
|
|
uint32_t temp;
|
|
|
|
if (len < 4) /* overflow */
|
|
goto done;
|
|
if (buf) {
|
|
temp = *((const uint32_t *)
|
|
LIBUSB20_ADD_BYTES(pd, pd_offset));
|
|
buf[3] = (temp >> 24) & 0xFF;
|
|
buf[2] = (temp >> 16) & 0xFF;
|
|
buf[1] = (temp >> 8) & 0xFF;
|
|
buf[0] = temp & 0xFF;
|
|
buf += 4;
|
|
}
|
|
pd_offset += 4;
|
|
len -= 4;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT64:
|
|
pd_offset = -((-pd_offset) & ~7); /* align */
|
|
while (pd_count--) {
|
|
uint64_t temp;
|
|
|
|
if (len < 8) /* overflow */
|
|
goto done;
|
|
if (buf) {
|
|
|
|
temp = *((const uint64_t *)
|
|
LIBUSB20_ADD_BYTES(pd, pd_offset));
|
|
buf[7] = (temp >> 56) & 0xFF;
|
|
buf[6] = (temp >> 48) & 0xFF;
|
|
buf[5] = (temp >> 40) & 0xFF;
|
|
buf[4] = (temp >> 32) & 0xFF;
|
|
buf[3] = (temp >> 24) & 0xFF;
|
|
buf[2] = (temp >> 16) & 0xFF;
|
|
buf[1] = (temp >> 8) & 0xFF;
|
|
buf[0] = temp & 0xFF;
|
|
buf += 8;
|
|
}
|
|
pd_offset += 8;
|
|
len -= 8;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_STRUCT:
|
|
pd_offset = -((-pd_offset) &
|
|
~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */
|
|
while (pd_count--) {
|
|
void *src_ptr;
|
|
uint16_t src_len;
|
|
struct libusb20_me_struct *ps;
|
|
|
|
ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
|
|
|
|
switch (ps->type) {
|
|
case LIBUSB20_ME_IS_RAW:
|
|
src_len = ps->len;
|
|
src_ptr = ps->ptr;
|
|
break;
|
|
|
|
case LIBUSB20_ME_IS_ENCODED:
|
|
if (ps->len == 0) {
|
|
/*
|
|
* Length is encoded
|
|
* in the data itself
|
|
* and should be
|
|
* correct:
|
|
*/
|
|
ps->len = 0xFFFF;
|
|
}
|
|
src_len = libusb20_me_get_1(pd, 0);
|
|
src_ptr = LIBUSB20_ADD_BYTES(ps->ptr, 1);
|
|
if (src_len == 0xFF) {
|
|
/* length is escaped */
|
|
src_len = libusb20_me_get_2(pd, 1);
|
|
src_ptr =
|
|
LIBUSB20_ADD_BYTES(ps->ptr, 3);
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_IS_DECODED:
|
|
/* reserve 3 length bytes */
|
|
src_len = libusb20_me_encode(NULL,
|
|
0xFFFF - 3, ps->ptr);
|
|
src_ptr = NULL;
|
|
break;
|
|
|
|
default: /* empty structure */
|
|
src_len = 0;
|
|
src_ptr = NULL;
|
|
break;
|
|
}
|
|
|
|
if (src_len > 0xFE) {
|
|
if (src_len > (0xFFFF - 3))
|
|
/* overflow */
|
|
goto done;
|
|
|
|
if (len < (src_len + 3))
|
|
/* overflow */
|
|
goto done;
|
|
|
|
if (buf) {
|
|
buf[0] = 0xFF;
|
|
buf[1] = (src_len & 0xFF);
|
|
buf[2] = (src_len >> 8) & 0xFF;
|
|
buf += 3;
|
|
}
|
|
len -= (src_len + 3);
|
|
} else {
|
|
if (len < (src_len + 1))
|
|
/* overflow */
|
|
goto done;
|
|
|
|
if (buf) {
|
|
buf[0] = (src_len & 0xFF);
|
|
buf += 1;
|
|
}
|
|
len -= (src_len + 1);
|
|
}
|
|
|
|
/* check for buffer and non-zero length */
|
|
|
|
if (buf && src_len) {
|
|
if (ps->type == LIBUSB20_ME_IS_DECODED) {
|
|
/*
|
|
* Repeat encode
|
|
* procedure - we have
|
|
* room for the
|
|
* complete structure:
|
|
*/
|
|
(void) libusb20_me_encode(buf,
|
|
0xFFFF - 3, ps->ptr);
|
|
} else {
|
|
bcopy(src_ptr, buf, src_len);
|
|
}
|
|
buf += src_len;
|
|
}
|
|
pd_offset += sizeof(struct libusb20_me_struct);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
goto done;
|
|
}
|
|
}
|
|
done:
|
|
return (len_old - len);
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*
|
|
* libusb20_me_decode - decode a message into a decoded structure
|
|
*
|
|
* Description of parameters:
|
|
* "ptr" - message pointer
|
|
* "len" - message length
|
|
* "pd" - pointer to decoded structure
|
|
*
|
|
* Returns:
|
|
* "0..65535" - number of bytes decoded, limited by "len"
|
|
*------------------------------------------------------------------------*/
|
|
uint16_t
|
|
libusb20_me_decode(const void *ptr, uint16_t len, void *pd)
|
|
{
|
|
const uint8_t *pf; /* pointer to format data */
|
|
const uint8_t *buf; /* pointer to input buffer */
|
|
|
|
uint32_t pd_offset; /* decoded structure offset */
|
|
uint16_t len_old; /* old length */
|
|
uint16_t pd_count; /* decoded element count */
|
|
uint8_t me; /* message element */
|
|
|
|
/* initialise */
|
|
|
|
len_old = len;
|
|
buf = ptr;
|
|
pd_offset = sizeof(void *);
|
|
pf = (*((struct libusb20_me_format **)pd))->format;
|
|
|
|
/* scan */
|
|
|
|
while (1) {
|
|
|
|
/* get information element */
|
|
|
|
me = (pf[0]) & LIBUSB20_ME_MASK;
|
|
pd_count = pf[1] | (pf[2] << 8);
|
|
pf += 3;
|
|
|
|
/* decode the message element by type */
|
|
|
|
switch (me) {
|
|
case LIBUSB20_ME_INT8:
|
|
while (pd_count--) {
|
|
uint8_t temp;
|
|
|
|
if (len < 1) {
|
|
len = 0;
|
|
temp = 0;
|
|
} else {
|
|
len -= 1;
|
|
temp = buf[0];
|
|
buf++;
|
|
}
|
|
*((uint8_t *)LIBUSB20_ADD_BYTES(pd,
|
|
pd_offset)) = temp;
|
|
pd_offset += 1;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT16:
|
|
pd_offset = -((-pd_offset) & ~1); /* align */
|
|
while (pd_count--) {
|
|
uint16_t temp;
|
|
|
|
if (len < 2) {
|
|
len = 0;
|
|
temp = 0;
|
|
} else {
|
|
len -= 2;
|
|
temp = buf[1] << 8;
|
|
temp |= buf[0];
|
|
buf += 2;
|
|
}
|
|
*((uint16_t *)LIBUSB20_ADD_BYTES(pd,
|
|
pd_offset)) = temp;
|
|
pd_offset += 2;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT32:
|
|
pd_offset = -((-pd_offset) & ~3); /* align */
|
|
while (pd_count--) {
|
|
uint32_t temp;
|
|
|
|
if (len < 4) {
|
|
len = 0;
|
|
temp = 0;
|
|
} else {
|
|
len -= 4;
|
|
temp = buf[3] << 24;
|
|
temp |= buf[2] << 16;
|
|
temp |= buf[1] << 8;
|
|
temp |= buf[0];
|
|
buf += 4;
|
|
}
|
|
|
|
*((uint32_t *)LIBUSB20_ADD_BYTES(pd,
|
|
pd_offset)) = temp;
|
|
pd_offset += 4;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_INT64:
|
|
pd_offset = -((-pd_offset) & ~7); /* align */
|
|
while (pd_count--) {
|
|
uint64_t temp;
|
|
|
|
if (len < 8) {
|
|
len = 0;
|
|
temp = 0;
|
|
} else {
|
|
len -= 8;
|
|
temp = ((uint64_t)buf[7]) << 56;
|
|
temp |= ((uint64_t)buf[6]) << 48;
|
|
temp |= ((uint64_t)buf[5]) << 40;
|
|
temp |= ((uint64_t)buf[4]) << 32;
|
|
temp |= buf[3] << 24;
|
|
temp |= buf[2] << 16;
|
|
temp |= buf[1] << 8;
|
|
temp |= buf[0];
|
|
buf += 8;
|
|
}
|
|
|
|
*((uint64_t *)LIBUSB20_ADD_BYTES(pd,
|
|
pd_offset)) = temp;
|
|
pd_offset += 8;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_STRUCT:
|
|
pd_offset = -((-pd_offset) &
|
|
~(LIBUSB20_ME_STRUCT_ALIGN - 1)); /* align */
|
|
while (pd_count--) {
|
|
uint16_t temp;
|
|
struct libusb20_me_struct *ps;
|
|
|
|
ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
|
|
|
|
if (ps->type == LIBUSB20_ME_IS_ENCODED) {
|
|
/*
|
|
* Pre-store a de-constified
|
|
* pointer to the raw
|
|
* structure:
|
|
*/
|
|
ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
|
|
|
|
/*
|
|
* Get the correct number of
|
|
* length bytes:
|
|
*/
|
|
if (len != 0) {
|
|
if (buf[0] == 0xFF) {
|
|
ps->len = 3;
|
|
} else {
|
|
ps->len = 1;
|
|
}
|
|
} else {
|
|
ps->len = 0;
|
|
}
|
|
}
|
|
/* get the structure length */
|
|
|
|
if (len != 0) {
|
|
if (buf[0] == 0xFF) {
|
|
if (len < 3) {
|
|
len = 0;
|
|
temp = 0;
|
|
} else {
|
|
len -= 3;
|
|
temp = buf[1] |
|
|
(buf[2] << 8);
|
|
buf += 3;
|
|
}
|
|
} else {
|
|
len -= 1;
|
|
temp = buf[0];
|
|
buf += 1;
|
|
}
|
|
} else {
|
|
len = 0;
|
|
temp = 0;
|
|
}
|
|
/* check for invalid length */
|
|
|
|
if (temp > len) {
|
|
len = 0;
|
|
temp = 0;
|
|
}
|
|
/* check wanted structure type */
|
|
|
|
switch (ps->type) {
|
|
case LIBUSB20_ME_IS_ENCODED:
|
|
/* check for zero length */
|
|
if (temp == 0) {
|
|
/*
|
|
* The pointer must
|
|
* be valid:
|
|
*/
|
|
ps->ptr = LIBUSB20_ADD_BYTES(
|
|
libusb20_me_encode_empty, 0);
|
|
ps->len = 1;
|
|
} else {
|
|
ps->len += temp;
|
|
}
|
|
break;
|
|
|
|
case LIBUSB20_ME_IS_RAW:
|
|
/* update length and pointer */
|
|
ps->len = temp;
|
|
ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
|
|
break;
|
|
|
|
case LIBUSB20_ME_IS_EMPTY:
|
|
case LIBUSB20_ME_IS_DECODED:
|
|
/* check for non-zero length */
|
|
if (temp != 0) {
|
|
/* update type */
|
|
ps->type = LIBUSB20_ME_IS_DECODED;
|
|
ps->len = 0;
|
|
/*
|
|
* Recursivly decode
|
|
* the next structure
|
|
*/
|
|
(void) libusb20_me_decode(buf,
|
|
temp, ps->ptr);
|
|
} else {
|
|
/* update type */
|
|
ps->type = LIBUSB20_ME_IS_EMPTY;
|
|
ps->len = 0;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
/*
|
|
* nothing to do - should
|
|
* not happen
|
|
*/
|
|
ps->ptr = NULL;
|
|
ps->len = 0;
|
|
break;
|
|
}
|
|
buf += temp;
|
|
len -= temp;
|
|
pd_offset += sizeof(struct libusb20_me_struct);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
goto done;
|
|
}
|
|
}
|
|
done:
|
|
return (len_old - len);
|
|
}
|