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