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