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