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