plugins/llrp/llrpparse.c

   1 /* EPCglobal Low-Level Reader Protocol Packet Dissector
   2  *
   3  * Copyright 2008, Intermec Technologies Corp. <matt.poduska@intermec.com>
   4  *
   5  * $Id$
   6  *
   7  * Wireshark - Network traffic analyzer
   8  * By Gerald Combs <gerald@wireshark.org>
   9  * Copyright 1999 Gerald Combs
  10  *
  11  * This program is free software; you can redistribute it and/or
  12  * modify it under the terms of the GNU General Public License
  13  * as published by the Free Software Foundation; either version 2
  14  * of the License, or (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program; if not, write to the Free Software
  23  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  24  */
  25
  26 #include <stdio.h> /* For NULL */
  27 #include "llrpparsetypes.h"
  28 #include "llrpparse.h"
  29
  30 /* ------------------------------------------------------------------------------- */
  31 /* Local Function Declarations                                                     */
  32
  33 static void llrp_ParseParameter(t_llrp_parse_context *context, unsigned short usType,
  34  int bImpliedLength, unsigned short usLength);
  35 static void llrp_ParseCustomParameter(t_llrp_parse_context *context, unsigned short usType,
  36  unsigned short usLength);
  37
  38 static void llrp_ParseCompoundItem(t_llrp_parse_context *context,
  39  t_llrp_compound_item *pCompoundItem, unsigned long ulTotalLength);
  40
  41 static int llrp_ParseFixedItems(t_llrp_parse_context *context, t_llrp_item *pItemList,
  42  unsigned short usTotalItems, unsigned short *pusParsedItems, unsigned long ulTotalLength,
  43  unsigned long *pulConsumedBytes);
  44 static int llrp_ParseVariableItems(t_llrp_parse_context *context, t_llrp_item *pItemList,
  45  unsigned short usTotalItems, unsigned long ulTotalLength);
  46
  47 static int llrp_HandleField(t_llrp_parse_context *context, t_llrp_item *pItem,
  48  unsigned short usFieldIndex, unsigned long ulTotalLength, unsigned char *pucBitAccumulator,
  49  unsigned char *pucAccumulatedBits);
  50
  51 static int llrp_ParseGetParameterContentLength(const t_llrp_compound_item *pItem,
  52  unsigned short *pusLength);
  53
  54 #define llrp_ReportError(pContext, parameterList) \
  55 { if((pContext)->parse_error_handler != NULL) (pContext)->parse_error_handler parameterList; }
  56
  57 #define llrp_ReportMessage(pContext, parameterList) \
  58 { if((pContext)->debug_message_handler != NULL) (pContext)->debug_message_handler parameterList; }
  59
  60 #define llrp_StreamRead(pContext, readLength, readWaitForever, pReadResult) \
  61     (pContext)->stream_read_handler(pContext, readLength, readWaitForever, pReadResult)
  62
  63 #define llrp_StreamGetOffset(pContext) \
  64     (pContext)->stream_get_offset_handler(pContext)
  65
  66 /* ------------------------------------------------------------------------------- */
  67 /* Parsing                                                                         */
  68
  69 int llrp_ParseMessage(t_llrp_parse_context *context)
  70 {
  71     unsigned char ucVersion;
  72     unsigned short *pusData, usValidatorIndex, usMessageIndex, usType;
  73     unsigned long *pulData, ulLength, ulID, ulReadBytes;
  74     t_llrp_parse_validator *validator;
  75     int bContinueParse;
  76
  77     /* Make sure the context is valid */
  78     if(context->stream_read_handler == NULL || context->stream_get_offset_handler == NULL)
  79     {
  80         llrp_ReportError(context, (context, LLRP_CONTEXT_ERROR, 0, "llrp_ParseMessage",
  81          "Invalid context"));
  82         return LLRP_PARSE_RESULT_FAILURE;
  83     }
  84
  85     /* Ensure the stream starts at offset 0 */
  86     if(llrp_StreamGetOffset(context) != 0)
  87     {
  88         llrp_ReportError(context, (context, LLRP_CONTEXT_ERROR, 0, "llrp_ParseMessage",
  89          "Stream not starting at offset zero (current offset %lu)", llrp_StreamGetOffset(context)));
  90         return LLRP_PARSE_RESULT_FAILURE;
  91     }
  92
  93     context->depth = 0; /* Messages always begin at parse depth 0 */
  94
  95     /* Bytes 0-1: Type and version */
  96     pusData= (unsigned short *) llrp_StreamRead(context, 2, 1, &ulReadBytes);
  97     if(ulReadBytes != 2)
  98     {
  99         /* No error here - this happens when the stream read timed out */
 100         return LLRP_PARSE_RESULT_NO_PARSE;
 101     }
 102     ucVersion = (unsigned char) (((unsigned char)((llrp_ntohs(*pusData)) >> 10)) & 0x07);
 103     usType = (unsigned short) (llrp_ntohs(*pusData) & 0x3FF);
 104
 105     /* Bytes 2-5: Message length */
 106     pulData = (unsigned long *) llrp_StreamRead(context, 4, 0, &ulReadBytes);
 107     if(ulReadBytes != 4)
 108     {
 109         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_MESSAGE_DATA_UNDERFLOW, 0,
 110          "llrp_ParseMessage", "Failed to read message length bytes"));
 111         return LLRP_PARSE_RESULT_PARSE_FAILED;
 112     }
 113     ulLength = llrp_ntohl(*pulData);
 114
 115     /* Bytes 6-9: Message ID */
 116     pulData = (unsigned long *) llrp_StreamRead(context, 4, 0, &ulReadBytes);
 117     if(ulReadBytes != 4)
 118     {
 119         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_MESSAGE_DATA_UNDERFLOW, 0,
 120          "llrp_ParseMessage", "Failed to read message ID bytes"));
 121         return LLRP_PARSE_RESULT_PARSE_FAILED;
 122     }
 123     ulID = llrp_ntohl(*pulData);
 124
 125     /* TODO: Use the message version to select the proper validator */
 126     for(usValidatorIndex = 0; usValidatorIndex < context->validator_count; usValidatorIndex++)
 127     {
 128         validator = context->validator_list[usValidatorIndex];
 129         for(usMessageIndex = 0; usMessageIndex < validator->message_count; usMessageIndex++)
 130         {
 131             if(validator->message_list[usMessageIndex].number == usType)
 132             {
 133                 llrp_ReportMessage(context, (context, "llrp_ParseMessage",
 134                  "Message header parsed: version %u, type %u, length %u, ID %u",
 135                  ucVersion, usType, ulLength, ulID));
 136
 137                 if(context->message_start_handler != NULL)
 138                 {
 139                     bContinueParse= context->message_start_handler(context, ucVersion, usType,
 140                      ulLength, ulID, (validator->message_list[usMessageIndex].item)->name);
 141                 }
 142                 else
 143                     bContinueParse = 1;
 144
 145                 if(bContinueParse)
 146                 {
 147                     llrp_ParseCompoundItem(context, validator->message_list[usMessageIndex].item,
 148                      ulLength-LLRP_HEADER_LENGTH);
 149                 }
 150                 else
 151                 {
 152                     llrp_ReportMessage(context, (context, "llrp_ParseMessage", "Skipping message parse"));
 153
 154                     /* Consume/discard all remaining message data */
 155                     (void)llrp_StreamRead(context, ulLength-LLRP_HEADER_LENGTH, 0, &ulReadBytes);
 156                 }
 157
 158                 if(context->message_finished_handler != NULL)
 159                 {
 160                     (void)context->message_finished_handler(context, ucVersion, usType,
 161                      ulLength, ulID, (validator->message_list[usMessageIndex].item)->name);
 162                 }
 163
 164                 return LLRP_PARSE_RESULT_SUCCESS;
 165             }
 166             if(validator->message_list[usMessageIndex].number > usType)
 167                 break;
 168         }
 169     }
 170
 171     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_MESSAGE_TYPE_UNKNOWN, usType,
 172      "llrp_ParseMessage", "Unknown message type (%u)", usType));
 173
 174     /* Consume/discard all remaining message data */
 175     (void)llrp_StreamRead(context, ulLength-LLRP_HEADER_LENGTH, 0, &ulReadBytes);
 176
 177     return LLRP_PARSE_RESULT_PARSE_FAILED;
 178 }
 179
 180 static void llrp_ParseParameter(t_llrp_parse_context *context, unsigned short usType,
 181  int bImpliedLength, unsigned short usLength)
 182 {
 183     unsigned short usTypeIndex, usValidatorIndex;
 184     unsigned long ulReadBytes;
 185     t_llrp_parse_validator *validator;
 186     t_llrp_standard_map_item *pMapItem;
 187     t_llrp_compound_item *pItem;
 188
 189     for(usValidatorIndex = 0; usValidatorIndex < context->validator_count; usValidatorIndex++)
 190     {
 191         validator = context->validator_list[usValidatorIndex];
 192         for(usTypeIndex = 0; usTypeIndex < validator->parameter_count; usTypeIndex++)
 193         {
 194             if(validator->parameter_list[usTypeIndex].number == usType)
 195             {
 196                 pMapItem = &(validator->parameter_list[usTypeIndex]);
 197                 pItem = pMapItem->item;
 198
 199                 if(bImpliedLength)
 200                 {
 201                     /* Calculate the length of all parameter contents. Normally this means
 202                      *  the parameter is TV encoded, containing only fixed-length fields
 203                      *  and no sub-parameters. */
 204                     if(!llrp_ParseGetParameterContentLength(pItem, &usLength))
 205                     {
 206                         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_TV_NOT_FOUND,
 207                          usType, "llrp_ParseParameter", "Failed to determine content size of parameter '%s' (ID %u)",
 208                          pItem->name, usType));
 209                         break;
 210                     }
 211                 }
 212
 213                 if(context->parameter_start_handler!= NULL)
 214                     context->parameter_start_handler(context, usType, pItem->name, usLength);
 215
 216                 llrp_ParseCompoundItem(context, pItem, usLength);
 217
 218                 if(context->parameter_finished_handler!= NULL)
 219                     context->parameter_finished_handler(context, usType, pItem->name, usLength);
 220                 return;
 221             }
 222
 223             /* Since the parameter list is ordered (ascending), break out of the loop
 224                once we've passed the usType we're looking for. */
 225             if(validator->parameter_list[usTypeIndex].number > usType)
 226                 break;
 227         }
 228     }
 229
 230     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_TYPE_UNKNOWN, usType,
 231      "llrp_ParseParameter", "Unknown parameter type (%u)", usType));
 232
 233     /* Discard the bytes in this unknown parameter */
 234     (void)llrp_StreamRead(context, usLength, 0, &ulReadBytes);
 235     if(ulReadBytes != usLength)
 236     {
 237         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW, usType,
 238          "llrp_ParseParameter", "Failed to read %u discarded bytes (%u read)",
 239          usLength, ulReadBytes));
 240     }
 241 }
 242
 243 static void llrp_ParseCustomParameter(t_llrp_parse_context *context, unsigned short usType,
 244  unsigned short usLength)
 245 {
 246     unsigned short usTypeIndex, usValidatorIndex;
 247     unsigned long *pulData, ulReadBytes, ulVendorID, ulSubtype;
 248     t_llrp_parse_validator *validator;
 249     t_llrp_custom_map_item *pMapItem;
 250     t_llrp_compound_item *pItem;
 251
 252     if(usLength < 8)
 253     {
 254         llrp_ReportMessage(context, (context, "llrp_ParseCustomParameter", "Invalid content length for custom parameter"));
 255     }
 256     else
 257     {
 258         /* Actual parameter length doesn't include the vendor ID or subtype */
 259         usLength = usLength - 8;
 260
 261         pulData = (unsigned long *)llrp_StreamRead(context, 4, 0, &ulReadBytes);
 262         if(ulReadBytes == 4)
 263             ulVendorID = llrp_ntohl(*pulData);
 264         else
 265         {
 266                 llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW, usType,
 267                             "llrp_ParseCustomParameter", "Failed to read vendor ID"));
 268                 return;
 269         }
 270
 271         pulData = (unsigned long *)llrp_StreamRead(context, 4, 0, &ulReadBytes);
 272         if(ulReadBytes == 4)
 273             ulSubtype = llrp_ntohl(*pulData);
 274         else
 275         {
 276                 llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW, usType,
 277              "llrp_ParseCustomParameter", "Failed to read subtype"));
 278                 return;
 279         }
 280
 281         for(usValidatorIndex = 0; usValidatorIndex < context->validator_count; usValidatorIndex++)
 282         {
 283             validator = context->validator_list[usValidatorIndex];
 284             for(usTypeIndex = 0; usTypeIndex < validator->custom_parameter_count; usTypeIndex++)
 285             {
 286                 pMapItem = &(validator->custom_parameter_list[usTypeIndex]);
 287                 if(pMapItem->vendor_id == ulVendorID && pMapItem->subtype == ulSubtype)
 288                 {
 289                     pItem = pMapItem->item;
 290
 291                     if(context->custom_parameter_start_handler!= NULL)
 292                     {
 293                         context->custom_parameter_start_handler(context, usType, ulVendorID,
 294                          ulSubtype, pItem->name, usLength);
 295                     }
 296
 297                     llrp_ParseCompoundItem(context, pItem, usLength);
 298
 299                     if(context->custom_parameter_finished_handler!= NULL)
 300                     {
 301                         context->custom_parameter_finished_handler(context, usType, ulVendorID,
 302                          ulSubtype, pItem->name, usLength);
 303                     }
 304                     return;
 305                 }
 306             }
 307         }
 308
 309         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_TYPE_UNKNOWN, usType,
 310          "llrp_ParseCustomParameter", "Unknown custom parameter (type %u, vendor %lu, subtype %lu)",
 311          usType, ulVendorID, ulSubtype));
 312     }
 313
 314     /* Discard the bytes in this unknown parameter */
 315     (void)llrp_StreamRead(context, usLength, 0, &ulReadBytes);
 316     if(ulReadBytes != usLength)
 317     {
 318         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW, usType,
 319          "llrp_ParseCustomParameter", "Failed to read %u discarded bytes (%u read)",
 320          usLength, ulReadBytes));
 321     }
 322 }
 323
 324 static void llrp_ParseCompoundItem(t_llrp_parse_context *context,
 325  t_llrp_compound_item *pCompoundItem, unsigned long ulTotalLength)
 326 {
 327     unsigned short usParsedItems, usItemsRemaining;
 328     unsigned long ulStartOffset, ulConsumedBytes, ulLengthRemaining;
 329
 330     ulStartOffset = llrp_StreamGetOffset(context);
 331
 332     llrp_ReportMessage(context, (context, "llrp_ParseCompoundItem",
 333      "Beginning parse of compound item '%s', type '%s', containing %u item%s, length %u bytes, stream offset %u",
 334      pCompoundItem->name, llrp_compound_item_name[pCompoundItem->type], pCompoundItem->item_count,
 335      (pCompoundItem->item_count == 1) ? "" : "s", ulTotalLength, ulStartOffset));
 336
 337     /* Each time a compound item is parsed, increase the depth. */
 338     (context->depth)++;
 339
 340     /* Parse all fixed-length items. These must occur first. */
 341     if(!llrp_ParseFixedItems(context, (t_llrp_item *) pCompoundItem->item_list,
 342      pCompoundItem->item_count, &usParsedItems, ulTotalLength, &ulConsumedBytes))
 343     {
 344         llrp_ReportMessage(context, (context, "llrp_ParseCompoundItem",
 345          "Error while parsing fixed items in '%s'", pCompoundItem->name));
 346     }
 347     else
 348     {
 349         ulLengthRemaining = (unsigned long) (ulTotalLength - ulConsumedBytes);
 350         usItemsRemaining = (unsigned short) ((pCompoundItem->item_count) - usParsedItems);
 351
 352         llrp_ReportMessage(context, (context, "llrp_ParseCompoundItem",
 353          "Finished parsing fixed items. Length remaining %lu, items remaining %u, stream offset %u",
 354          ulLengthRemaining, usItemsRemaining, llrp_StreamGetOffset(context)));
 355
 356         /* Parse all remaining data in the message */
 357         if(usItemsRemaining > 0)
 358         {
 359             (void)llrp_ParseVariableItems(context, &((t_llrp_item *) pCompoundItem->item_list)[usParsedItems],
 360              usItemsRemaining, ulLengthRemaining);
 361         }
 362     }
 363
 364     ulConsumedBytes = (unsigned long) (llrp_StreamGetOffset(context) - ulStartOffset);
 365
 366     llrp_ReportMessage(context, (context, "llrp_ParseCompoundItems",
 367      "Finished parsing compound item '%s'. Buffer offset %lu, consumed bytes %lu",
 368      pCompoundItem->name, llrp_StreamGetOffset(context), ulConsumedBytes));
 369
 370     /* Make sure we've consumed the exact number of expected bytes */
 371     if(ulConsumedBytes < ulTotalLength)
 372     {
 373         unsigned long ulReadLength;
 374
 375         ulLengthRemaining = (unsigned long) (ulTotalLength - ulConsumedBytes);
 376         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_OVERFLOW,
 377          pCompoundItem->type, "llrp_ParseCompoundItem", "%u leftover bytes in parameter %s",
 378          ulLengthRemaining, pCompoundItem->name));
 379         (void)llrp_StreamRead(context, ulLengthRemaining, 0, &ulReadLength);
 380         if(ulReadLength != ulLengthRemaining)
 381         {
 382             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW,
 383              pCompoundItem->type, "llrp_ParseCompoundItem",
 384              "Failed to read %u leftover bytes (%u read)", ulLengthRemaining, ulReadLength));
 385         }
 386     }
 387     else if(ulConsumedBytes > ulTotalLength)
 388     {
 389         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_OVERFLOW,
 390          pCompoundItem->type, "llrp_ParseCompoundItem",
 391          "Buffer read overflow while parsing parameter %s (total: %u, consumed %u)",
 392          pCompoundItem->name, ulTotalLength, ulConsumedBytes));
 393     }
 394
 395     (context->depth)--;
 396
 397     llrp_ReportMessage(context, (context, "llrp_ParseCompoundItem",
 398      "Finished parsing compound item '%s'", pCompoundItem->name));
 399 }
 400
 401 static int llrp_ParseFixedItems(t_llrp_parse_context *context, t_llrp_item *pItemList,
 402  unsigned short usTotalItems, unsigned short *pusParsedItems, unsigned long ulTotalLength,
 403  unsigned long *pulConsumedBytes)
 404 {
 405     t_llrp_item *pItem = pItemList;
 406     int bDone = 0, bSuccess = 1;
 407     unsigned short usItemIndex, usFieldIndex;
 408     unsigned char ucAccumulatedBits, ucBitAccumulator;
 409     unsigned long ulStartOffset, ulConsumedBytes;
 410
 411     usFieldIndex = 0;
 412     usItemIndex = 0;
 413     ucAccumulatedBits = 0;
 414     ulConsumedBytes = 0;
 415     ulStartOffset = llrp_StreamGetOffset(context);
 416     while(!bDone)
 417     {
 418         ulConsumedBytes = (unsigned long) (llrp_StreamGetOffset(context) - ulStartOffset);
 419
 420         if(usItemIndex >= usTotalItems)
 421             bDone = 1;
 422         else
 423         {
 424             llrp_ReportMessage(context, (context, "llrp_ParseFixedItems",
 425              "Parsing fixed item index %u of %u, type %u, consumed bytes %lu, offset %lu",
 426              usItemIndex, usTotalItems, pItem->item_type, ulConsumedBytes,
 427              llrp_StreamGetOffset(context)));
 428
 429             switch(pItem->item_type)
 430             {
 431                 case LLRP_ITEM_FIELD:
 432                     if(!llrp_HandleField(context, pItem, usFieldIndex,
 433                      (unsigned long) (ulTotalLength-ulConsumedBytes), &ucBitAccumulator,
 434                      &ucAccumulatedBits))
 435                     {
 436                         bSuccess = 0;
 437                         bDone = 1;
 438                     }
 439                     usFieldIndex++;
 440                     break;
 441                 case LLRP_ITEM_RESERVED:
 442                     if((pItem->min_repeat_count)%8 > ucAccumulatedBits)
 443                     {
 444                         llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW,
 445                          usFieldIndex, "llrp_ParseFixedItems",
 446                          "Only %u accumulated bits remaining for %u bit reserved field",
 447                          ucAccumulatedBits, pItem->min_repeat_count));
 448                         ucAccumulatedBits = 0;  /* Try to zero the accumulator, problems will likely follow */
 449                     }
 450                     else
 451                     {
 452                         if(pItem->min_repeat_count >= 8)
 453                         {
 454                             unsigned long ulReadBytes;
 455
 456                             /* Discard whole bytes... */
 457                             (void)llrp_StreamRead(context, (pItem->min_repeat_count)/8, 0, &ulReadBytes);
 458                             if(ulReadBytes != (unsigned long)((pItem->min_repeat_count)/8))
 459                             {
 460                                 llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW,
 461                                  usFieldIndex, "llrp_ParseFixedItems",
 462                                  "Failed to consume %u reserved bytes from message stream for reserved field of %u bits",
 463                                  (pItem->min_repeat_count)/8, pItem->min_repeat_count));
 464                             }
 465                         }
 466
 467                         ucAccumulatedBits = (unsigned char) (ucAccumulatedBits-((pItem->min_repeat_count)%8));
 468                         llrp_ReportMessage(context, (context, "llrp_ParseFixedItems",
 469                          "Consumed %u reserved bits (accumulator now %u)", pItem->min_repeat_count,
 470                          ucAccumulatedBits));
 471                     }
 472                     break;
 473                 case LLRP_ITEM_PARAMETER:
 474                 case LLRP_ITEM_CHOICE:
 475                     /* When we encounter the first variable-length item, we're done parsing
 476                        the fixed-length items. */
 477                     bDone = 1;
 478                     break;
 479                 default:
 480                     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_TYPE_UNKNOWN,
 481                      usFieldIndex, "llrp_ParseFixedItems", "Unknown fixed item field type (%u)",
 482                      pItem->item_type));
 483                     break;
 484             }
 485
 486             if(!bDone)
 487             {
 488                 usItemIndex++;
 489                 pItem++;
 490             }
 491         }
 492     }
 493
 494     if(context->field_complete_handler != NULL)
 495         context->field_complete_handler(context, usFieldIndex);
 496
 497     if(pusParsedItems != NULL)
 498         *pusParsedItems = usItemIndex;
 499     if(pulConsumedBytes != NULL)
 500         *pulConsumedBytes = ulConsumedBytes;
 501
 502     return bSuccess;
 503 }
 504
 505 static int llrp_HandleField(t_llrp_parse_context *context, t_llrp_item *pItem,
 506  unsigned short usFieldIndex, unsigned long ulTotalLength,
 507  unsigned char *pucBitAccumulator, unsigned char *pucAccumulatedBits)
 508 {
 509     unsigned long ulItemBits, ulItemBytes, ulLeftoverBits, ulReadBytes, ulLengthRemaining;
 510     unsigned char *pucData, ucUsedAccumulator;
 511
 512     if(LLRP_FIELDTYPE_IS_VARIABLE(pItem->field_type))
 513     {
 514         unsigned short *pusData;
 515
 516         if(ulTotalLength < 2)
 517         {
 518             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW, usFieldIndex,
 519              "llrp_HandleField", "Buffer underrun while reading length of variable field '%s', index %u",
 520              pItem->name, usFieldIndex));
 521             return 0; /* Failed to handle this field */
 522         }
 523
 524         pusData = (unsigned short *) llrp_StreamRead(context, 2, 0, &ulReadBytes);
 525         if(ulReadBytes != 2)
 526         {
 527             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW,
 528              usFieldIndex, "llrp_HandleField",
 529              "Failed to read the length of variable field '%s', index %u", pItem->name, usFieldIndex));
 530             return 0; /* Failed to handle this field */
 531         }
 532         ulItemBits = (unsigned long) (llrp_ntohs(*pusData)*
 533          llrp_variable_field_bitlength[LLRP_FIELDTYPE_INDEX_VARIABLE(pItem->field_type)]);
 534         ulLengthRemaining = (unsigned long) (ulTotalLength-2);
 535     }
 536     else if(pItem->field_type == LLRP_FIELDTYPE_bytesToEnd)
 537     {
 538         ulLengthRemaining = ulTotalLength;  /* Consume the entire remaining length */
 539         ulItemBits = (unsigned long) (ulLengthRemaining*8);
 540     }
 541     else
 542     {
 543         ulItemBits = llrp_fixed_field_bitlength[pItem->field_type];
 544         ulLengthRemaining = ulTotalLength;
 545     }
 546
 547     ulItemBytes = (unsigned long) ((ulItemBits/8) + ((ulItemBits%8) ? 1 : 0));
 548     ulLeftoverBits = (unsigned long) ((ulItemBytes*8) - ulItemBits);
 549
 550     /* Is there enough data stored in the accumulator? */
 551     if(ulItemBits <= *pucAccumulatedBits)
 552     {
 553         /* LLRP is bitwise big-endian; extract the topmost ulItemBits bits from the accumulator */
 554         ucUsedAccumulator = (unsigned char) ((*pucBitAccumulator) >> ((*pucAccumulatedBits)-ulItemBits));
 555         ucUsedAccumulator &= ((1 << ulItemBits)-1); /* Mask off unwanted bits */
 556         pucData = &ucUsedAccumulator;
 557         /* No need to clear the used bits from the accumulator. They're invalidated by
 558          *  an update to pucAccumulatedBits. */
 559         *pucAccumulatedBits = (unsigned char) ((*pucAccumulatedBits)-ulItemBits);
 560         ulLeftoverBits = 0;
 561     }
 562     else /* No, we must get the data from the message */
 563     {
 564         if(ulItemBytes > ulLengthRemaining)
 565         {
 566             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW,
 567              usFieldIndex, "llrp_HandleField",
 568              "Data underrun for field '%s', index %u (expected %u bytes, %u remain)",
 569              pItem->name, usFieldIndex, ulItemBytes, ulLengthRemaining));
 570             return 0; /* Failed to handle this field */
 571         }
 572         pucData= llrp_StreamRead(context, ulItemBytes, 0, &ulReadBytes);
 573         if(ulReadBytes!= ulItemBytes)
 574         {
 575             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_UNDERFLOW,
 576              usFieldIndex, "llrp_HandleField",
 577              "Failed to read %u data bytes of field '%s', index %u (%u read)",
 578              ulItemBytes, pItem->name, usFieldIndex, ulReadBytes));
 579             return 0; /* Failed to handle this field */
 580         }
 581     }
 582
 583     if(ulLeftoverBits > 0)
 584     {
 585         /* If there is any data that has been read from the message, but doesn't belong to */
 586         /*  this field, add it to the accumulator. */
 587         if((ulLeftoverBits+*pucAccumulatedBits) >= 8)
 588         {
 589             llrp_ReportError(context, (context, LLRP_PARSE_ERROR_FIELD_DATA_OVERFLOW,
 590              usFieldIndex, "llrp_HandleField",
 591              "Leftover bit accumulator overflow (accumulator bits: %u, new leftover bits: %u)",
 592              *pucAccumulatedBits, ulLeftoverBits));
 593         }
 594         else
 595         {
 596             /* Bits are always added to the accumulator from the right (least significant bit) side */
 597             *pucBitAccumulator<<= ulLeftoverBits;
 598             *pucBitAccumulator|= pucData[ulItemBytes-1] & ((1 << ulLeftoverBits)-1);
 599             *pucAccumulatedBits= (unsigned char) ((*pucAccumulatedBits)+ulLeftoverBits);
 600         }
 601
 602         /* Always mask-off the bits that don't belong to this field */
 603         if(ulItemBits <= 8)
 604         {
 605             ucUsedAccumulator= (unsigned char) (((*pucData) >> (8-ulItemBits)) & ((1 << ulItemBits)-1));
 606             pucData= &ucUsedAccumulator;
 607         }
 608     }
 609
 610     llrp_ReportMessage(context, (context, "llrp_HandleField",
 611      "Field '%s' parsed: fieldtype %u, bitlength %u, accumulator %u",
 612      pItem->name, pItem->field_type, ulItemBits, *pucAccumulatedBits));
 613     if(context->field_handler!= NULL)
 614     {
 615         context->field_handler(context, usFieldIndex, pItem->field_type, pItem->name,
 616          ulItemBits, pucData, pItem->data);
 617     }
 618
 619     return 1; /* Successfully handled */
 620 }
 621
 622 /* Function: llrp_ParseVariableItems */
 623 /* Description: Parse all remaining data in the current compound item (message or parameter).
 624  *  Parsing is completed only when all data has been exhausted from the current compound item.
 625  *  It is assumed that no fixed-length items remain in the current compound item, they have
 626  *  already been parsed. */
 627 /* Returns: nonzero upon successful parsing of all variable items in the compound item,
 628  *  zero upon failure. */
 629 #define LLRP_VARIABLE_ITEM_MIN_HEADER_LENGTH  1 /* TV parameters require only one byte */
 630 static int llrp_ParseVariableItems(t_llrp_parse_context *context, t_llrp_item *pItemList,
 631  unsigned short usTotalItems, unsigned long ulTotalLength)
 632 {
 633     t_llrp_item *pItem = pItemList;
 634     int bDone = 0, bImpliedLength;
 635     unsigned long ulReadBytes, ulConsumedLength, ulStartOffset;
 636     unsigned char *pucReadByte, ucHeaderLength;
 637     unsigned short *pusLength, usType, usLength;
 638
 639     pItem=pItem; usTotalItems=usTotalItems;
 640
 641     ulStartOffset = llrp_StreamGetOffset(context);
 642     while(!bDone)
 643     {
 644         ulConsumedLength = (unsigned long) (llrp_StreamGetOffset(context) - ulStartOffset);
 645
 646         /* Make sure we can at least read the minimum header length */
 647         if((ulConsumedLength+LLRP_VARIABLE_ITEM_MIN_HEADER_LENGTH) > ulTotalLength)
 648             bDone = 1;
 649         else
 650         {
 651             /* Determine the type and length */
 652             ucHeaderLength = LLRP_VARIABLE_ITEM_MIN_HEADER_LENGTH;
 653             pucReadByte = (unsigned char *) llrp_StreamRead(context, 1, 0, &ulReadBytes);
 654             if(ulReadBytes != 1)
 655             {
 656                 llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW,
 657                  0, "llrp_ParseVariableItems", "Failed to read first type byte of parameter"));
 658                 return 0; /* Failed to parse */
 659             }
 660             usType = *pucReadByte;
 661             if(usType & 0x80) /* Is this a TV encoded parameter? */
 662             {
 663                 usType &= ~0x80;
 664                 usLength = 0;
 665                 bImpliedLength = 1; /* Length is implied - make llrp_ParseParameter() derive it */
 666             }
 667             else
 668             {
 669                 ucHeaderLength = 4; /* TLV parameter headers have 2 type bytes, 2 length bytes */
 670                 bImpliedLength = 0;
 671
 672                 /* Get the second byte of the TLV parameter's type */
 673                 pucReadByte = (unsigned char *) llrp_StreamRead(context, 1, 0, &ulReadBytes);
 674                 if(ulReadBytes != 1)
 675                 {
 676                     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW,
 677                      0, "llrp_ParseVariableItems", "Failed to read second type byte of TLV parameter"));
 678                     return 0; /* Failed to parse the variable item */
 679                 }
 680                 usType <<= 8;
 681                 usType = (unsigned short) (usType+(*pucReadByte));
 682
 683                 /* TLV parameters have 2 bytes for the length */
 684                 pusLength = (unsigned short *) llrp_StreamRead(context, 2, 0, &ulReadBytes);
 685                 if(ulReadBytes != 2)
 686                 {
 687                     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW,
 688                      usType, "llrp_ParseVariableItems", "Failed to read length bytes of TLV parameter"));
 689                     return 0; /* Failed to parse the variable item */
 690                 }
 691                 usLength = llrp_ntohs(*pusLength);
 692
 693                 /* A rather pedantic test... */
 694                 if(usLength < ucHeaderLength)
 695                 {
 696                     llrp_ReportError(context, (context, LLRP_PARSE_ERROR_PARAMETER_DATA_UNDERFLOW,
 697                      usType, "llrp_ParseVariableItems",
 698                      "Length underflow for variable item type %u (requires %u bytes, %u in header)",
 699                      usType, usLength, ucHeaderLength));
 700                     return 0; /* Failed to parse the variable item */
 701                 }
 702                 usLength = (unsigned short) (usLength-ucHeaderLength);
 703             }
 704
 705             llrp_ReportMessage(context, (context, "llrp_ParseVariableItems",
 706              "Parsing variable item: type %u, header length %u, data length %u",
 707              usType, ucHeaderLength, usLength));
 708
 709             /* TODO: Validate the parameter found against pItem */
 710
 711             if(usType == 1023) /* v1.0 Custom parameter type */
 712                 llrp_ParseCustomParameter(context, usType, usLength);
 713             else
 714                 llrp_ParseParameter(context, usType, bImpliedLength, usLength);
 715         }
 716     }
 717
 718     if(context->all_parameters_complete_handler!= NULL)
 719         context->all_parameters_complete_handler(context);
 720
 721     return 1; /* Variable item parsed successfully */
 722 }
 723
 724 /* Determine the length of the contents of the specified compound item. This will only succeed
 725  *  when the compound item contains nothing more than fixed-length fields. */
 726 static int llrp_ParseGetParameterContentLength(const t_llrp_compound_item *pItem,
 727  unsigned short *pusLength)
 728 {
 729     unsigned short usIndex, usLength;
 730     t_llrp_item *pField;
 731
 732     usLength= 0;
 733     for(usIndex= 0; usIndex< pItem->item_count; usIndex++)
 734     {
 735         pField= &((t_llrp_item *) (pItem->item_list))[usIndex];
 736         switch(pField->item_type)
 737         {
 738             case LLRP_ITEM_FIELD:
 739                 if(LLRP_FIELDTYPE_IS_VARIABLE(pField->field_type))
 740                     return 0; /* Not a constant content length, can't calculate content length */
 741                 usLength= (unsigned short) (usLength+((llrp_fixed_field_bitlength[pField->field_type])/8));
 742                 break;
 743             default:
 744                 return 0; /* Contains items that are not fields, can't calculate content length */
 745         }
 746     }
 747
 748     if(pusLength!= NULL)
 749         *pusLength= usLength;
 750     return 1; /* Length calculated successfully */
 751 }