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 /** Service Discovery Protocol attribute, indicating the service's availability. */
  35 const struct
  36 {
  37         uint8_t Header;
  38         uint8_t Data;
  39 } PROGMEM SDP_Attribute_Availability = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_8Bit), 0xFF};
  40
  41 const struct
  42 {
  43         uint8_t  Header;
  44         uint32_t Data;
  45 } PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), 0x00010000};
  46
  47 const struct
  48 {
  49         uint8_t   Header;
  50         uint8_t   Size;
  51         Version_t VersionList[];
  52 } PROGMEM SDP_Attribute_Version =
  53         {
  54                 .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit),
  55                 .Size   = (sizeof(Version_t) * 1),
  56                 .VersionList =
  57                         {
  58                                 {.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Version = 0x0100}
  59                         }
  60         };
  61
  62 const struct
  63 {
  64         uint8_t     Header;
  65         uint16_t    Size;
  66         ClassUUID_t UUIDList[];
  67 } PROGMEM SDP_Attribute_ServiceClassIDs =
  68         {
  69                 .Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),
  70                 .Size   = (sizeof(ClassUUID_t) * 1),
  71                 .UUIDList =
  72                         {
  73                                 {.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00}}
  74                         }
  75         };
  76
  77 /** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
  78 const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
  79         {
  80                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &SDP_Attribute_ServiceHandle    },
  81                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &SDP_Attribute_ServiceClassIDs  },
  82                 {.AttributeID = SDP_ATTRIBUTE_ID_VERSION,             .Data = &SDP_Attribute_Version          },
  83
  84                 SERVICE_ATTRIBUTE_TABLE_TERMINATOR
  85         };
  86
  87 /** Master service table, listing all supported services (and their attribute tables) of the device, including
  88  *  each service's UUID.
  89  */
  90 const ServiceTable_t SDP_Services_Table[] PROGMEM =
  91         {
  92                 {   // 128-bit UUID for the SDP service
  93                         .UUID  = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x01},
  94                         .AttributeTable = SDP_Attribute_Table,
  95                 },
  96         };
  97
  98 /** Base UUID value common to all standardized Bluetooth services */
  99 const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
 100
 101
 102 /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
 103  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
 104  *  services the local device exposes.
 105  *
 106  *  \param[in]  Data     Incomming packet data containing the SDP request
 107  *  \param[in]  Channel  Channel the request was issued to by the remote device
 108  */
 109 void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
 110 {
 111         SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
 112         SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);
 113
 114         BT_SDP_DEBUG(1, "SDP Packet Received");
 115         BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
 116         BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);
 117
 118         switch (SDPHeader->PDU)
 119         {
 120                 case SDP_PDU_SERVICESEARCHREQUEST:
 121                         SDP_ProcessServiceSearch(SDPHeader, Channel);
 122                         break;
 123                 case SDP_PDU_SERVICEATTRIBUTEREQUEST:
 124                         SDP_ProcessServiceAttribute(SDPHeader, Channel);
 125                         break;
 126                 case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
 127                         SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
 128                         break;
 129         }
 130 }
 131
 132 /** Internal processing routine for SDP Service Search Requests.
 133  *
 134  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 135  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 136  */
 137 static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 138 {
 139         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 140
 141         BT_SDP_DEBUG(1, "<< Service Search");
 142
 143         /* Retrieve the list of search UUIDs from the request */
 144         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 145         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
 146         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 147
 148         /* Retrieve the maximum service record reponse count from the request */
 149         uint16_t MaxServiceRecordCount = SwapEndian_16(*((uint16_t*)CurrentParameter));
 150         CurrentParameter += sizeof(uint16_t);
 151         BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
 152
 153         struct
 154         {
 155                 SDP_PDUHeader_t SDPHeader;
 156                 uint16_t        TotalServiceRecordCount;
 157                 uint16_t        CurrentServiceRecordCount;
 158                 uint8_t         ResponseData[100];
 159         } ResponsePacket;
 160
 161         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 162         void* CurrResponsePos = ResponsePacket.ResponseData;
 163
 164         uint8_t AddedServiceHandles = 0;
 165
 166         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 167         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 168         {
 169                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 170                 ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
 171
 172                 /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
 173                 if (AttributeTable == NULL)
 174                   continue;
 175
 176                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 177
 178                 /* Retrieve a PROGMEM pointer to the value of the service's record handle */
 179                 const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 180
 181                 /* Copy over the service record handle to the response list */
 182                 uint8_t AttrHeaderSize;
 183                 SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
 184                 memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, sizeof(uint32_t));
 185                 CurrResponsePos += AttrHeaderSize + sizeof(uint32_t);
 186
 187                 /* Increment the total number of service records added to the list */
 188                 AddedServiceHandles++;
 189         }
 190
 191         /* Continuation state - always zero */
 192         *((uint8_t*)CurrResponsePos) = 0;
 193
 194         /* Fill out the service record count values in the returned packet */
 195         ResponsePacket.TotalServiceRecordCount   = SwapEndian_16(AddedServiceHandles);
 196         ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;
 197
 198         /* Fill in the response packet's header */
 199         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
 200         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 201         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16((ResponsePacket.CurrentServiceRecordCount << 2) +
 202                                                                  sizeof(ResponsePacket.CurrentServiceRecordCount) +
 203                                                                  sizeof(ResponsePacket.TotalServiceRecordCount) +
 204                                                                  sizeof(uint8_t));
 205
 206         BT_SDP_DEBUG(1, ">> Service Search Response");
 207
 208         /* Send the completed response packet to the sender */
 209         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
 210                              Channel);
 211 }
 212
 213 /** Internal processing routine for SDP Service Attribute Requests.
 214  *
 215  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 216  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 217  */
 218 static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 219 {
 220         BT_SDP_DEBUG(1, "<< Service Attribute");
 221 }
 222
 223 /** Internal processing routine for SDP Service Search Attribute Requests.
 224  *
 225  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 226  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 227  */
 228 static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 229 {
 230         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 231
 232         BT_SDP_DEBUG(1, "<< Service Search Attribute");
 233
 234         /* Retrieve the list of search UUIDs from the request */
 235         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 236         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
 237         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 238
 239         /* Retrieve the maximum Attribute reponse size from the request */
 240         uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
 241         CurrentParameter += sizeof(uint16_t);
 242         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 243
 244         /* Retrieve the list of Attributes from the request */
 245         uint16_t AttributeList[15][2];
 246         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 247         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 248
 249         struct
 250         {
 251                 SDP_PDUHeader_t SDPHeader;
 252                 uint16_t        AttributeListByteCount;
 253                 uint8_t         ResponseData[100];
 254         } ResponsePacket;
 255
 256         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 257         void* CurrResponsePos = ResponsePacket.ResponseData;
 258
 259         /* Clamp the maximum attribute size to the size of the allocated buffer */
 260         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 261           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 262
 263         /* Add the outer Data Element Sequence header for all of the retrieved Attributes */
 264         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 265
 266         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 267         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 268         {
 269                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 270                 ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
 271
 272                 /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
 273                 if (AttributeTable == NULL)
 274                   continue;
 275
 276                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 277
 278                 /* Add an inner Data Element Sequence header for the current UUID's found Attributes */
 279                 uint16_t* CurrentUUIDResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 280
 281                 /* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
 282                 for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
 283                 {
 284                         uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
 285
 286                         /* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
 287                         for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
 288                         {
 289                                 /* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
 290                                 const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
 291
 292                                 /* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
 293                                 if (AttributeValue == NULL)
 294                                   continue;
 295
 296                                 BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
 297
 298                                 /* Increment the current UUID's returned Attribute container size by the number of added bytes */
 299                                 *CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
 300                         }
 301
 302                         /* Increment the outer container size by the number of added bytes */
 303                         *TotalResponseSize += 3 + *CurrentUUIDResponseSize;
 304                 }
 305
 306                 /* Flip the endianness of the container's size */
 307                 *CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize);
 308         }
 309
 310         /* Continuation state - always zero */
 311         *((uint8_t*)CurrResponsePos) = 0;
 312
 313         /* Set the total response list size to the size of the outer container plus its header size and continuation state */
 314         ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
 315
 316         /* Fill in the response packet's header */
 317         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 318         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 319         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(sizeof(ResponsePacket.AttributeListByteCount) +
 320                                                                  (3 + *TotalResponseSize) +
 321                                                                  sizeof(uint8_t));
 322
 323         /* Flip the endianness of the container's size */
 324         *TotalResponseSize = SwapEndian_16(*TotalResponseSize);
 325
 326         BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
 327
 328         /* Send the completed response packet to the sender */
 329         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
 330                              Channel);
 331 }
 332
 333 /** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
 334  *
 335  *  \param[in] AttributeID          Attribute ID to add to the response buffer
 336  *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM
 337  *  \param[in, out] ResponseBuffer  Pointer to a buffer where the Attribute and Attribute Value is to be added
 338  *
 339  *  \return Number of bytes added to the response buffer
 340  */
 341 static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
 342 {
 343         /* Retrieve the size of the attribute value from its container header */
 344         uint8_t  AttributeHeaderLength;
 345         uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
 346
 347         /* Add a Data Element header to the response for the Attribute ID */
 348         *((uint8_t*)*ResponseBuffer) = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit);
 349         *ResponseBuffer += sizeof(uint8_t);
 350
 351         /* Add the Attribute ID to the created Data Element */
 352         *((uint16_t*)*ResponseBuffer) = SwapEndian_16(AttributeID);
 353         *ResponseBuffer += sizeof(uint16_t);
 354
 355         /* Copy over the Attribute value Data Element container to the response */
 356         memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
 357         SwapEndian_n(*ResponseBuffer, AttributeHeaderLength);
 358         *ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
 359
 360         return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
 361 }
 362
 363 /** Retrieves a pointer to the value of the given Attribute ID from the given Attribute table.
 364  *
 365  *  \param[in] AttributeTable  Pointer to the Attribute table to search in
 366  *  \param[in] AttributeID     Attribute ID to search for within the table
 367  *
 368  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
 369  */
 370 static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
 371 {
 372         void* CurrTableItemData;
 373
 374         /* Search through the current Attribute table, abort when the terminator item has been reached */
 375         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 376         {
 377                 /* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
 378                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 379                   return CurrTableItemData;
 380
 381                 AttributeTable++;
 382         }
 383
 384         return NULL;
 385 }
 386
 387 /** Retrieves the Attribute table for the given UUID if it exists.
 388  *
 389  *  \param[in] UUID  UUID to search for
 390  *
 391  *  \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise
 392  */
 393 static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 394 {
 395         /* Search through the global UUID list an item at a time */
 396         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 397         {
 398                 /* Read in a pointer to the current UUID table entry's Attribute table */
 399                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 400
 401                 /* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
 402                 if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 403                   return CurrAttributeTable;
 404
 405                 /* Retrieve the list of the service's Class UUIDs from its Attribute table */
 406                 void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS);
 407
 408                 /* Go to the next UUID in the table if the current item does not have a list of Class UUIDs */
 409                 if (ClassUUIDs == NULL)
 410                   continue;
 411
 412                 /* Retrieve the size of the Class UUID list and skip past the header to the first Class UUID in the list */
 413                 uint8_t  ClassUUIDListHeaderSize;
 414                 uint32_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize);
 415                 ClassUUIDs += ClassUUIDListHeaderSize;
 416
 417                 /* Check each class UUID in turn for a match */
 418                 while (ClassUUIDListSize)
 419                 {
 420                         /* Current Service UUID's Class UUID list has a matching entry, return the Attribute table */
 421                         if (!(memcmp_P(UUID, &((ClassUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
 422                           return CurrAttributeTable;
 423
 424                         ClassUUIDListSize -= sizeof(ClassUUID_t);
 425                         ClassUUIDs        += sizeof(ClassUUID_t);
 426                 }
 427         }
 428
 429         return NULL;
 430 }
 431
 432 /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given
 433  *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 434  *
 435  *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 436  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 437  *
 438  *  \return Total number of Attribute ranges stored in the Data Element Sequence
 439  */
 440 static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
 441 {
 442         uint8_t ElementHeaderSize;
 443         uint8_t TotalAttributes = 0;
 444
 445         /* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
 446         uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 447         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 448         while (AttributeIDListLength)
 449         {
 450                 /* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
 451                 uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
 452                 uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 453
 454                 /* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
 455                 memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
 456
 457                 /* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
 458                 if (AttributeLength == 2)
 459                   CurrentAttributeRange[1] = CurrentAttributeRange[0];
 460
 461                 /* Swap the endianness of the attribute range values */
 462                 CurrentAttributeRange[0] = SwapEndian_16(CurrentAttributeRange[0]);
 463                 CurrentAttributeRange[1] = SwapEndian_16(CurrentAttributeRange[1]);
 464
 465                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
 466
 467                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 468                 *CurrentParameter     += AttributeLength;
 469         }
 470
 471         return TotalAttributes;
 472 }
 473
 474 /** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given
 475  *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 476  *
 477  *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 478  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 479  *
 480  *  \return Total number of UUIDs stored in the Data Element Sequence
 481  */
 482 static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
 483 {
 484         uint8_t ElementHeaderSize;
 485         uint8_t TotalUUIDs = 0;
 486
 487         /* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
 488         uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 489         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 490         while (ServicePatternLength)
 491         {
 492                 /* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
 493                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 494                 uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 495
 496                 /* Copy over the base UUID value to the free UUID slot in the list */
 497                 memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 498
 499                 /* Copy over UUID from the container to the free slot - if a short UUID (<= 4 bytes) it replaces the lower
 500                    4 bytes of the base UUID, otherwise it replaces the UUID completely */
 501                 memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
 502
 503                 BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 504                                 UUIDLength,
 505                                 CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
 506                                 CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
 507                                 CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
 508                                 CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
 509
 510                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 511                 *CurrentParameter    += UUIDLength;
 512         }
 513
 514         return TotalUUIDs;
 515 }
 516
 517 /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 518  *
 519  *  \param[in] AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 520  *
 521  *  \return Size in bytes of the entire attribute container, including the header
 522  */
 523 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
 524 {
 525         /* Fetch the size of the Data Element structure from the header */
 526         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 527
 528         /* Convert the Data Element size index into a size in bytes */
 529         switch (SizeIndex)
 530         {
 531                 case SDP_DATASIZE_Variable8Bit:
 532                         *HeaderSize = (1 + sizeof(uint8_t));
 533                         return pgm_read_byte(AttributeData + 1);
 534                 case SDP_DATASIZE_Variable16Bit:
 535                         *HeaderSize = (1 + sizeof(uint16_t));
 536                         return pgm_read_word(AttributeData + 1);
 537                 case SDP_DATASIZE_Variable32Bit:
 538                         *HeaderSize = (1 + sizeof(uint32_t));
 539                         return pgm_read_dword(AttributeData + 1);
 540                 default:
 541                         *HeaderSize = 1;
 542                         return (1 << SizeIndex);
 543         }
 544
 545         return 0;
 546 }
 547
 548 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 549  *  pointer to the start of the Data Element's contents.
 550  *
 551  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 552  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 553  *
 554  *  \return Size in bytes of the Data Element container's contents, minus the header
 555  */
 556 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 557 {
 558         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 559         uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
 560         *DataElementHeader += sizeof(uint8_t);
 561
 562         uint32_t ElementValueSize;
 563
 564         /* Convert the Data Element size index into a size in bytes */
 565         switch (SizeIndex)
 566         {
 567                 case SDP_DATASIZE_Variable8Bit:
 568                         ElementValueSize    = *((uint8_t*)*DataElementHeader);
 569                         *DataElementHeader += sizeof(uint8_t);
 570                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 571                         break;
 572                 case SDP_DATASIZE_Variable16Bit:
 573                         ElementValueSize    = SwapEndian_16(*((uint16_t*)*DataElementHeader));
 574                         *DataElementHeader += sizeof(uint16_t);
 575                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 576                         break;
 577                 case SDP_DATASIZE_Variable32Bit:
 578                         ElementValueSize    = SwapEndian_32(*((uint32_t*)*DataElementHeader));
 579                         *DataElementHeader += sizeof(uint32_t);
 580                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 581                         break;
 582                 default:
 583                         ElementValueSize    = (1 << SizeIndex);
 584                         *ElementHeaderSize  = 1;
 585                         break;
 586         }
 587
 588         return ElementValueSize;
 589 }