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         uint16_t TotalResponseSize = 0;
 203
 204         /* Search through the global UUID list an item at a time */
 205         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 206         {
 207                 /* Read in a pointer to the current UUID table entry's Attribute table */
 208                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 209
 210                 /* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
 211                 const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 212
 213                 /* Get the size of the header for the Service Record Handle */
 214                 uint8_t AttrHeaderSize;
 215                 SDP_GetLocalAttributeContainerSize(ServiceRecord, &AttrHeaderSize);
 216
 217                 /* Retrieve the endian-swapped service handle of the current service being examined */
 218                 uint32_t CurrServiceHandle = SwapEndian_32(pgm_read_dword(ServiceRecord + AttrHeaderSize));
 219
 220                 /* Check if the current service in the service table has the requested service handle */
 221                 if (ServiceHandle == CurrServiceHandle)
 222                 {
 223                         /* Add the listed attributes for the found UUID to the response */
 224                         TotalResponseSize = SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
 225                                                                           &CurrResponsePos);
 226
 227                         /* Requested service found, abort the search through the service table */
 228                         break;
 229                 }
 230         }
 231
 232         /* Continuation state - always zero */
 233         SDP_WriteData8(&CurrResponsePos, 0);
 234
 235         /* Set the total response list size to the size of the outer container plus its header size and continuation state */
 236         ResponsePacket.AttributeListByteCount    = SwapEndian_16(TotalResponseSize);
 237
 238         /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
 239            value list and the SDP continuation state */
 240         uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + TotalResponseSize + sizeof(uint8_t));
 241
 242         /* Fill in the response packet's header */
 243         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICEATTRIBUTERESPONSE;
 244         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 245         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
 246
 247         BT_SDP_DEBUG(1, ">> Service Attribute Response");
 248         BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
 249
 250         /* Send the completed response packet to the sender */
 251         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 252 }
 253
 254 /** Internal processing routine for SDP Service Search Attribute Requests.
 255  *
 256  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 257  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 258  */
 259 static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 260 {
 261         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 262
 263         BT_SDP_DEBUG(1, "<< Service Search Attribute");
 264
 265         /* Retrieve the list of search UUIDs from the request */
 266         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 267         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
 268         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 269
 270         /* Retrieve the maximum Attribute reponse size from the request */
 271         uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
 272         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 273
 274         /* Retrieve the list of Attributes from the request */
 275         uint16_t AttributeList[15][2];
 276         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 277         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 278
 279         struct
 280         {
 281                 SDP_PDUHeader_t SDPHeader;
 282                 uint16_t        AttributeListByteCount;
 283                 uint8_t         ResponseData[100];
 284         } ResponsePacket;
 285
 286         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 287         void* CurrResponsePos = ResponsePacket.ResponseData;
 288
 289         /* Clamp the maximum attribute size to the size of the allocated buffer */
 290         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 291           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 292
 293         /* Add the outer Data Element Sequence header for all of the retrieved Attributes */
 294         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 295
 296         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 297         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 298         {
 299                 ServiceAttributeTable_t* AttributeTable;
 300
 301                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 302                 if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
 303                   continue;
 304
 305                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 306
 307                 /* Add the listed attributes for the found UUID to the response */
 308                 *TotalResponseSize += SDP_AddListedAttributesToResponse(AttributeTable, AttributeList, TotalAttributes,
 309                                                                         &CurrResponsePos);
 310         }
 311
 312         /* Continuation state - always zero */
 313         SDP_WriteData8(&CurrResponsePos, 0);
 314
 315         /* Set the total response list size to the size of the outer container plus its header size and continuation state */
 316         ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
 317
 318         /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
 319            value list and the SDP continuation state */
 320         uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) +
 321                                 (3 + *TotalResponseSize) +
 322                                 sizeof(uint8_t));
 323
 324         /* Flip the endianness of the container's size */
 325         *TotalResponseSize = SwapEndian_16(*TotalResponseSize);
 326
 327         /* Fill in the response packet's header */
 328         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 329         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 330         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
 331
 332         BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
 333         BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
 334
 335         /* Send the completed response packet to the sender */
 336         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 337 }
 338
 339 /** Adds all the Attributes in the given service table to the response that appear in the Attribute table.
 340  *
 341  *  \param[in]  AttributeTable   Pointer to an Attribute table for the service to examine
 342  *  \param[in]  AttributeList    Pointer to a list of Attribute ranges
 343  *  \param[in]  TotalAttributes  Number of Attributes stored in the Attribute list
 344  *  \param[out] BufferPos       Pointer to the output buffer position where the retrieved attributes are to be stored
 345  *
 346  *  \return Number of bytes added to the output buffer
 347  */
 348 static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable, uint16_t AttributeList[][2],
 349                                                   const uint8_t TotalAttributes, void** const BufferPos)
 350 {
 351         uint16_t TotalResponseSize;
 352
 353         /* Add an inner Data Element Sequence header for the current services's found Attributes */
 354         uint16_t* AttributeListSize = SDP_AddDataElementHeader16(BufferPos, SDP_DATATYPE_Sequence);
 355
 356         /* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
 357         for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
 358         {
 359                 uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
 360                 void*     AttributeValue;
 361
 362                 /* Look through the current service's attribute list, examining all the attributes */
 363                 while ((AttributeValue = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 364                 {
 365                         /* Get the current Attribute's ID from the current attribute table entry */
 366                         uint16_t CurrAttributeID = pgm_read_word(&AttributeTable->AttributeID);
 367
 368                         /* Check if the current Attribute's ID is within the current Attribute range */
 369                         if ((CurrAttributeID >= AttributeIDRange[0]) && (CurrAttributeID <= AttributeIDRange[1]))
 370                         {
 371                                 /* Increment the current UUID's returned Attribute container size by the number of added bytes */
 372                                 *AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos);
 373                         }
 374
 375                         AttributeTable++;
 376                 }
 377         }
 378
 379         /* Record the total number of added bytes to the buffer */
 380         TotalResponseSize = 3 + *AttributeListSize;
 381
 382         /* Fix endianness of the added attribute data element sequence */
 383         *AttributeListSize = SwapEndian_16(*AttributeListSize);
 384
 385         return TotalResponseSize;
 386 }
 387
 388 /** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
 389  *
 390  *  \param[in] AttributeID          Attribute ID to add to the response buffer
 391  *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM
 392  *  \param[in, out] ResponseBuffer  Pointer to a buffer where the Attribute and Attribute Value is to be added
 393  *
 394  *  \return Number of bytes added to the response buffer
 395  */
 396 static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
 397 {
 398         /* Retrieve the size of the attribute value from its container header */
 399         uint8_t  AttributeHeaderLength;
 400         uint16_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
 401
 402         BT_SDP_DEBUG(2, " -- Add Attribute (0x%04X) 0x%04X", (AttributeHeaderLength + AttributeValueLength), AttributeID);
 403
 404         /* Add a Data Element header to the response for the Attribute ID */
 405         SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
 406
 407         /* Add the Attribute ID to the created Data Element */
 408         SDP_WriteData16(ResponseBuffer, AttributeID);
 409
 410         /* Copy over the Attribute value Data Element container to the response */
 411         memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
 412         *ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
 413
 414         return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
 415 }
 416
 417 /** Retrieves a pointer to the value of the given Attribute ID from the given Attribute table.
 418  *
 419  *  \param[in] AttributeTable  Pointer to the Attribute table to search in
 420  *  \param[in] AttributeID     Attribute ID to search for within the table
 421  *
 422  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
 423  */
 424 static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
 425 {
 426         void* CurrTableItemData;
 427
 428         /* Search through the current Attribute table, abort when the terminator item has been reached */
 429         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 430         {
 431                 /* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
 432                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 433                   return CurrTableItemData;
 434
 435                 AttributeTable++;
 436         }
 437
 438         return NULL;
 439 }
 440
 441 /** Retrieves the Attribute table for the given UUID if it exists.
 442  *
 443  *  \param[in] UUID  UUID to search for
 444  *
 445  *  \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise
 446  */
 447 static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 448 {
 449         /* Search through the global UUID list an item at a time */
 450         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 451         {
 452                 /* Read in a pointer to the current UUID table entry's Attribute table */
 453                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 454
 455                 /* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
 456                 if (!(memcmp_P(UUID, &SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 457                   return CurrAttributeTable;
 458
 459                 /* Retrieve the list of the service's Class UUIDs from its Attribute table */
 460                 void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS);
 461
 462                 /* Go to the next UUID in the table if the current item does not have a list of Class UUIDs */
 463                 if (ClassUUIDs == NULL)
 464                   continue;
 465
 466                 /* Retrieve the size of the Class UUID list and skip past the header to the first Class UUID in the list */
 467                 uint8_t  ClassUUIDListHeaderSize;
 468                 uint16_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize);
 469                 ClassUUIDs += ClassUUIDListHeaderSize;
 470
 471                 /* Check each class UUID in turn for a match */
 472                 while (ClassUUIDListSize)
 473                 {
 474                         /* Current Service UUID's Class UUID list has a matching entry, return the Attribute table */
 475                         if (!(memcmp_P(UUID, &((ItemUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
 476                           return CurrAttributeTable;
 477
 478                         ClassUUIDListSize -= sizeof(ItemUUID_t);
 479                         ClassUUIDs        += sizeof(ItemUUID_t);
 480                 }
 481         }
 482
 483         return NULL;
 484 }
 485
 486 /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given
 487  *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 488  *
 489  *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 490  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 491  *
 492  *  \return Total number of Attribute ranges stored in the Data Element Sequence
 493  */
 494 static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
 495 {
 496         uint8_t ElementHeaderSize;
 497         uint8_t TotalAttributes = 0;
 498
 499         /* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
 500         uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 501         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 502         while (AttributeIDListLength)
 503         {
 504                 /* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
 505                 uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
 506                 uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 507
 508                 /* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
 509                 memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
 510
 511                 /* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
 512                 if (AttributeLength == 2)
 513                   CurrentAttributeRange[1] = CurrentAttributeRange[0];
 514
 515                 /* Swap the endianness of the attribute range values */
 516                 CurrentAttributeRange[0] = SwapEndian_16(CurrentAttributeRange[0]);
 517                 CurrentAttributeRange[1] = SwapEndian_16(CurrentAttributeRange[1]);
 518
 519                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
 520
 521                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 522                 *CurrentParameter     += AttributeLength;
 523         }
 524
 525         return TotalAttributes;
 526 }
 527
 528 /** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given
 529  *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 530  *
 531  *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 532  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 533  *
 534  *  \return Total number of UUIDs stored in the Data Element Sequence
 535  */
 536 static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
 537 {
 538         uint8_t ElementHeaderSize;
 539         uint8_t TotalUUIDs = 0;
 540
 541         /* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
 542         uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 543         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 544         while (ServicePatternLength)
 545         {
 546                 /* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
 547                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 548                 uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 549
 550                 /* Copy over the base UUID value to the free UUID slot in the list */
 551                 memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID));
 552
 553                 /* Copy over UUID from the container to the free slot */
 554                 memcpy(&CurrentUUID[UUID_SIZE_BYTES - UUIDLength], *CurrentParameter, UUIDLength);
 555
 556                 BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 557                                 UUIDLength,
 558                                 CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
 559                                 CurrentUUID[4], CurrentUUID[5],
 560                                                 CurrentUUID[6],  CurrentUUID[7],
 561                                 CurrentUUID[8],  CurrentUUID[9],
 562                                                 CurrentUUID[10],  CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
 563
 564                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 565                 *CurrentParameter    += UUIDLength;
 566         }
 567
 568         return TotalUUIDs;
 569 }
 570
 571 /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 572  *
 573  *  \param[in]  AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 574  *  \param[out] HeaderSize     Pointer to a location where the header size of the data element is to be stored
 575  *
 576  *  \return Size in bytes of the entire attribute container, including the header
 577  */
 578 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
 579 {
 580         /* Fetch the size of the Data Element structure from the header */
 581         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 582
 583         uint32_t ElementValueSize;
 584
 585         /* Convert the Data Element size index into a size in bytes */
 586         switch (SizeIndex)
 587         {
 588                 case SDP_DATASIZE_Variable8Bit:
 589                         *HeaderSize = (1 + sizeof(uint8_t));
 590                         ElementValueSize = pgm_read_byte(AttributeData + 1);
 591                         break;
 592                 case SDP_DATASIZE_Variable16Bit:
 593                         *HeaderSize = (1 + sizeof(uint16_t));
 594                         ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
 595                         break;
 596                 case SDP_DATASIZE_Variable32Bit:
 597                         *HeaderSize = (1 + sizeof(uint32_t));
 598                         ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
 599                         break;
 600                 default:
 601                         *HeaderSize = 1;
 602                         ElementValueSize = (1 << SizeIndex);
 603                         break;
 604         }
 605
 606         return ElementValueSize;
 607 }
 608
 609 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 610  *  pointer to the start of the Data Element's contents.
 611  *
 612  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 613  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 614  *
 615  *  \return Size in bytes of the Data Element container's contents, minus the header
 616  */
 617 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 618 {
 619         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 620         uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);
 621
 622         uint32_t ElementValueSize;
 623
 624         /* Convert the Data Element size index into a size in bytes */
 625         switch (SizeIndex)
 626         {
 627                 case SDP_DATASIZE_Variable8Bit:
 628                         ElementValueSize    = SDP_ReadData8(DataElementHeader);
 629                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 630                         break;
 631                 case SDP_DATASIZE_Variable16Bit:
 632                         ElementValueSize    = SDP_ReadData16(DataElementHeader);
 633                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 634                         break;
 635                 case SDP_DATASIZE_Variable32Bit:
 636                         ElementValueSize    = SDP_ReadData32(DataElementHeader);
 637                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 638                         break;
 639                 default:
 640                         ElementValueSize    = (1 << SizeIndex);
 641                         *ElementHeaderSize  = 1;
 642                         break;
 643         }
 644
 645         return ElementValueSize;
 646 }