Demos/Host/Incomplete/BluetoothHost/Lib/ServiceDiscoveryProtocol.c

   1 /*
   2              LUFA Library
   3      Copyright (C) Dean Camera, 2010.
   4
   5   dean [at] fourwalledcubicle [dot] com
   6       www.fourwalledcubicle.com
   7 */
   8
   9 /*
  10   Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)
  11
  12   Permission to use, copy, modify, distribute, and sell this
  13   software and its documentation for any purpose is hereby granted
  14   without fee, provided that the above copyright notice appear in
  15   all copies and that both that the copyright notice and this
  16   permission notice and warranty disclaimer appear in supporting
  17   documentation, and that the name of the author not be used in
  18   advertising or publicity pertaining to distribution of the
  19   software without specific, written prior permission.
  20
  21   The author disclaim all warranties with regard to this
  22   software, including all implied warranties of merchantability
  23   and fitness.  In no event shall the author be liable for any
  24   special, indirect or consequential damages or any damages
  25   whatsoever resulting from loss of use, data or profits, whether
  26   in an action of contract, negligence or other tortious action,
  27   arising out of or in connection with the use or performance of
  28   this software.
  29 */
  30
  31 #define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
  32 #include "ServiceDiscoveryProtocol.h"
  33
  34 const struct
  35 {
  36         uint8_t  Header;
  37         uint32_t Data;
  38 } PROGMEM ServiceDiscoveryServer_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), 0x00000000};
  39
  40 const struct
  41 {
  42         uint8_t     Header;
  43         uint16_t    Size;
  44         ClassUUID_t UUIDList[];
  45 } PROGMEM ServiceDiscoveryServer_Attribute_ServiceClassIDs =
  46         {
  47                 (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit), (sizeof(ClassUUID_t) * 1),
  48                 {
  49                         {.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00}}
  50                 }
  51         };
  52
  53 const struct
  54 {
  55         uint8_t  Header;
  56         uint16_t Data;
  57 } PROGMEM ServiceDiscoveryServer_Attribute_Version = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), 0x0100};
  58
  59 /** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
  60 const ServiceAttributeTable_t ServiceDiscoveryServer_Attribute_Table[] PROGMEM =
  61         {
  62                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &ServiceDiscoveryServer_Attribute_ServiceHandle   },
  63                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &ServiceDiscoveryServer_Attribute_ServiceClassIDs },
  64                 {.AttributeID = SDP_ATTRIBUTE_ID_VERSION,             .Data = &ServiceDiscoveryServer_Attribute_Version         },
  65
  66                 SERVICE_ATTRIBUTE_TABLE_TERMINATOR
  67         };
  68
  69 /** Service Discovery Protocol attribute, indicationg the service's name. */
  70 const struct
  71 {
  72         uint8_t Header;
  73         uint8_t Length;
  74         uint8_t Data[];
  75 } PROGMEM SDP_Attribute_Name = {(SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), sizeof("SDP"), "SDP"};
  76
  77 /** Service Discovery Protocol attribute, indicationg the service's description. */
  78 const struct
  79 {
  80         uint8_t Header;
  81         uint8_t Length;
  82         uint8_t Data[];
  83 } PROGMEM SDP_Attribute_Description = {(SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit), sizeof("BT Service Discovery"), "BT Service Discovery"};
  84
  85 /** Service Discovery Protocol attribute, indicationg the service's availability. */
  86 const struct
  87 {
  88         uint8_t Header;
  89         uint8_t Data;
  90 } PROGMEM SDP_Attribute_Availability = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_8Bit), 0xFF};
  91
  92 const struct
  93 {
  94         uint8_t  Header;
  95         uint16_t Data;
  96 } PROGMEM SDP_Attribute_LanguageOffset = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), SDP_ATTRIBUTE_LANGOFFSET};
  97
  98 const struct
  99 {
 100         uint8_t  Header;
 101         uint32_t Data;
 102 } PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), 0x00000001};
 103
 104 const struct
 105 {
 106         uint8_t     Header;
 107         uint16_t    Size;
 108         ClassUUID_t UUIDList[];
 109 } PROGMEM SDP_Attribute_ServiceClassIDs =
 110         {
 111                 (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit), (sizeof(ClassUUID_t) * 1),
 112                 {
 113                         {.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x01}}
 114                 }
 115         };
 116
 117 /** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
 118 const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
 119         {
 120                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &SDP_Attribute_ServiceHandle    },
 121                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &SDP_Attribute_ServiceClassIDs  },
 122                 {.AttributeID = SDP_ATTRIBUTE_ID_LANGIDOFFSET,        .Data = &SDP_Attribute_LanguageOffset   },
 123                 {.AttributeID = SDP_ATTRIBUTE_ID_NAME,                .Data = &SDP_Attribute_Name             },
 124                 {.AttributeID = SDP_ATTRIBUTE_ID_DESCRIPTION,         .Data = &SDP_Attribute_Description      },
 125
 126                 SERVICE_ATTRIBUTE_TABLE_TERMINATOR
 127         };
 128
 129 /** Master service table, listing all supported services (and their attribute tables) of the device, including
 130  *  each service's UUID.
 131  */
 132 const ServiceTable_t SDP_Services_Table[] PROGMEM =
 133         {
 134                 {   // 128-bit UUID for the Service Discovery Server Service
 135                         .UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
 136                         .AttributeTable = ServiceDiscoveryServer_Attribute_Table,
 137                 },
 138                 {   // 128-bit UUID for the SDP service
 139                         .UUID  = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x01},
 140                         .AttributeTable = SDP_Attribute_Table,
 141                 },
 142 #if 0
 143                 {   // 128-bit UUID for the RFCOMM service
 144                         .UUID  = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00},
 145                         .AttributeTable = RFCOMM_Attribute_Table,
 146                 },
 147 #endif
 148         };
 149
 150 /** Base UUID value common to all standardized Bluetooth services */
 151 const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
 152
 153
 154 /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
 155  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
 156  *  services the local device exposes.
 157  *
 158  *  \param[in]  Data     Incomming packet data containing the SDP request
 159  *  \param[in]  Channel  Channel the request was issued to by the remote device
 160  */
 161 void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
 162 {
 163         SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
 164         SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);
 165
 166         BT_SDP_DEBUG(1, "SDP Packet Received");
 167         BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
 168         BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);
 169
 170         switch (SDPHeader->PDU)
 171         {
 172                 case SDP_PDU_SERVICESEARCHREQUEST:
 173                         SDP_ProcessServiceSearch(SDPHeader, Channel);
 174                         break;
 175                 case SDP_PDU_SERVICEATTRIBUTEREQUEST:
 176                         SDP_ProcessServiceAttribute(SDPHeader, Channel);
 177                         break;
 178                 case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
 179                         SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
 180                         break;
 181         }
 182 }
 183
 184 /** Internal processing routine for SDP Service Search Requests.
 185  *
 186  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 187  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 188  */
 189 static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 190 {
 191         BT_SDP_DEBUG(1, "<< Service Search");
 192 }
 193
 194 /** Internal processing routine for SDP Service Attribute Requests.
 195  *
 196  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 197  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 198  */
 199 static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 200 {
 201         BT_SDP_DEBUG(1, "<< Service Attribute");
 202 }
 203
 204 /** Internal processing routine for SDP Service Search Attribute Requests.
 205  *
 206  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 207  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 208  */
 209 static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 210 {
 211         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 212
 213         BT_SDP_DEBUG(1, "<< Service Search Attribute");
 214
 215         /* Retrieve the list of search UUIDs from the request */
 216         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 217         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
 218         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 219
 220         /* Retrieve the maximum Attribute reponse size from the request */
 221         uint16_t MaxAttributeSize = *((uint16_t*)CurrentParameter);
 222         CurrentParameter += sizeof(uint16_t);
 223         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 224
 225         /* Retrieve the list of Attributes from the request */
 226         uint16_t AttributeList[15][2];
 227         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 228         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 229
 230         struct
 231         {
 232                 SDP_PDUHeader_t SDPHeader;
 233                 uint16_t        AttributeListByteCount;
 234                 uint8_t         ResponseData[100];
 235         } ResponsePacket;
 236
 237         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 238         void* CurrResponsePos = ResponsePacket.ResponseData;
 239
 240         /* Clamp the maximum attribute size to the size of the allocated buffer */
 241         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 242           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 243
 244         /* Add the outer Data Element Sequence header for the retrieved Attributes */
 245         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 246
 247         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 248         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 249         {
 250                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 251                 ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
 252
 253                 /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
 254                 if (AttributeTable == NULL)
 255                   continue;
 256
 257                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 258
 259                 /* Add an inner Data Element Sequence header for the current UUID's found Attributes */
 260                 uint16_t* CurrentUUIDResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 261
 262                 /* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
 263                 for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
 264                 {
 265                         uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
 266
 267                         /* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
 268                         for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
 269                         {
 270                                 /* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
 271                                 const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
 272
 273                                 /* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
 274                                 if (AttributeValue == NULL)
 275                                   continue;
 276
 277                                 BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
 278
 279                                 /* Increment the current UUID's returned Attribute container size by the number of added bytes */
 280                                 *CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
 281                         }
 282
 283                         /* Increment the outer container size by the number of added bytes */
 284                         *TotalResponseSize += 3 + *CurrentUUIDResponseSize;
 285                 }
 286         }
 287
 288         /* Set the total response list size to the size of the outer container plus its header size */
 289         ResponsePacket.AttributeListByteCount    = 3 + *TotalResponseSize;
 290
 291         /* Fill in the response packet's header */
 292         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 293         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 294         ResponsePacket.SDPHeader.ParameterLength = (ResponsePacket.AttributeListByteCount + sizeof(ResponsePacket.AttributeListByteCount));
 295
 296         BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
 297         BT_SDP_DEBUG(2, "-- Total Parameter Length: 0x%04X", ResponsePacket.SDPHeader.ParameterLength);
 298
 299         /* Send the completed response packet to the sender */
 300         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
 301                              Channel);
 302 }
 303
 304 /** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
 305  *
 306  *  \param[in] AttributeID          Attribute ID to add to the response buffer
 307  *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM
 308  *  \param[in, out] ResponseBuffer  Pointer to a buffer where the Attribute and Attribute Value is to be added
 309  *
 310  *  \return Number of bytes added to the response buffer
 311  */
 312 static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
 313 {
 314         /* Retrieve the size of the attribute value from its container header */
 315         uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue);
 316
 317         /* Add a Data Element header to the response for the Attribute ID */
 318         *((uint8_t*)*ResponseBuffer) = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit);
 319         *ResponseBuffer += sizeof(uint8_t);
 320
 321         /* Add the Attribute ID to the created Data Element */
 322         *((uint16_t*)*ResponseBuffer) = AttributeID;
 323         *ResponseBuffer += sizeof(uint16_t);
 324
 325         /* Copy over the Attribute value Data Element container to the response */
 326         memcpy_P(*ResponseBuffer, AttributeValue, AttributeValueLength);
 327         *ResponseBuffer += AttributeValueLength;
 328
 329         return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeValueLength);
 330 }
 331
 332 /** Retrieves a pointer to the value of the given Attribute ID from the given Attribute table.
 333  *
 334  *  \param[in] AttributeTable  Pointer to the Attribute table to search in
 335  *  \param[in] AttributeID     Attribute ID to search for within the table
 336  *
 337  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
 338  */
 339 static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
 340 {
 341         void* CurrTableItemData;
 342
 343         /* Search through the current Attribute table, abort when the terminator item has been reached */
 344         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 345         {
 346                 /* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
 347                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 348                   return CurrTableItemData;
 349
 350                 AttributeTable++;
 351         }
 352
 353         return NULL;
 354 }
 355
 356 /** Retrieves the Attribute table for the given UUID if it exists.
 357  *
 358  *  \param[in] UUID  UUID to search for
 359  *
 360  *  \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise
 361  */
 362 static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 363 {
 364         /* Search through the global UUID list an item at a time */
 365         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 366         {
 367                 /* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
 368                 if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 369                   return (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 370         }
 371
 372         return NULL;
 373 }
 374
 375 /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given
 376  *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 377  *
 378  *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 379  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 380  *
 381  *  \return Total number of Attribute ranges stored in the Data Element Sequence
 382  */
 383 static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
 384 {
 385         uint8_t ElementHeaderSize;
 386         uint8_t TotalAttributes = 0;
 387
 388         /* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
 389         uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 390         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 391         while (AttributeIDListLength)
 392         {
 393                 /* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
 394                 uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
 395                 uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 396
 397                 /* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
 398                 memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
 399
 400                 /* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
 401                 if (AttributeLength == 2)
 402                   memcpy(&CurrentAttributeRange[1], *CurrentParameter, 2);
 403
 404                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
 405
 406                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 407                 *CurrentParameter     += AttributeLength;
 408         }
 409
 410         return TotalAttributes;
 411 }
 412
 413 /** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given
 414  *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 415  *
 416  *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 417  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 418  *
 419  *  \return Total number of UUIDs stored in the Data Element Sequence
 420  */
 421 static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
 422 {
 423         uint8_t ElementHeaderSize;
 424         uint8_t TotalUUIDs = 0;
 425
 426         /* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
 427         uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 428         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 429         while (ServicePatternLength)
 430         {
 431                 /* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
 432                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 433                 uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 434
 435                 /* Copy over the base UUID value to the free UUID slot in the list */
 436                 memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 437
 438                 /* Copy over UUID from the container to the free slot - if a short UUID (<= 4 bytes) it replaces the lower
 439                    4 bytes of the base UUID, otherwise it replaces the UUID completely */
 440                 memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
 441
 442                 BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 443                                 UUIDLength,
 444                                 CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
 445                                 CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
 446                                 CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
 447                                 CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
 448
 449                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 450                 *CurrentParameter    += UUIDLength;
 451         }
 452
 453         return TotalUUIDs;
 454 }
 455
 456 /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 457  *
 458  *  \param[in] AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 459  *
 460  *  \return Size in bytes of the entire attribute container, including the header
 461  */
 462 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData)
 463 {
 464         /* Fetch the size of the Data Element structure from the header */
 465         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 466
 467         /* Convert the Data Element size index into a size in bytes */
 468         switch (SizeIndex)
 469         {
 470                 case SDP_DATASIZE_Variable8Bit:
 471                         return (1 + sizeof(uint8_t))  + pgm_read_byte(AttributeData + 1);
 472                 case SDP_DATASIZE_Variable16Bit:
 473                         return (1 + sizeof(uint16_t)) + pgm_read_word(AttributeData + 1);
 474                 case SDP_DATASIZE_Variable32Bit:
 475                         return (1 + sizeof(uint32_t)) + pgm_read_dword(AttributeData + 1);
 476                 default:
 477                         return (1 + (1 << SizeIndex));
 478         }
 479
 480         return 0;
 481 }
 482
 483 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 484  *  pointer to the start of the Data Element's contents.
 485  *
 486  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 487  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 488  *
 489  *  \return Size in bytes of the Data Element container's contents, minus the header
 490  */
 491 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 492 {
 493         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 494         uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
 495         *DataElementHeader += sizeof(uint8_t);
 496
 497         uint32_t ElementValue;
 498
 499         /* Convert the Data Element size index into a size in bytes */
 500         switch (SizeIndex)
 501         {
 502                 case SDP_DATASIZE_Variable8Bit:
 503                         ElementValue = *((uint8_t*)*DataElementHeader);
 504                         *DataElementHeader += sizeof(uint8_t);
 505                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 506                         break;
 507                 case SDP_DATASIZE_Variable16Bit:
 508                         ElementValue = *((uint16_t*)*DataElementHeader);
 509                         *DataElementHeader += sizeof(uint16_t);
 510                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 511                         break;
 512                 case SDP_DATASIZE_Variable32Bit:
 513                         ElementValue = *((uint32_t*)*DataElementHeader);
 514                         *DataElementHeader += sizeof(uint32_t);
 515                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 516                         break;
 517                 default:
 518                         ElementValue = (1 << SizeIndex);
 519                         *ElementHeaderSize = 1;
 520                         break;
 521         }
 522
 523         return ElementValue;
 524 }