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

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