1 /* $FreeBSD: head/lib/libusb/libusb20_desc.c 248236 2013-03-13 12:23:14Z hselasky $ */
3 * Copyright (c) 2008 Hans Petter Selasky. All rights reserved.
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27 #ifdef LIBUSB_GLOBAL_INCLUDE_FILE
28 #include LIBUSB_GLOBAL_INCLUDE_FILE
34 #include <sys/param.h>
35 #include <sys/queue.h>
39 #include "libusb20_desc.h"
40 #include "libusb20_int.h"
42 static const uint32_t libusb20_me_encode_empty[2]; /* dummy */
44 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_DEVICE_DESC);
45 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_ENDPOINT_DESC);
46 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_INTERFACE_DESC);
47 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONFIG_DESC);
48 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONTROL_SETUP);
49 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_ENDPT_COMP_DESC);
50 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_USB_20_DEVCAP_DESC);
51 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_USB_DEVCAP_DESC);
52 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_BOS_DESCRIPTOR);
54 /*------------------------------------------------------------------------*
55 * libusb20_parse_config_desc
58 * NULL: Out of memory.
59 * Else: A valid config structure pointer which must be passed to "free()"
60 *------------------------------------------------------------------------*/
61 struct libusb20_config *
62 libusb20_parse_config_desc(const void *config_desc)
64 struct libusb20_config *lub_config;
65 struct libusb20_interface *lub_interface;
66 struct libusb20_interface *lub_alt_interface;
67 struct libusb20_interface *last_if;
68 struct libusb20_endpoint *lub_endpoint;
69 struct libusb20_endpoint *last_ep;
71 struct libusb20_me_struct pcdesc;
74 uint16_t niface_no_alt;
80 if (ptr[1] != LIBUSB20_DT_CONFIG) {
81 return (NULL); /* not config descriptor */
84 * The first "bInterfaceNumber" should never have the value 0xff.
93 /* get "wTotalLength" and setup "pcdesc" */
94 pcdesc.ptr = LIBUSB20_ADD_BYTES(config_desc, 0);
96 ((const uint8_t *)config_desc)[2] |
97 (((const uint8_t *)config_desc)[3] << 8);
98 pcdesc.type = LIBUSB20_ME_IS_RAW;
100 /* descriptor pre-scan */
101 while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
102 if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
104 } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
106 /* check "bInterfaceNumber" */
107 if (ptr[2] != iface_no) {
114 /* sanity checking */
116 return (NULL); /* corrupt */
118 if (nendpoint >= 256) {
119 return (NULL); /* corrupt */
121 size = sizeof(*lub_config) +
122 (niface * sizeof(*lub_interface)) +
123 (nendpoint * sizeof(*lub_endpoint)) +
126 lub_config = malloc(size);
127 if (lub_config == NULL) {
128 return (NULL); /* out of memory */
130 /* make sure memory is initialised */
131 memset(lub_config, 0, size);
133 lub_interface = (void *)(lub_config + 1);
134 lub_alt_interface = (void *)(lub_interface + niface_no_alt);
135 lub_endpoint = (void *)(lub_interface + niface);
138 * Make a copy of the config descriptor, so that the caller can free
139 * the inital config descriptor pointer!
141 ptr = (void *)(lub_endpoint + nendpoint);
142 memcpy(LIBUSB20_ADD_BYTES(ptr, 0), config_desc, pcdesc.len);
143 pcdesc.ptr = LIBUSB20_ADD_BYTES(ptr, 0);
144 config_desc = LIBUSB20_ADD_BYTES(ptr, 0);
146 /* init config structure */
150 LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc);
152 if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) {
155 lub_config->num_interface = 0;
156 lub_config->interface = lub_interface;
157 lub_config->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
158 lub_config->extra.len = -ptr[0];
159 lub_config->extra.type = LIBUSB20_ME_IS_RAW;
170 /* descriptor pre-scan */
171 while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
172 if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
175 last_ep = lub_endpoint;
176 last_if->num_endpoints++;
178 LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC, &last_ep->desc);
180 if (libusb20_me_decode(ptr, ptr[0], &last_ep->desc)) {
183 last_ep->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
184 last_ep->extra.len = 0;
185 last_ep->extra.type = LIBUSB20_ME_IS_RAW;
187 lub_config->extra.len += ptr[0];
190 } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
191 if (ptr[2] != iface_no) {
195 lub_config->num_interface++;
196 last_if = lub_interface;
199 /* one more alternate setting */
200 lub_interface->num_altsetting++;
201 last_if = lub_alt_interface;
205 LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC, &last_if->desc);
207 if (libusb20_me_decode(ptr, ptr[0], &last_if->desc)) {
211 * Sometimes USB devices have corrupt interface
212 * descriptors and we need to overwrite the provided
215 last_if->desc.bInterfaceNumber = niface - 1;
216 last_if->extra.ptr = LIBUSB20_ADD_BYTES(ptr, ptr[0]);
217 last_if->extra.len = 0;
218 last_if->extra.type = LIBUSB20_ME_IS_RAW;
219 last_if->endpoints = lub_endpoint + 1;
220 last_if->altsetting = lub_alt_interface;
221 last_if->num_altsetting = 0;
222 last_if->num_endpoints = 0;
225 /* unknown descriptor */
228 last_ep->extra.len += ptr[0];
230 last_if->extra.len += ptr[0];
233 lub_config->extra.len += ptr[0];
240 /*------------------------------------------------------------------------*
241 * libusb20_desc_foreach
243 * Safe traversal of USB descriptors.
246 * NULL: End of descriptors
247 * Else: Pointer to next descriptor
248 *------------------------------------------------------------------------*/
250 libusb20_desc_foreach(const struct libusb20_me_struct *pdesc,
251 const uint8_t *psubdesc)
253 const uint8_t *start;
255 const uint8_t *desc_next;
261 start = (const uint8_t *)pdesc->ptr;
262 end = LIBUSB20_ADD_BYTES(start, pdesc->len);
264 /* get start of next descriptor */
265 if (psubdesc == NULL)
268 psubdesc = psubdesc + psubdesc[0];
270 /* check that the next USB descriptor is within the range */
271 if ((psubdesc < start) || (psubdesc >= end))
272 return (NULL); /* out of range, or EOD */
274 /* check start of the second next USB descriptor, if any */
275 desc_next = psubdesc + psubdesc[0];
276 if ((desc_next < start) || (desc_next > end))
277 return (NULL); /* out of range */
279 /* check minimum descriptor length */
281 return (NULL); /* too short descriptor */
283 return (psubdesc); /* return start of next descriptor */
286 /*------------------------------------------------------------------------*
287 * libusb20_me_get_1 - safety wrapper to read out one byte
288 *------------------------------------------------------------------------*/
290 libusb20_me_get_1(const struct libusb20_me_struct *ie, uint16_t offset)
292 if (offset < ie->len) {
293 return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie->ptr, offset)));
298 /*------------------------------------------------------------------------*
299 * libusb20_me_get_2 - safety wrapper to read out one word
300 *------------------------------------------------------------------------*/
302 libusb20_me_get_2(const struct libusb20_me_struct *ie, uint16_t offset)
304 return (libusb20_me_get_1(ie, offset) |
305 (libusb20_me_get_1(ie, offset + 1) << 8));
308 /*------------------------------------------------------------------------*
309 * libusb20_me_encode - encode a message structure
311 * Description of parameters:
312 * "len" - maximum length of output buffer
313 * "ptr" - pointer to output buffer. If NULL, no data will be written
314 * "pd" - source structure
317 * 0..65535 - Number of bytes used, limited by the "len" input parameter.
318 *------------------------------------------------------------------------*/
320 libusb20_me_encode(void *ptr, uint16_t len, const void *pd)
322 const uint8_t *pf; /* pointer to format data */
323 uint8_t *buf; /* pointer to output buffer */
325 uint32_t pd_offset; /* decoded structure offset */
326 uint16_t len_old; /* old length */
327 uint16_t pd_count; /* decoded element count */
328 uint8_t me; /* message element */
334 pd_offset = sizeof(void *);
335 pf = (*((struct libusb20_me_format *const *)pd))->format;
341 /* get information element */
343 me = (pf[0]) & LIBUSB20_ME_MASK;
344 pd_count = pf[1] | (pf[2] << 8);
347 /* encode the message element */
350 case LIBUSB20_ME_INT8:
354 if (len < 1) /* overflow */
357 temp = *((const uint8_t *)
358 LIBUSB20_ADD_BYTES(pd, pd_offset));
367 case LIBUSB20_ME_INT16:
368 pd_offset = -((-pd_offset) & ~1); /* align */
372 if (len < 2) /* overflow */
376 temp = *((const uint16_t *)
377 LIBUSB20_ADD_BYTES(pd, pd_offset));
378 buf[1] = (temp >> 8) & 0xFF;
379 buf[0] = temp & 0xFF;
387 case LIBUSB20_ME_INT32:
388 pd_offset = -((-pd_offset) & ~3); /* align */
392 if (len < 4) /* overflow */
395 temp = *((const uint32_t *)
396 LIBUSB20_ADD_BYTES(pd, pd_offset));
397 buf[3] = (temp >> 24) & 0xFF;
398 buf[2] = (temp >> 16) & 0xFF;
399 buf[1] = (temp >> 8) & 0xFF;
400 buf[0] = temp & 0xFF;
408 case LIBUSB20_ME_INT64:
409 pd_offset = -((-pd_offset) & ~7); /* align */
413 if (len < 8) /* overflow */
417 temp = *((const uint64_t *)
418 LIBUSB20_ADD_BYTES(pd, pd_offset));
419 buf[7] = (temp >> 56) & 0xFF;
420 buf[6] = (temp >> 48) & 0xFF;
421 buf[5] = (temp >> 40) & 0xFF;
422 buf[4] = (temp >> 32) & 0xFF;
423 buf[3] = (temp >> 24) & 0xFF;
424 buf[2] = (temp >> 16) & 0xFF;
425 buf[1] = (temp >> 8) & 0xFF;
426 buf[0] = temp & 0xFF;
434 case LIBUSB20_ME_STRUCT:
435 pd_offset = -rounddown2(-pd_offset, LIBUSB20_ME_STRUCT_ALIGN); /* align */
439 struct libusb20_me_struct *ps;
441 ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
444 case LIBUSB20_ME_IS_RAW:
449 case LIBUSB20_ME_IS_ENCODED:
459 src_len = libusb20_me_get_1(pd, 0);
460 src_ptr = LIBUSB20_ADD_BYTES(ps->ptr, 1);
461 if (src_len == 0xFF) {
462 /* length is escaped */
463 src_len = libusb20_me_get_2(pd, 1);
465 LIBUSB20_ADD_BYTES(ps->ptr, 3);
469 case LIBUSB20_ME_IS_DECODED:
470 /* reserve 3 length bytes */
471 src_len = libusb20_me_encode(NULL,
472 0xFFFF - 3, ps->ptr);
476 default: /* empty structure */
482 if (src_len > 0xFE) {
483 if (src_len > (0xFFFF - 3))
487 if (len < (src_len + 3))
493 buf[1] = (src_len & 0xFF);
494 buf[2] = (src_len >> 8) & 0xFF;
497 len -= (src_len + 3);
499 if (len < (src_len + 1))
504 buf[0] = (src_len & 0xFF);
507 len -= (src_len + 1);
510 /* check for buffer and non-zero length */
512 if (buf && src_len) {
513 if (ps->type == LIBUSB20_ME_IS_DECODED) {
516 * procedure - we have
518 * complete structure:
522 dummy = libusb20_me_encode(buf,
523 0xFFFF - 3, ps->ptr);
525 bcopy(src_ptr, buf, src_len);
529 pd_offset += sizeof(struct libusb20_me_struct);
538 return (len_old - len);
541 /*------------------------------------------------------------------------*
542 * libusb20_me_decode - decode a message into a decoded structure
544 * Description of parameters:
545 * "ptr" - message pointer
546 * "len" - message length
547 * "pd" - pointer to decoded structure
550 * "0..65535" - number of bytes decoded, limited by "len"
551 *------------------------------------------------------------------------*/
553 libusb20_me_decode(const void *ptr, uint16_t len, void *pd)
555 const uint8_t *pf; /* pointer to format data */
556 const uint8_t *buf; /* pointer to input buffer */
558 uint32_t pd_offset; /* decoded structure offset */
559 uint16_t len_old; /* old length */
560 uint16_t pd_count; /* decoded element count */
561 uint8_t me; /* message element */
567 pd_offset = sizeof(void *);
568 pf = (*((struct libusb20_me_format **)pd))->format;
574 /* get information element */
576 me = (pf[0]) & LIBUSB20_ME_MASK;
577 pd_count = pf[1] | (pf[2] << 8);
580 /* decode the message element by type */
583 case LIBUSB20_ME_INT8:
595 *((uint8_t *)LIBUSB20_ADD_BYTES(pd,
601 case LIBUSB20_ME_INT16:
602 pd_offset = -((-pd_offset) & ~1); /* align */
615 *((uint16_t *)LIBUSB20_ADD_BYTES(pd,
621 case LIBUSB20_ME_INT32:
622 pd_offset = -((-pd_offset) & ~3); /* align */
632 temp |= buf[2] << 16;
638 *((uint32_t *)LIBUSB20_ADD_BYTES(pd,
644 case LIBUSB20_ME_INT64:
645 pd_offset = -((-pd_offset) & ~7); /* align */
654 temp = ((uint64_t)buf[7]) << 56;
655 temp |= ((uint64_t)buf[6]) << 48;
656 temp |= ((uint64_t)buf[5]) << 40;
657 temp |= ((uint64_t)buf[4]) << 32;
658 temp |= buf[3] << 24;
659 temp |= buf[2] << 16;
665 *((uint64_t *)LIBUSB20_ADD_BYTES(pd,
671 case LIBUSB20_ME_STRUCT:
672 pd_offset = -rounddown2(-pd_offset, LIBUSB20_ME_STRUCT_ALIGN); /* align */
676 struct libusb20_me_struct *ps;
678 ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
680 if (ps->type == LIBUSB20_ME_IS_ENCODED) {
682 * Pre-store a de-constified
686 ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
689 * Get the correct number of
693 if (buf[0] == 0xFF) {
702 /* get the structure length */
705 if (buf[0] == 0xFF) {
724 /* check for invalid length */
730 /* check wanted structure type */
733 case LIBUSB20_ME_IS_ENCODED:
734 /* check for zero length */
740 ps->ptr = LIBUSB20_ADD_BYTES(
741 libusb20_me_encode_empty, 0);
748 case LIBUSB20_ME_IS_RAW:
749 /* update length and pointer */
751 ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
754 case LIBUSB20_ME_IS_EMPTY:
755 case LIBUSB20_ME_IS_DECODED:
756 /* check for non-zero length */
759 ps->type = LIBUSB20_ME_IS_DECODED;
765 dummy = libusb20_me_decode(buf,
769 ps->type = LIBUSB20_ME_IS_EMPTY;
776 * nothing to do - should
785 pd_offset += sizeof(struct libusb20_me_struct);
794 return (len_old - len);