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

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