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/queue.h>
38 #include "libusb20_desc.h"
39 #include "libusb20_int.h"
41 static const uint32_t libusb20_me_encode_empty
[2]; /* dummy */
43 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_DEVICE_DESC
);
44 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_ENDPOINT_DESC
);
45 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_INTERFACE_DESC
);
46 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONFIG_DESC
);
47 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_CONTROL_SETUP
);
48 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_ENDPT_COMP_DESC
);
49 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_USB_20_DEVCAP_DESC
);
50 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_SS_USB_DEVCAP_DESC
);
51 LIBUSB20_MAKE_STRUCT_FORMAT(LIBUSB20_BOS_DESCRIPTOR
);
53 /*------------------------------------------------------------------------*
54 * libusb20_parse_config_desc
57 * NULL: Out of memory.
58 * Else: A valid config structure pointer which must be passed to "free()"
59 *------------------------------------------------------------------------*/
60 struct libusb20_config
*
61 libusb20_parse_config_desc(const void *config_desc
)
63 struct libusb20_config
*lub_config
;
64 struct libusb20_interface
*lub_interface
;
65 struct libusb20_interface
*lub_alt_interface
;
66 struct libusb20_interface
*last_if
;
67 struct libusb20_endpoint
*lub_endpoint
;
68 struct libusb20_endpoint
*last_ep
;
70 struct libusb20_me_struct pcdesc
;
73 uint16_t niface_no_alt
;
79 if (ptr
[1] != LIBUSB20_DT_CONFIG
) {
80 return (NULL
); /* not config descriptor */
83 * The first "bInterfaceNumber" should never have the value 0xff.
92 /* get "wTotalLength" and setup "pcdesc" */
93 pcdesc
.ptr
= LIBUSB20_ADD_BYTES(config_desc
, 0);
95 ((const uint8_t *)config_desc
)[2] |
96 (((const uint8_t *)config_desc
)[3] << 8);
97 pcdesc
.type
= LIBUSB20_ME_IS_RAW
;
99 /* descriptor pre-scan */
100 while ((ptr
= libusb20_desc_foreach(&pcdesc
, ptr
))) {
101 if (ptr
[1] == LIBUSB20_DT_ENDPOINT
) {
103 } else if ((ptr
[1] == LIBUSB20_DT_INTERFACE
) && (ptr
[0] >= 4)) {
105 /* check "bInterfaceNumber" */
106 if (ptr
[2] != iface_no
) {
113 /* sanity checking */
115 return (NULL
); /* corrupt */
117 if (nendpoint
>= 256) {
118 return (NULL
); /* corrupt */
120 size
= sizeof(*lub_config
) +
121 (niface
* sizeof(*lub_interface
)) +
122 (nendpoint
* sizeof(*lub_endpoint
)) +
125 lub_config
= malloc(size
);
126 if (lub_config
== NULL
) {
127 return (NULL
); /* out of memory */
129 /* make sure memory is initialised */
130 memset(lub_config
, 0, size
);
132 lub_interface
= (void *)(lub_config
+ 1);
133 lub_alt_interface
= (void *)(lub_interface
+ niface_no_alt
);
134 lub_endpoint
= (void *)(lub_interface
+ niface
);
137 * Make a copy of the config descriptor, so that the caller can free
138 * the inital config descriptor pointer!
140 ptr
= (void *)(lub_endpoint
+ nendpoint
);
141 memcpy(LIBUSB20_ADD_BYTES(ptr
, 0), config_desc
, pcdesc
.len
);
142 pcdesc
.ptr
= LIBUSB20_ADD_BYTES(ptr
, 0);
143 config_desc
= LIBUSB20_ADD_BYTES(ptr
, 0);
145 /* init config structure */
149 LIBUSB20_INIT(LIBUSB20_CONFIG_DESC
, &lub_config
->desc
);
151 if (libusb20_me_decode(ptr
, ptr
[0], &lub_config
->desc
)) {
154 lub_config
->num_interface
= 0;
155 lub_config
->interface
= lub_interface
;
156 lub_config
->extra
.ptr
= LIBUSB20_ADD_BYTES(ptr
, ptr
[0]);
157 lub_config
->extra
.len
= -ptr
[0];
158 lub_config
->extra
.type
= LIBUSB20_ME_IS_RAW
;
169 /* descriptor pre-scan */
170 while ((ptr
= libusb20_desc_foreach(&pcdesc
, ptr
))) {
171 if (ptr
[1] == LIBUSB20_DT_ENDPOINT
) {
174 last_ep
= lub_endpoint
;
175 last_if
->num_endpoints
++;
177 LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC
, &last_ep
->desc
);
179 if (libusb20_me_decode(ptr
, ptr
[0], &last_ep
->desc
)) {
182 last_ep
->extra
.ptr
= LIBUSB20_ADD_BYTES(ptr
, ptr
[0]);
183 last_ep
->extra
.len
= 0;
184 last_ep
->extra
.type
= LIBUSB20_ME_IS_RAW
;
186 lub_config
->extra
.len
+= ptr
[0];
189 } else if ((ptr
[1] == LIBUSB20_DT_INTERFACE
) && (ptr
[0] >= 4)) {
190 if (ptr
[2] != iface_no
) {
194 lub_config
->num_interface
++;
195 last_if
= lub_interface
;
198 /* one more alternate setting */
199 lub_interface
->num_altsetting
++;
200 last_if
= lub_alt_interface
;
204 LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC
, &last_if
->desc
);
206 if (libusb20_me_decode(ptr
, ptr
[0], &last_if
->desc
)) {
210 * Sometimes USB devices have corrupt interface
211 * descriptors and we need to overwrite the provided
214 last_if
->desc
.bInterfaceNumber
= niface
- 1;
215 last_if
->extra
.ptr
= LIBUSB20_ADD_BYTES(ptr
, ptr
[0]);
216 last_if
->extra
.len
= 0;
217 last_if
->extra
.type
= LIBUSB20_ME_IS_RAW
;
218 last_if
->endpoints
= lub_endpoint
+ 1;
219 last_if
->altsetting
= lub_alt_interface
;
220 last_if
->num_altsetting
= 0;
221 last_if
->num_endpoints
= 0;
224 /* unknown descriptor */
227 last_ep
->extra
.len
+= ptr
[0];
229 last_if
->extra
.len
+= ptr
[0];
232 lub_config
->extra
.len
+= ptr
[0];
239 /*------------------------------------------------------------------------*
240 * libusb20_desc_foreach
242 * Safe traversal of USB descriptors.
245 * NULL: End of descriptors
246 * Else: Pointer to next descriptor
247 *------------------------------------------------------------------------*/
249 libusb20_desc_foreach(const struct libusb20_me_struct
*pdesc
,
250 const uint8_t *psubdesc
)
252 const uint8_t *start
;
254 const uint8_t *desc_next
;
260 start
= (const uint8_t *)pdesc
->ptr
;
261 end
= LIBUSB20_ADD_BYTES(start
, pdesc
->len
);
263 /* get start of next descriptor */
264 if (psubdesc
== NULL
)
267 psubdesc
= psubdesc
+ psubdesc
[0];
269 /* check that the next USB descriptor is within the range */
270 if ((psubdesc
< start
) || (psubdesc
>= end
))
271 return (NULL
); /* out of range, or EOD */
273 /* check start of the second next USB descriptor, if any */
274 desc_next
= psubdesc
+ psubdesc
[0];
275 if ((desc_next
< start
) || (desc_next
> end
))
276 return (NULL
); /* out of range */
278 /* check minimum descriptor length */
280 return (NULL
); /* too short descriptor */
282 return (psubdesc
); /* return start of next descriptor */
285 /*------------------------------------------------------------------------*
286 * libusb20_me_get_1 - safety wrapper to read out one byte
287 *------------------------------------------------------------------------*/
289 libusb20_me_get_1(const struct libusb20_me_struct
*ie
, uint16_t offset
)
291 if (offset
< ie
->len
) {
292 return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie
->ptr
, offset
)));
297 /*------------------------------------------------------------------------*
298 * libusb20_me_get_2 - safety wrapper to read out one word
299 *------------------------------------------------------------------------*/
301 libusb20_me_get_2(const struct libusb20_me_struct
*ie
, uint16_t offset
)
303 return (libusb20_me_get_1(ie
, offset
) |
304 (libusb20_me_get_1(ie
, offset
+ 1) << 8));
307 /*------------------------------------------------------------------------*
308 * libusb20_me_encode - encode a message structure
310 * Description of parameters:
311 * "len" - maximum length of output buffer
312 * "ptr" - pointer to output buffer. If NULL, no data will be written
313 * "pd" - source structure
316 * 0..65535 - Number of bytes used, limited by the "len" input parameter.
317 *------------------------------------------------------------------------*/
319 libusb20_me_encode(void *ptr
, uint16_t len
, const void *pd
)
321 const uint8_t *pf
; /* pointer to format data */
322 uint8_t *buf
; /* pointer to output buffer */
324 uint32_t pd_offset
; /* decoded structure offset */
325 uint16_t len_old
; /* old length */
326 uint16_t pd_count
; /* decoded element count */
327 uint8_t me
; /* message element */
333 pd_offset
= sizeof(void *);
334 pf
= (*((struct libusb20_me_format
*const *)pd
))->format
;
340 /* get information element */
342 me
= (pf
[0]) & LIBUSB20_ME_MASK
;
343 pd_count
= pf
[1] | (pf
[2] << 8);
346 /* encode the message element */
349 case LIBUSB20_ME_INT8
:
353 if (len
< 1) /* overflow */
356 temp
= *((const uint8_t *)
357 LIBUSB20_ADD_BYTES(pd
, pd_offset
));
366 case LIBUSB20_ME_INT16
:
367 pd_offset
= -((-pd_offset
) & ~1); /* align */
371 if (len
< 2) /* overflow */
375 temp
= *((const uint16_t *)
376 LIBUSB20_ADD_BYTES(pd
, pd_offset
));
377 buf
[1] = (temp
>> 8) & 0xFF;
378 buf
[0] = temp
& 0xFF;
386 case LIBUSB20_ME_INT32
:
387 pd_offset
= -((-pd_offset
) & ~3); /* align */
391 if (len
< 4) /* overflow */
394 temp
= *((const uint32_t *)
395 LIBUSB20_ADD_BYTES(pd
, pd_offset
));
396 buf
[3] = (temp
>> 24) & 0xFF;
397 buf
[2] = (temp
>> 16) & 0xFF;
398 buf
[1] = (temp
>> 8) & 0xFF;
399 buf
[0] = temp
& 0xFF;
407 case LIBUSB20_ME_INT64
:
408 pd_offset
= -((-pd_offset
) & ~7); /* align */
412 if (len
< 8) /* overflow */
416 temp
= *((const uint64_t *)
417 LIBUSB20_ADD_BYTES(pd
, pd_offset
));
418 buf
[7] = (temp
>> 56) & 0xFF;
419 buf
[6] = (temp
>> 48) & 0xFF;
420 buf
[5] = (temp
>> 40) & 0xFF;
421 buf
[4] = (temp
>> 32) & 0xFF;
422 buf
[3] = (temp
>> 24) & 0xFF;
423 buf
[2] = (temp
>> 16) & 0xFF;
424 buf
[1] = (temp
>> 8) & 0xFF;
425 buf
[0] = temp
& 0xFF;
433 case LIBUSB20_ME_STRUCT
:
434 pd_offset
= -((-pd_offset
) &
435 ~(LIBUSB20_ME_STRUCT_ALIGN
- 1)); /* 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
= -((-pd_offset
) &
673 ~(LIBUSB20_ME_STRUCT_ALIGN
- 1)); /* align */
677 struct libusb20_me_struct
*ps
;
679 ps
= LIBUSB20_ADD_BYTES(pd
, pd_offset
);
681 if (ps
->type
== LIBUSB20_ME_IS_ENCODED
) {
683 * Pre-store a de-constified
687 ps
->ptr
= LIBUSB20_ADD_BYTES(buf
, 0);
690 * Get the correct number of
694 if (buf
[0] == 0xFF) {
703 /* get the structure length */
706 if (buf
[0] == 0xFF) {
725 /* check for invalid length */
731 /* check wanted structure type */
734 case LIBUSB20_ME_IS_ENCODED
:
735 /* check for zero length */
741 ps
->ptr
= LIBUSB20_ADD_BYTES(
742 libusb20_me_encode_empty
, 0);
749 case LIBUSB20_ME_IS_RAW
:
750 /* update length and pointer */
752 ps
->ptr
= LIBUSB20_ADD_BYTES(buf
, 0);
755 case LIBUSB20_ME_IS_EMPTY
:
756 case LIBUSB20_ME_IS_DECODED
:
757 /* check for non-zero length */
760 ps
->type
= LIBUSB20_ME_IS_DECODED
;
766 dummy
= libusb20_me_decode(buf
,
770 ps
->type
= LIBUSB20_ME_IS_EMPTY
;
777 * nothing to do - should
786 pd_offset
+= sizeof(struct libusb20_me_struct
);
795 return (len_old
- len
);