lib/libusb/libusb20_desc.c

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