lib/libusb/libusb20_desc.c

   1 /* $FreeBSD: head/lib/libusb/libusb20_desc.c 248236 2013-03-13 12:23:14Z hselasky $ */
   2 /*-
   3  * Copyright (c) 2008 Hans Petter Selasky. All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   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.
  13  *
  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
  24  * SUCH DAMAGE.
  25  */
  26
  27 #ifdef LIBUSB_GLOBAL_INCLUDE_FILE
  28 #include LIBUSB_GLOBAL_INCLUDE_FILE
  29 #else
  30 #include <stdio.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <time.h>
  34 #include <sys/param.h>
  35 #include <sys/queue.h>
  36 #endif
  37
  38 #include "libusb20.h"
  39 #include "libusb20_desc.h"
  40 #include "libusb20_int.h"
  41
  42 static const uint32_t libusb20_me_encode_empty[2];      /* dummy */
  43
  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);
  53
  54 /*------------------------------------------------------------------------*
  55  *      libusb20_parse_config_desc
  56  *
  57  * Return values:
  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)
  63 {
  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;
  70
  71         struct libusb20_me_struct pcdesc;
  72         const uint8_t *ptr;
  73         uint32_t size;
  74         uint16_t niface_no_alt;
  75         uint16_t niface;
  76         uint16_t nendpoint;
  77         uint16_t iface_no;
  78
  79         ptr = config_desc;
  80         if (ptr[1] != LIBUSB20_DT_CONFIG) {
  81                 return (NULL);          /* not config descriptor */
  82         }
  83         /*
  84          * The first "bInterfaceNumber" should never have the value 0xff.
  85          * Then it is corrupt.
  86          */
  87         niface_no_alt = 0;
  88         nendpoint = 0;
  89         niface = 0;
  90         iface_no = 0xFFFF;
  91         ptr = NULL;
  92
  93         /* get "wTotalLength" and setup "pcdesc" */
  94         pcdesc.ptr = LIBUSB20_ADD_BYTES(config_desc, 0);
  95         pcdesc.len =
  96             ((const uint8_t *)config_desc)[2] |
  97             (((const uint8_t *)config_desc)[3] << 8);
  98         pcdesc.type = LIBUSB20_ME_IS_RAW;
  99
 100         /* descriptor pre-scan */
 101         while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
 102                 if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
 103                         nendpoint++;
 104                 } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
 105                         niface++;
 106                         /* check "bInterfaceNumber" */
 107                         if (ptr[2] != iface_no) {
 108                                 iface_no = ptr[2];
 109                                 niface_no_alt++;
 110                         }
 111                 }
 112         }
 113
 114         /* sanity checking */
 115         if (niface >= 256) {
 116                 return (NULL);          /* corrupt */
 117         }
 118         if (nendpoint >= 256) {
 119                 return (NULL);          /* corrupt */
 120         }
 121         size = sizeof(*lub_config) +
 122             (niface * sizeof(*lub_interface)) +
 123             (nendpoint * sizeof(*lub_endpoint)) +
 124             pcdesc.len;
 125
 126         lub_config = malloc(size);
 127         if (lub_config == NULL) {
 128                 return (NULL);          /* out of memory */
 129         }
 130         /* make sure memory is initialised */
 131         memset(lub_config, 0, size);
 132
 133         lub_interface = (void *)(lub_config + 1);
 134         lub_alt_interface = (void *)(lub_interface + niface_no_alt);
 135         lub_endpoint = (void *)(lub_interface + niface);
 136
 137         /*
 138          * Make a copy of the config descriptor, so that the caller can free
 139          * the inital config descriptor pointer!
 140          */
 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);
 145
 146         /* init config structure */
 147
 148         ptr = config_desc;
 149
 150         LIBUSB20_INIT(LIBUSB20_CONFIG_DESC, &lub_config->desc);
 151
 152         if (libusb20_me_decode(ptr, ptr[0], &lub_config->desc)) {
 153                 /* ignore */
 154         }
 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;
 160
 161         /* reset states */
 162         niface = 0;
 163         iface_no = 0xFFFF;
 164         ptr = NULL;
 165         lub_interface--;
 166         lub_endpoint--;
 167         last_if = NULL;
 168         last_ep = NULL;
 169
 170         /* descriptor pre-scan */
 171         while ((ptr = libusb20_desc_foreach(&pcdesc, ptr))) {
 172                 if (ptr[1] == LIBUSB20_DT_ENDPOINT) {
 173                         if (last_if) {
 174                                 lub_endpoint++;
 175                                 last_ep = lub_endpoint;
 176                                 last_if->num_endpoints++;
 177
 178                                 LIBUSB20_INIT(LIBUSB20_ENDPOINT_DESC, &last_ep->desc);
 179
 180                                 if (libusb20_me_decode(ptr, ptr[0], &last_ep->desc)) {
 181                                         /* ignore */
 182                                 }
 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;
 186                         } else {
 187                                 lub_config->extra.len += ptr[0];
 188                         }
 189
 190                 } else if ((ptr[1] == LIBUSB20_DT_INTERFACE) && (ptr[0] >= 4)) {
 191                         if (ptr[2] != iface_no) {
 192                                 /* new interface */
 193                                 iface_no = ptr[2];
 194                                 lub_interface++;
 195                                 lub_config->num_interface++;
 196                                 last_if = lub_interface;
 197                                 niface++;
 198                         } else {
 199                                 /* one more alternate setting */
 200                                 lub_interface->num_altsetting++;
 201                                 last_if = lub_alt_interface;
 202                                 lub_alt_interface++;
 203                         }
 204
 205                         LIBUSB20_INIT(LIBUSB20_INTERFACE_DESC, &last_if->desc);
 206
 207                         if (libusb20_me_decode(ptr, ptr[0], &last_if->desc)) {
 208                                 /* ignore */
 209                         }
 210                         /*
 211                          * Sometimes USB devices have corrupt interface
 212                          * descriptors and we need to overwrite the provided
 213                          * interface number!
 214                          */
 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;
 223                         last_ep = NULL;
 224                 } else {
 225                         /* unknown descriptor */
 226                         if (last_if) {
 227                                 if (last_ep) {
 228                                         last_ep->extra.len += ptr[0];
 229                                 } else {
 230                                         last_if->extra.len += ptr[0];
 231                                 }
 232                         } else {
 233                                 lub_config->extra.len += ptr[0];
 234                         }
 235                 }
 236         }
 237         return (lub_config);
 238 }
 239
 240 /*------------------------------------------------------------------------*
 241  *      libusb20_desc_foreach
 242  *
 243  * Safe traversal of USB descriptors.
 244  *
 245  * Return values:
 246  * NULL: End of descriptors
 247  * Else: Pointer to next descriptor
 248  *------------------------------------------------------------------------*/
 249 const uint8_t *
 250 libusb20_desc_foreach(const struct libusb20_me_struct *pdesc,
 251     const uint8_t *psubdesc)
 252 {
 253         const uint8_t *start;
 254         const uint8_t *end;
 255         const uint8_t *desc_next;
 256
 257         /* be NULL safe */
 258         if (pdesc == NULL)
 259                 return (NULL);
 260
 261         start = (const uint8_t *)pdesc->ptr;
 262         end = LIBUSB20_ADD_BYTES(start, pdesc->len);
 263
 264         /* get start of next descriptor */
 265         if (psubdesc == NULL)
 266                 psubdesc = start;
 267         else
 268                 psubdesc = psubdesc + psubdesc[0];
 269
 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 */
 273
 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 */
 278
 279         /* check minimum descriptor length */
 280         if (psubdesc[0] < 3)
 281                 return (NULL);          /* too short descriptor */
 282
 283         return (psubdesc);              /* return start of next descriptor */
 284 }
 285
 286 /*------------------------------------------------------------------------*
 287  *      libusb20_me_get_1 - safety wrapper to read out one byte
 288  *------------------------------------------------------------------------*/
 289 uint8_t
 290 libusb20_me_get_1(const struct libusb20_me_struct *ie, uint16_t offset)
 291 {
 292         if (offset < ie->len) {
 293                 return (*((uint8_t *)LIBUSB20_ADD_BYTES(ie->ptr, offset)));
 294         }
 295         return (0);
 296 }
 297
 298 /*------------------------------------------------------------------------*
 299  *      libusb20_me_get_2 - safety wrapper to read out one word
 300  *------------------------------------------------------------------------*/
 301 uint16_t
 302 libusb20_me_get_2(const struct libusb20_me_struct *ie, uint16_t offset)
 303 {
 304         return (libusb20_me_get_1(ie, offset) |
 305             (libusb20_me_get_1(ie, offset + 1) << 8));
 306 }
 307
 308 /*------------------------------------------------------------------------*
 309  *      libusb20_me_encode - encode a message structure
 310  *
 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
 315  *
 316  * Return values:
 317  * 0..65535 - Number of bytes used, limited by the "len" input parameter.
 318  *------------------------------------------------------------------------*/
 319 uint16_t
 320 libusb20_me_encode(void *ptr, uint16_t len, const void *pd)
 321 {
 322         const uint8_t *pf;              /* pointer to format data */
 323         uint8_t *buf;                   /* pointer to output buffer */
 324
 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 */
 329
 330         /* initialise */
 331
 332         len_old = len;
 333         buf = ptr;
 334         pd_offset = sizeof(void *);
 335         pf = (*((struct libusb20_me_format *const *)pd))->format;
 336
 337         /* scan */
 338
 339         while (1) {
 340
 341                 /* get information element */
 342
 343                 me = (pf[0]) & LIBUSB20_ME_MASK;
 344                 pd_count = pf[1] | (pf[2] << 8);
 345                 pf += 3;
 346
 347                 /* encode the message element */
 348
 349                 switch (me) {
 350                 case LIBUSB20_ME_INT8:
 351                         while (pd_count--) {
 352                                 uint8_t temp;
 353
 354                                 if (len < 1)    /* overflow */
 355                                         goto done;
 356                                 if (buf) {
 357                                         temp = *((const uint8_t *)
 358                                             LIBUSB20_ADD_BYTES(pd, pd_offset));
 359                                         buf[0] = temp;
 360                                         buf += 1;
 361                                 }
 362                                 pd_offset += 1;
 363                                 len -= 1;
 364                         }
 365                         break;
 366
 367                 case LIBUSB20_ME_INT16:
 368                         pd_offset = -((-pd_offset) & ~1);       /* align */
 369                         while (pd_count--) {
 370                                 uint16_t temp;
 371
 372                                 if (len < 2)    /* overflow */
 373                                         goto done;
 374
 375                                 if (buf) {
 376                                         temp = *((const uint16_t *)
 377                                             LIBUSB20_ADD_BYTES(pd, pd_offset));
 378                                         buf[1] = (temp >> 8) & 0xFF;
 379                                         buf[0] = temp & 0xFF;
 380                                         buf += 2;
 381                                 }
 382                                 pd_offset += 2;
 383                                 len -= 2;
 384                         }
 385                         break;
 386
 387                 case LIBUSB20_ME_INT32:
 388                         pd_offset = -((-pd_offset) & ~3);       /* align */
 389                         while (pd_count--) {
 390                                 uint32_t temp;
 391
 392                                 if (len < 4)    /* overflow */
 393                                         goto done;
 394                                 if (buf) {
 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;
 401                                         buf += 4;
 402                                 }
 403                                 pd_offset += 4;
 404                                 len -= 4;
 405                         }
 406                         break;
 407
 408                 case LIBUSB20_ME_INT64:
 409                         pd_offset = -((-pd_offset) & ~7);       /* align */
 410                         while (pd_count--) {
 411                                 uint64_t temp;
 412
 413                                 if (len < 8)    /* overflow */
 414                                         goto done;
 415                                 if (buf) {
 416
 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;
 427                                         buf += 8;
 428                                 }
 429                                 pd_offset += 8;
 430                                 len -= 8;
 431                         }
 432                         break;
 433
 434                 case LIBUSB20_ME_STRUCT:
 435                         pd_offset = -rounddown2(-pd_offset, LIBUSB20_ME_STRUCT_ALIGN);  /* align */
 436                         while (pd_count--) {
 437                                 void *src_ptr;
 438                                 uint16_t src_len;
 439                                 struct libusb20_me_struct *ps;
 440
 441                                 ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
 442
 443                                 switch (ps->type) {
 444                                 case LIBUSB20_ME_IS_RAW:
 445                                         src_len = ps->len;
 446                                         src_ptr = ps->ptr;
 447                                         break;
 448
 449                                 case LIBUSB20_ME_IS_ENCODED:
 450                                         if (ps->len == 0) {
 451                                                 /*
 452                                                  * Length is encoded
 453                                                  * in the data itself
 454                                                  * and should be
 455                                                  * correct:
 456                                                  */
 457                                                 ps->len = 0xFFFF;
 458                                         }
 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);
 464                                                 src_ptr =
 465                                                     LIBUSB20_ADD_BYTES(ps->ptr, 3);
 466                                         }
 467                                         break;
 468
 469                                 case LIBUSB20_ME_IS_DECODED:
 470                                         /* reserve 3 length bytes */
 471                                         src_len = libusb20_me_encode(NULL,
 472                                             0xFFFF - 3, ps->ptr);
 473                                         src_ptr = NULL;
 474                                         break;
 475
 476                                 default:        /* empty structure */
 477                                         src_len = 0;
 478                                         src_ptr = NULL;
 479                                         break;
 480                                 }
 481
 482                                 if (src_len > 0xFE) {
 483                                         if (src_len > (0xFFFF - 3))
 484                                                 /* overflow */
 485                                                 goto done;
 486
 487                                         if (len < (src_len + 3))
 488                                                 /* overflow */
 489                                                 goto done;
 490
 491                                         if (buf) {
 492                                                 buf[0] = 0xFF;
 493                                                 buf[1] = (src_len & 0xFF);
 494                                                 buf[2] = (src_len >> 8) & 0xFF;
 495                                                 buf += 3;
 496                                         }
 497                                         len -= (src_len + 3);
 498                                 } else {
 499                                         if (len < (src_len + 1))
 500                                                 /* overflow */
 501                                                 goto done;
 502
 503                                         if (buf) {
 504                                                 buf[0] = (src_len & 0xFF);
 505                                                 buf += 1;
 506                                         }
 507                                         len -= (src_len + 1);
 508                                 }
 509
 510                                 /* check for buffer and non-zero length */
 511
 512                                 if (buf && src_len) {
 513                                         if (ps->type == LIBUSB20_ME_IS_DECODED) {
 514                                                 /*
 515                                                  * Repeat encode
 516                                                  * procedure - we have
 517                                                  * room for the
 518                                                  * complete structure:
 519                                                  */
 520                                                 uint16_t dummy;
 521
 522                                                 dummy = libusb20_me_encode(buf,
 523                                                     0xFFFF - 3, ps->ptr);
 524                                         } else {
 525                                                 bcopy(src_ptr, buf, src_len);
 526                                         }
 527                                         buf += src_len;
 528                                 }
 529                                 pd_offset += sizeof(struct libusb20_me_struct);
 530                         }
 531                         break;
 532
 533                 default:
 534                         goto done;
 535                 }
 536         }
 537 done:
 538         return (len_old - len);
 539 }
 540
 541 /*------------------------------------------------------------------------*
 542  *      libusb20_me_decode - decode a message into a decoded structure
 543  *
 544  * Description of parameters:
 545  * "ptr" - message pointer
 546  * "len" - message length
 547  * "pd" - pointer to decoded structure
 548  *
 549  * Returns:
 550  * "0..65535" - number of bytes decoded, limited by "len"
 551  *------------------------------------------------------------------------*/
 552 uint16_t
 553 libusb20_me_decode(const void *ptr, uint16_t len, void *pd)
 554 {
 555         const uint8_t *pf;              /* pointer to format data */
 556         const uint8_t *buf;             /* pointer to input buffer */
 557
 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 */
 562
 563         /* initialise */
 564
 565         len_old = len;
 566         buf = ptr;
 567         pd_offset = sizeof(void *);
 568         pf = (*((struct libusb20_me_format **)pd))->format;
 569
 570         /* scan */
 571
 572         while (1) {
 573
 574                 /* get information element */
 575
 576                 me = (pf[0]) & LIBUSB20_ME_MASK;
 577                 pd_count = pf[1] | (pf[2] << 8);
 578                 pf += 3;
 579
 580                 /* decode the message element by type */
 581
 582                 switch (me) {
 583                 case LIBUSB20_ME_INT8:
 584                         while (pd_count--) {
 585                                 uint8_t temp;
 586
 587                                 if (len < 1) {
 588                                         len = 0;
 589                                         temp = 0;
 590                                 } else {
 591                                         len -= 1;
 592                                         temp = buf[0];
 593                                         buf++;
 594                                 }
 595                                 *((uint8_t *)LIBUSB20_ADD_BYTES(pd,
 596                                     pd_offset)) = temp;
 597                                 pd_offset += 1;
 598                         }
 599                         break;
 600
 601                 case LIBUSB20_ME_INT16:
 602                         pd_offset = -((-pd_offset) & ~1);       /* align */
 603                         while (pd_count--) {
 604                                 uint16_t temp;
 605
 606                                 if (len < 2) {
 607                                         len = 0;
 608                                         temp = 0;
 609                                 } else {
 610                                         len -= 2;
 611                                         temp = buf[1] << 8;
 612                                         temp |= buf[0];
 613                                         buf += 2;
 614                                 }
 615                                 *((uint16_t *)LIBUSB20_ADD_BYTES(pd,
 616                                     pd_offset)) = temp;
 617                                 pd_offset += 2;
 618                         }
 619                         break;
 620
 621                 case LIBUSB20_ME_INT32:
 622                         pd_offset = -((-pd_offset) & ~3);       /* align */
 623                         while (pd_count--) {
 624                                 uint32_t temp;
 625
 626                                 if (len < 4) {
 627                                         len = 0;
 628                                         temp = 0;
 629                                 } else {
 630                                         len -= 4;
 631                                         temp = buf[3] << 24;
 632                                         temp |= buf[2] << 16;
 633                                         temp |= buf[1] << 8;
 634                                         temp |= buf[0];
 635                                         buf += 4;
 636                                 }
 637
 638                                 *((uint32_t *)LIBUSB20_ADD_BYTES(pd,
 639                                     pd_offset)) = temp;
 640                                 pd_offset += 4;
 641                         }
 642                         break;
 643
 644                 case LIBUSB20_ME_INT64:
 645                         pd_offset = -((-pd_offset) & ~7);       /* align */
 646                         while (pd_count--) {
 647                                 uint64_t temp;
 648
 649                                 if (len < 8) {
 650                                         len = 0;
 651                                         temp = 0;
 652                                 } else {
 653                                         len -= 8;
 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;
 660                                         temp |= buf[1] << 8;
 661                                         temp |= buf[0];
 662                                         buf += 8;
 663                                 }
 664
 665                                 *((uint64_t *)LIBUSB20_ADD_BYTES(pd,
 666                                     pd_offset)) = temp;
 667                                 pd_offset += 8;
 668                         }
 669                         break;
 670
 671                 case LIBUSB20_ME_STRUCT:
 672                         pd_offset = -rounddown2(-pd_offset, LIBUSB20_ME_STRUCT_ALIGN);  /* align */
 673                         while (pd_count--) {
 674                                 uint16_t temp;
 675                                 uint16_t dummy;
 676                                 struct libusb20_me_struct *ps;
 677
 678                                 ps = LIBUSB20_ADD_BYTES(pd, pd_offset);
 679
 680                                 if (ps->type == LIBUSB20_ME_IS_ENCODED) {
 681                                         /*
 682                                          * Pre-store a de-constified
 683                                          * pointer to the raw
 684                                          * structure:
 685                                          */
 686                                         ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
 687
 688                                         /*
 689                                          * Get the correct number of
 690                                          * length bytes:
 691                                          */
 692                                         if (len != 0) {
 693                                                 if (buf[0] == 0xFF) {
 694                                                         ps->len = 3;
 695                                                 } else {
 696                                                         ps->len = 1;
 697                                                 }
 698                                         } else {
 699                                                 ps->len = 0;
 700                                         }
 701                                 }
 702                                 /* get the structure length */
 703
 704                                 if (len != 0) {
 705                                         if (buf[0] == 0xFF) {
 706                                                 if (len < 3) {
 707                                                         len = 0;
 708                                                         temp = 0;
 709                                                 } else {
 710                                                         len -= 3;
 711                                                         temp = buf[1] |
 712                                                             (buf[2] << 8);
 713                                                         buf += 3;
 714                                                 }
 715                                         } else {
 716                                                 len -= 1;
 717                                                 temp = buf[0];
 718                                                 buf += 1;
 719                                         }
 720                                 } else {
 721                                         len = 0;
 722                                         temp = 0;
 723                                 }
 724                                 /* check for invalid length */
 725
 726                                 if (temp > len) {
 727                                         len = 0;
 728                                         temp = 0;
 729                                 }
 730                                 /* check wanted structure type */
 731
 732                                 switch (ps->type) {
 733                                 case LIBUSB20_ME_IS_ENCODED:
 734                                         /* check for zero length */
 735                                         if (temp == 0) {
 736                                                 /*
 737                                                  * The pointer must
 738                                                  * be valid:
 739                                                  */
 740                                                 ps->ptr = LIBUSB20_ADD_BYTES(
 741                                                     libusb20_me_encode_empty, 0);
 742                                                 ps->len = 1;
 743                                         } else {
 744                                                 ps->len += temp;
 745                                         }
 746                                         break;
 747
 748                                 case LIBUSB20_ME_IS_RAW:
 749                                         /* update length and pointer */
 750                                         ps->len = temp;
 751                                         ps->ptr = LIBUSB20_ADD_BYTES(buf, 0);
 752                                         break;
 753
 754                                 case LIBUSB20_ME_IS_EMPTY:
 755                                 case LIBUSB20_ME_IS_DECODED:
 756                                         /* check for non-zero length */
 757                                         if (temp != 0) {
 758                                                 /* update type */
 759                                                 ps->type = LIBUSB20_ME_IS_DECODED;
 760                                                 ps->len = 0;
 761                                                 /*
 762                                                  * Recursivly decode
 763                                                  * the next structure
 764                                                  */
 765                                                 dummy = libusb20_me_decode(buf,
 766                                                     temp, ps->ptr);
 767                                         } else {
 768                                                 /* update type */
 769                                                 ps->type = LIBUSB20_ME_IS_EMPTY;
 770                                                 ps->len = 0;
 771                                         }
 772                                         break;
 773
 774                                 default:
 775                                         /*
 776                                          * nothing to do - should
 777                                          * not happen
 778                                          */
 779                                         ps->ptr = NULL;
 780                                         ps->len = 0;
 781                                         break;
 782                                 }
 783                                 buf += temp;
 784                                 len -= temp;
 785                                 pd_offset += sizeof(struct libusb20_me_struct);
 786                         }
 787                         break;
 788
 789                 default:
 790                         goto done;
 791                 }
 792         }
 793 done:
 794         return (len_old - len);
 795 }