Demos/Host/Incomplete/BluetoothHost/Lib/SDP.c

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