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