#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];
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);
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);
}
niface_no_alt = 0;
nendpoint = 0;
niface = 0;
iface_no = 0xFFFF;
ptr = NULL;
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;
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++;
if (ptr[2] != iface_no) {
iface_no = ptr[2];
niface_no_alt++;
}
}
}
if (niface >= 256) {
return (NULL);
}
if (nendpoint >= 256) {
return (NULL);
}
size = sizeof(*lub_config) +
(niface * sizeof(*lub_interface)) +
(nendpoint * sizeof(*lub_endpoint)) +
pcdesc.len;
lub_config = malloc(size);
if (lub_config == NULL) {
return (NULL);
}
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);
memcpy((void *)(lub_endpoint + nendpoint), config_desc, pcdesc.len);
ptr = (const void *)(lub_endpoint + nendpoint);
pcdesc.ptr = LIBUSB20_ADD_BYTES(ptr, 0);
LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc);
if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) {
}
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;
niface = 0;
iface_no = 0xFFFF;
ptr = NULL;
lub_interface--;
lub_endpoint--;
last_if = NULL;
last_ep = NULL;
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)) {
}
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) {
iface_no = ptr[2];
lub_interface++;
lub_config->num_interface++;
last_if = lub_interface;
niface++;
} else {
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)) {
}
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 {
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);
}
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;
if (pdesc == NULL)
return (NULL);
start = (const uint8_t *)pdesc->ptr;
end = LIBUSB20_ADD_BYTES(start, pdesc->len);
if (psubdesc == NULL)
psubdesc = start;
else
psubdesc = psubdesc + psubdesc[0];
if ((psubdesc < start) || (psubdesc >= end))
return (NULL);
desc_next = psubdesc + psubdesc[0];
if ((desc_next < start) || (desc_next > end))
return (NULL);
if (psubdesc[0] < 3)
return (NULL);
return (psubdesc);
}
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);
}
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));
}
uint16_t
libusb20_me_encode(void *ptr, uint16_t len, const void *pd)
{
const uint8_t *pf;
uint8_t *buf;
uint32_t pd_offset;
uint16_t len_old;
uint16_t pd_count;
uint8_t me;
len_old = len;
buf = ptr;
pd_offset = sizeof(void *);
pf = (*((struct libusb20_me_format *const *)pd))->format;
while (1) {
me = (pf[0]) & LIBUSB20_ME_MASK;
pd_count = pf[1] | (pf[2] << 8);
pf += 3;
switch (me) {
case LIBUSB20_ME_INT8:
while (pd_count--) {
uint8_t temp;
if (len < 1)
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);
while (pd_count--) {
uint16_t temp;
if (len < 2)
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);
while (pd_count--) {
uint32_t temp;
if (len < 4)
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);
while (pd_count--) {
uint64_t temp;
if (len < 8)
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));
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) {
ps->len = 0xFFFF;
}
src_len = libusb20_me_get_1(pd, 0);
src_ptr = LIBUSB20_ADD_BYTES(ps->ptr, 1);
if (src_len == 0xFF) {
src_len = libusb20_me_get_2(pd, 1);
src_ptr =
LIBUSB20_ADD_BYTES(ps->ptr, 3);
}
break;
case LIBUSB20_ME_IS_DECODED:
src_len = libusb20_me_encode(NULL,
0xFFFF - 3, ps->ptr);
src_ptr = NULL;
break;
default:
src_len = 0;
src_ptr = NULL;
break;
}
if (src_len > 0xFE) {
if (src_len > (0xFFFF - 3))
goto done;
if (len < (src_len + 3))
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))
goto done;
if (buf) {
buf[0] = (src_len & 0xFF);
buf += 1;
}
len -= (src_len + 1);
}
if (buf && src_len) {
if (ps->type == LIBUSB20_ME_IS_DECODED) {
(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);
}
uint16_t
libusb20_me_decode(const void *ptr, uint16_t len, void *pd)
{
const uint8_t *pf;
const uint8_t *buf;
uint32_t pd_offset;
uint16_t len_old;
uint16_t pd_count;
uint8_t me;
len_old = len;
buf = ptr;
pd_offset = sizeof(void *);
pf = (*((struct libusb20_me_format **)pd))->format;
while (1) {
me = (pf[0]) & LIBUSB20_ME_MASK;
pd_count = pf[1] | (pf[2] << 8);
pf += 3;
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);
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);
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);
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));
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) {
ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
if (len != 0) {
if (buf[0] == 0xFF) {
ps->len = 3;
} else {
ps->len = 1;
}
} else {
ps->len = 0;
}
}
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;
}
if (temp > len) {
len = 0;
temp = 0;
}
switch (ps->type) {
case LIBUSB20_ME_IS_ENCODED:
if (temp == 0) {
ps->ptr = LIBUSB20_ADD_BYTES(
libusb20_me_encode_empty, 0);
ps->len = 1;
} else {
ps->len += temp;
}
break;
case LIBUSB20_ME_IS_RAW:
ps->len = temp;
ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
break;
case LIBUSB20_ME_IS_EMPTY:
case LIBUSB20_ME_IS_DECODED:
if (temp != 0) {
ps->type = LIBUSB20_ME_IS_DECODED;
ps->len = 0;
(void) libusb20_me_decode(buf,
temp, ps->ptr);
} else {
ps->type = LIBUSB20_ME_IS_EMPTY;
ps->len = 0;
}
break;
default:
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);
}