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 /** Master service table, listing all supported services (and their attribute tables) of the device, including
  35  *  each service's UUID.
  36  */
  37 const ServiceTable_t SDP_Services_Table[] PROGMEM =
  38         {
  39                 {   // 128-bit UUID for the SDP service
  40                         .UUID  = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x01}},
  41                         .AttributeTable = SDP_Attribute_Table,
  42                 },
  43                 {   // 128-bit UUID for the RFCOMM service
  44                         .UUID  = {BASE_80BIT_UUID, {0x00, 0x00, 0x00, 0x00, 0x00, 0x03}},
  45                         .AttributeTable = RFCOMM_Attribute_Table,
  46                 },
  47         };
  48
  49 /** Base UUID value common to all standardized Bluetooth services */
  50 const UUID_t BaseUUID PROGMEM = {BASE_80BIT_UUID, {0, 0, 0, 0, 0, 0}};
  51
  52 /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
  53  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
  54  *  services the local device exposes.
  55  *
  56  *  \param[in]  Data     Incomming packet data containing the SDP request
  57  *  \param[in]  Channel  Channel the request was issued to by the remote device
  58  */
  59 void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
  60 {
  61         SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
  62         SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);
  63
  64         BT_SDP_DEBUG(1, "SDP Packet Received");
  65         BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
  66         BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);
  67
  68         switch (SDPHeader->PDU)
  69         {
  70                 case SDP_PDU_SERVICESEARCHREQUEST:
  71                         SDP_ProcessServiceSearch(SDPHeader, Channel);
  72                         break;
  73                 case SDP_PDU_SERVICEATTRIBUTEREQUEST:
  74                         SDP_ProcessServiceAttribute(SDPHeader, Channel);
  75                         break;
  76                 case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
  77                         SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
  78                         break;
  79         }
  80 }
  81
  82 /** Internal processing routine for SDP Service Search Requests.
  83  *
  84  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
  85  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
  86  */
  87 static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
  88 {
  89         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
  90
  91         BT_SDP_DEBUG(1, "<< Service Search");
  92
  93         /* Retrieve the list of search UUIDs from the request */
  94         uint8_t UUIDList[12][UUID_SIZE_BYTES];
  95         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
  96         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
  97
  98         /* Retrieve the maximum service record reponse count from the request */
  99         uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
 100         BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
 101
 102         struct
 103         {
 104                 SDP_PDUHeader_t SDPHeader;
 105                 uint16_t        TotalServiceRecordCount;
 106                 uint16_t        CurrentServiceRecordCount;
 107                 uint8_t         ResponseData[100];
 108         } ResponsePacket;
 109
 110         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 111         void* CurrResponsePos = ResponsePacket.ResponseData;
 112
 113         uint8_t AddedServiceHandles = 0;
 114
 115         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 116         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 117         {
 118                 ServiceAttributeTable_t* AttributeTable;
 119
 120                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 121                 if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
 122                   continue;
 123
 124                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 125
 126                 /* Retrieve a PROGMEM pointer to the value of the service's record handle */
 127                 const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 128
 129                 /* Copy over the service record handle to the response list */
 130                 uint8_t AttrHeaderSize;
 131                 SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
 132                 memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, sizeof(uint32_t));
 133                 CurrResponsePos += AttrHeaderSize + sizeof(uint32_t);
 134
 135                 /* Increment the total number of service records added to the list */
 136                 AddedServiceHandles++;
 137         }
 138
 139         /* Continuation state - always zero */
 140         SDP_WriteData8(&CurrResponsePos, 0);
 141
 142         /* Fill out the service record count values in the returned packet */
 143         ResponsePacket.TotalServiceRecordCount   = SwapEndian_16(AddedServiceHandles);
 144         ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;
 145
 146         /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created service
 147            handle list and the SDP continuation state */
 148         uint16_t ParamLength = (ResponsePacket.CurrentServiceRecordCount << 2) +
 149                                 sizeof(ResponsePacket.CurrentServiceRecordCount) +
 150                                 sizeof(ResponsePacket.TotalServiceRecordCount) +
 151                                 sizeof(uint8_t);
 152
 153         /* Fill in the response packet's header */
 154         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
 155         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 156         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
 157
 158         BT_SDP_DEBUG(1, ">> Service Search Response");
 159
 160         /* Send the completed response packet to the sender */
 161         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 162 }
 163
 164 /** Internal processing routine for SDP Service Attribute Requests.
 165  *
 166  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 167  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 168  */
 169 static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 170 {
 171         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 172
 173         BT_SDP_DEBUG(1, "<< Service Attribute");
 174
 175         /* Retrieve the service handle whose attributes are to be examined */
 176         uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
 177         BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
 178
 179         /* Retrieve the maximum Attribute reponse size from the request */
 180         uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
 181         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 182
 183         /* Retrieve the list of Attributes from the request */
 184         uint16_t AttributeList[15][2];
 185         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 186         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 187
 188         struct
 189         {
 190                 SDP_PDUHeader_t SDPHeader;
 191                 uint16_t        AttributeListByteCount;
 192                 uint8_t         ResponseData[100];
 193         } ResponsePacket;
 194
 195         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 196         void* CurrResponsePos = ResponsePacket.ResponseData;
 197
 198         /* Clamp the maximum attribute size to the size of the allocated buffer */
 199         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 200           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 201
 202         /* Add the outer Data Element Sequence header for all of the retrieved Attributes */
 203         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 204
 205         /* Search through the global UUID list an item at a time */
 206         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 207         {
 208                 /* Read in a pointer to the current UUID table entry's Attribute table */
 209                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 210
 211                 /* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
 212                 const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 213
 214                 /* Get the size of the header for the Service Record Handle */
 215                 uint8_t AttrHeaderSize;
 216                 SDP_GetLocalAttributeContainerSize(ServiceRecord, &AttrHeaderSize);
 217
 218                 /* Retrieve the endian-swapped service handle of the current service being examined */
 219                 uint32_t CurrServiceHandle = SwapEndian_32(pgm_read_dword(ServiceRecord + AttrHeaderSize));
 220
 221                 /* Check if the current service in the service table has the requested service handle */
 222                 if (ServiceHandle == CurrServiceHandle)
 223                 {
 224                         /* Add the listed attributes for the found UUID to the response */
 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, uint16_t AttributeList[][2],
 355                                                   const uint8_t TotalAttributes, void** const 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  *  \param[out] HeaderSize     Pointer to a location where the header size of the data element is to be stored
 581  *
 582  *  \return Size in bytes of the entire attribute container, including the header
 583  */
 584 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
 585 {
 586         /* Fetch the size of the Data Element structure from the header */
 587         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 588
 589         uint32_t ElementValueSize;
 590
 591         /* Convert the Data Element size index into a size in bytes */
 592         switch (SizeIndex)
 593         {
 594                 case SDP_DATASIZE_Variable8Bit:
 595                         *HeaderSize = (1 + sizeof(uint8_t));
 596                         ElementValueSize = pgm_read_byte(AttributeData + 1);
 597                         break;
 598                 case SDP_DATASIZE_Variable16Bit:
 599                         *HeaderSize = (1 + sizeof(uint16_t));
 600                         ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
 601                         break;
 602                 case SDP_DATASIZE_Variable32Bit:
 603                         *HeaderSize = (1 + sizeof(uint32_t));
 604                         ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
 605                         break;
 606                 default:
 607                         *HeaderSize = 1;
 608                         ElementValueSize = (1 << SizeIndex);
 609                         break;
 610         }
 611
 612         return ElementValueSize;
 613 }
 614
 615 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 616  *  pointer to the start of the Data Element's contents.
 617  *
 618  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 619  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 620  *
 621  *  \return Size in bytes of the Data Element container's contents, minus the header
 622  */
 623 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 624 {
 625         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 626         uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);
 627
 628         uint32_t ElementValueSize;
 629
 630         /* Convert the Data Element size index into a size in bytes */
 631         switch (SizeIndex)
 632         {
 633                 case SDP_DATASIZE_Variable8Bit:
 634                         ElementValueSize    = SDP_ReadData8(DataElementHeader);
 635                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 636                         break;
 637                 case SDP_DATASIZE_Variable16Bit:
 638                         ElementValueSize    = SDP_ReadData16(DataElementHeader);
 639                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 640                         break;
 641                 case SDP_DATASIZE_Variable32Bit:
 642                         ElementValueSize    = SDP_ReadData32(DataElementHeader);
 643                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 644                         break;
 645                 default:
 646                         ElementValueSize    = (1 << SizeIndex);
 647                         *ElementHeaderSize  = 1;
 648                         break;
 649         }
 650
 651         return ElementValueSize;
 652 }