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                 SerialPort_Attribute_Table,
  38         };
  39
  40 /** Base UUID value common to all standardized Bluetooth services */
  41 const UUID_t BaseUUID PROGMEM = {0x00000000, BASE_80BIT_UUID};
  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                         uint8_t CurrentUUID[16];
 491                         memcpy_P(CurrentUUID, (CurrAttribute + 1), 16);
 492
 493                         BT_SDP_DEBUG(2, "-- TEST UUID: %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 494                                 CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
 495                                 CurrentUUID[4], CurrentUUID[5],
 496                                                 CurrentUUID[6], CurrentUUID[7],
 497                                 CurrentUUID[8], CurrentUUID[9],
 498                                                 CurrentUUID[10], CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
 499
 500                         if (!(UUIDMatch[i]) && !(memcmp_P(UUIDList[i], (CurrAttribute + 1), UUID_SIZE_BYTES)))
 501                         {
 502                                 /* Indicate match found for the current attribute UUID and early-abort */
 503                                 UUIDMatch[i] = true;
 504                                 break;
 505                         }
 506                 }
 507         }
 508         else if (CurrAttributeType == SDP_DATATYPE_Sequence)
 509         {
 510                 uint8_t  SequenceHeaderSize;
 511                 uint16_t SequenceSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &SequenceHeaderSize);
 512
 513                 CurrAttribute += SequenceHeaderSize;
 514
 515                 /* Recursively unwrap the sequence container, and re-search its contents for UUIDs */
 516                 while (SequenceSize)
 517                 {
 518                         uint8_t  InnerHeaderSize;
 519                         uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize);
 520
 521                         SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute);
 522
 523                         SequenceSize  -= InnerHeaderSize + InnerSize;
 524                         CurrAttribute += InnerHeaderSize + InnerSize;
 525                 }
 526         }
 527 }
 528
 529 /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given
 530  *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 531  *
 532  *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 533  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 534  *
 535  *  \return Total number of Attribute ranges stored in the Data Element Sequence
 536  */
 537 static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
 538 {
 539         uint8_t ElementHeaderSize;
 540         uint8_t TotalAttributes = 0;
 541
 542         /* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
 543         uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 544         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 545         while (AttributeIDListLength)
 546         {
 547                 /* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
 548                 uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
 549                 uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 550
 551                 /* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
 552                 memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
 553
 554                 /* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
 555                 if (AttributeLength == 2)
 556                   CurrentAttributeRange[1] = CurrentAttributeRange[0];
 557
 558                 /* Swap the endianness of the attribute range values */
 559                 CurrentAttributeRange[0] = SwapEndian_16(CurrentAttributeRange[0]);
 560                 CurrentAttributeRange[1] = SwapEndian_16(CurrentAttributeRange[1]);
 561
 562                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
 563
 564                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 565                 *CurrentParameter     += AttributeLength;
 566         }
 567
 568         return TotalAttributes;
 569 }
 570
 571 /** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given
 572  *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 573  *
 574  *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 575  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 576  *
 577  *  \return Total number of UUIDs stored in the Data Element Sequence
 578  */
 579 static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
 580 {
 581         uint8_t ElementHeaderSize;
 582         uint8_t TotalUUIDs = 0;
 583
 584         /* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
 585         uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 586         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 587         while (ServicePatternLength)
 588         {
 589                 /* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
 590                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 591                 uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 592
 593                 /* Copy over UUID from the container to the free slot */
 594                 if (UUIDLength <= 4)
 595                 {
 596                         /* Copy over the base UUID value to the free UUID slot in the list */
 597                         memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID));
 598
 599                         /* Copy over short UUID */
 600                         memcpy(CurrentUUID + (4 - UUIDLength), *CurrentParameter, UUIDLength);
 601                 }
 602                 else
 603                 {
 604                         /* Copy over full UUID */
 605                         memcpy(CurrentUUID, *CurrentParameter, UUIDLength);
 606                 }
 607
 608                 BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 609                                 UUIDLength,
 610                                 CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
 611                                 CurrentUUID[4], CurrentUUID[5],
 612                                                 CurrentUUID[6], CurrentUUID[7],
 613                                 CurrentUUID[8], CurrentUUID[9],
 614                                                 CurrentUUID[10], CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
 615
 616                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 617                 *CurrentParameter    += UUIDLength;
 618         }
 619
 620         return TotalUUIDs;
 621 }
 622
 623 /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 624  *
 625  *  \param[in]  AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 626  *  \param[out] HeaderSize     Pointer to a location where the header size of the data element is to be stored
 627  *
 628  *  \return Size in bytes of the entire attribute container, including the header
 629  */
 630 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
 631 {
 632         /* Fetch the size of the Data Element structure from the header */
 633         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 634
 635         uint32_t ElementValueSize;
 636
 637         /* Convert the Data Element size index into a size in bytes */
 638         switch (SizeIndex)
 639         {
 640                 case SDP_DATASIZE_Variable8Bit:
 641                         *HeaderSize = (1 + sizeof(uint8_t));
 642                         ElementValueSize = pgm_read_byte(AttributeData + 1);
 643                         break;
 644                 case SDP_DATASIZE_Variable16Bit:
 645                         *HeaderSize = (1 + sizeof(uint16_t));
 646                         ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
 647                         break;
 648                 case SDP_DATASIZE_Variable32Bit:
 649                         *HeaderSize = (1 + sizeof(uint32_t));
 650                         ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
 651                         break;
 652                 default:
 653                         *HeaderSize = 1;
 654                         ElementValueSize = (1 << SizeIndex);
 655                         break;
 656         }
 657
 658         return ElementValueSize;
 659 }
 660
 661 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 662  *  pointer to the start of the Data Element's contents.
 663  *
 664  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 665  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 666  *
 667  *  \return Size in bytes of the Data Element container's contents, minus the header
 668  */
 669 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 670 {
 671         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 672         uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);
 673
 674         uint32_t ElementValueSize;
 675
 676         /* Convert the Data Element size index into a size in bytes */
 677         switch (SizeIndex)
 678         {
 679                 case SDP_DATASIZE_Variable8Bit:
 680                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 681                         ElementValueSize    = SDP_ReadData8(DataElementHeader);
 682                         break;
 683                 case SDP_DATASIZE_Variable16Bit:
 684                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 685                         ElementValueSize    = SDP_ReadData16(DataElementHeader);
 686                         break;
 687                 case SDP_DATASIZE_Variable32Bit:
 688                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 689                         ElementValueSize    = SDP_ReadData32(DataElementHeader);
 690                         break;
 691                 default:
 692                         *ElementHeaderSize  = 1;
 693                         ElementValueSize    = (1 << SizeIndex);
 694                         break;
 695         }
 696
 697         return ElementValueSize;
 698 }