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