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