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         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 318
 319         BT_SDP_DEBUG(1, "<< Service Attribute");
 320
 321         /* Retrieve the service handle whose attributes are to be examined */
 322         uint32_t ServiceHandle = SwapEndian_32(*((uint16_t*)CurrentParameter));
 323         CurrentParameter += sizeof(uint32_t);
 324         BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
 325
 326         /* Retrieve the maximum Attribute reponse size from the request */
 327         uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
 328         CurrentParameter += sizeof(uint16_t);
 329         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 330
 331         /* Retrieve the list of Attributes from the request */
 332         uint16_t AttributeList[15][2];
 333         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 334         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 335
 336         struct
 337         {
 338                 SDP_PDUHeader_t SDPHeader;
 339                 uint16_t        AttributeListByteCount;
 340                 uint8_t         ResponseData[100];
 341         } ResponsePacket;
 342
 343         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 344         void* CurrResponsePos = ResponsePacket.ResponseData;
 345
 346         /* Clamp the maximum attribute size to the size of the allocated buffer */
 347         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 348           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 349
 350         /* Add the outer Data Element Sequence header for all of the retrieved Attributes */
 351         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 352
 353         /* Search through the global UUID list an item at a time */
 354         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 355         {
 356                 /* Read in a pointer to the current UUID table entry's Attribute table */
 357                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 358
 359                 /* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
 360                 const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
 361
 362                 /* Check if the current service in the service table has the requested service handle */
 363                 if (memcmp_P(ServiceRecord, &ServiceHandle, sizeof(uint32_t)) == 0)
 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
 370                                 /* Look in the current Attribute Range for a matching Attribute ID in the service's Attribute table */
 371                                 for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
 372                                 {
 373                                         /* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the service's Attribute table */
 374                                         const void* AttributeValue = SDP_GetAttributeValue(CurrAttributeTable, CurrAttributeID);
 375
 376                                         /* If the Attribute does not exist in the current service's Attribute table, continue to the next Attribute ID */
 377                                         if (AttributeValue == NULL)
 378                                           continue;
 379
 380                                         BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
 381
 382                                         /* Increment the service's returned Attribute container size by the number of added bytes */
 383                                         *TotalResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
 384                                 }
 385                         }
 386
 387                         /* Requested service found, abort the search through the service table */
 388                         break;
 389                 }
 390         }
 391
 392         /* Continuation state - always zero */
 393         *((uint8_t*)CurrResponsePos) = 0;
 394
 395         /* Set the total response list size to the size of the outer container plus its header size and continuation state */
 396         ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
 397
 398         /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
 399            value list and the SDP continuation state */
 400         uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) +
 401                                 (3 + *TotalResponseSize) +
 402                                 sizeof(uint8_t));
 403
 404         /* Fill in the response packet's header */
 405         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICEATTRIBUTERESPONSE;
 406         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 407         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
 408
 409         /* Flip the endianness of the container's size */
 410         *TotalResponseSize = SwapEndian_16(*TotalResponseSize);
 411
 412         BT_SDP_DEBUG(1, ">> Service Attribute Response");
 413         BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
 414
 415         /* Send the completed response packet to the sender */
 416         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 417 }
 418
 419 /** Internal processing routine for SDP Service Search Attribute Requests.
 420  *
 421  *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 422  *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 423  */
 424 static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
 425 {
 426         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 427
 428         BT_SDP_DEBUG(1, "<< Service Search Attribute");
 429
 430         /* Retrieve the list of search UUIDs from the request */
 431         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 432         uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
 433         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 434
 435         /* Retrieve the maximum Attribute reponse size from the request */
 436         uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
 437         CurrentParameter += sizeof(uint16_t);
 438         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 439
 440         /* Retrieve the list of Attributes from the request */
 441         uint16_t AttributeList[15][2];
 442         uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
 443         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 444
 445         struct
 446         {
 447                 SDP_PDUHeader_t SDPHeader;
 448                 uint16_t        AttributeListByteCount;
 449                 uint8_t         ResponseData[100];
 450         } ResponsePacket;
 451
 452         /* Create a pointer to the buffer to indicate the current location for response data to be added */
 453         void* CurrResponsePos = ResponsePacket.ResponseData;
 454
 455         /* Clamp the maximum attribute size to the size of the allocated buffer */
 456         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 457           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 458
 459         /* Add the outer Data Element Sequence header for all of the retrieved Attributes */
 460         uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 461
 462         /* Search through the list of UUIDs one at a time looking for matching search Attributes */
 463         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 464         {
 465                 /* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
 466                 ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
 467
 468                 /* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
 469                 if (AttributeTable == NULL)
 470                   continue;
 471
 472                 BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);
 473
 474                 /* Add an inner Data Element Sequence header for the current UUID's found Attributes */
 475                 uint16_t* CurrentUUIDResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
 476
 477                 /* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
 478                 for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
 479                 {
 480                         uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
 481
 482                         /* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
 483                         for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
 484                         {
 485                                 /* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
 486                                 const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
 487
 488                                 /* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
 489                                 if (AttributeValue == NULL)
 490                                   continue;
 491
 492                                 BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
 493
 494                                 /* Increment the current UUID's returned Attribute container size by the number of added bytes */
 495                                 *CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
 496                         }
 497
 498                         /* Increment the outer container size by the number of added bytes */
 499                         *TotalResponseSize += 3 + *CurrentUUIDResponseSize;
 500                 }
 501
 502                 /* Flip the endianness of the container's size */
 503                 *CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize);
 504         }
 505
 506         /* Continuation state - always zero */
 507         *((uint8_t*)CurrResponsePos) = 0;
 508
 509         /* Set the total response list size to the size of the outer container plus its header size and continuation state */
 510         ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
 511
 512         /* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
 513            value list and the SDP continuation state */
 514         uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) +
 515                                 (3 + *TotalResponseSize) +
 516                                 sizeof(uint8_t));
 517
 518         /* Fill in the response packet's header */
 519         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 520         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 521         ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
 522
 523         /* Flip the endianness of the container's size */
 524         *TotalResponseSize = SwapEndian_16(*TotalResponseSize);
 525
 526         BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
 527         BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
 528
 529         /* Send the completed response packet to the sender */
 530         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
 531 }
 532
 533 /** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
 534  *
 535  *  \param[in] AttributeID          Attribute ID to add to the response buffer
 536  *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM
 537  *  \param[in, out] ResponseBuffer  Pointer to a buffer where the Attribute and Attribute Value is to be added
 538  *
 539  *  \return Number of bytes added to the response buffer
 540  */
 541 static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
 542 {
 543         /* Retrieve the size of the attribute value from its container header */
 544         uint8_t  AttributeHeaderLength;
 545         uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
 546
 547         /* Add a Data Element header to the response for the Attribute ID */
 548         *((uint8_t*)*ResponseBuffer) = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit);
 549         *ResponseBuffer += sizeof(uint8_t);
 550
 551         /* Add the Attribute ID to the created Data Element */
 552         *((uint16_t*)*ResponseBuffer) = SwapEndian_16(AttributeID);
 553         *ResponseBuffer += sizeof(uint16_t);
 554
 555         /* Copy over the Attribute value Data Element container to the response */
 556         memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
 557         *ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
 558
 559         return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
 560 }
 561
 562 /** Retrieves a pointer to the value of the given Attribute ID from the given Attribute table.
 563  *
 564  *  \param[in] AttributeTable  Pointer to the Attribute table to search in
 565  *  \param[in] AttributeID     Attribute ID to search for within the table
 566  *
 567  *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
 568  */
 569 static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
 570 {
 571         void* CurrTableItemData;
 572
 573         /* Search through the current Attribute table, abort when the terminator item has been reached */
 574         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 575         {
 576                 /* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
 577                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 578                   return CurrTableItemData;
 579
 580                 AttributeTable++;
 581         }
 582
 583         return NULL;
 584 }
 585
 586 /** Retrieves the Attribute table for the given UUID if it exists.
 587  *
 588  *  \param[in] UUID  UUID to search for
 589  *
 590  *  \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise
 591  */
 592 static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
 593 {
 594         /* Search through the global UUID list an item at a time */
 595         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 596         {
 597                 /* Read in a pointer to the current UUID table entry's Attribute table */
 598                 ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 599
 600                 /* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
 601                 if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 602                   return CurrAttributeTable;
 603
 604                 /* Retrieve the list of the service's Class UUIDs from its Attribute table */
 605                 void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS);
 606
 607                 /* Go to the next UUID in the table if the current item does not have a list of Class UUIDs */
 608                 if (ClassUUIDs == NULL)
 609                   continue;
 610
 611                 /* Retrieve the size of the Class UUID list and skip past the header to the first Class UUID in the list */
 612                 uint8_t  ClassUUIDListHeaderSize;
 613                 uint32_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize);
 614                 ClassUUIDs += ClassUUIDListHeaderSize;
 615
 616                 /* Check each class UUID in turn for a match */
 617                 while (ClassUUIDListSize)
 618                 {
 619                         /* Current Service UUID's Class UUID list has a matching entry, return the Attribute table */
 620                         if (!(memcmp_P(UUID, &((ClassUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
 621                           return CurrAttributeTable;
 622
 623                         ClassUUIDListSize -= sizeof(ClassUUID_t);
 624                         ClassUUIDs        += sizeof(ClassUUID_t);
 625                 }
 626         }
 627
 628         return NULL;
 629 }
 630
 631 /** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given
 632  *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 633  *
 634  *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 635  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 636  *
 637  *  \return Total number of Attribute ranges stored in the Data Element Sequence
 638  */
 639 static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
 640 {
 641         uint8_t ElementHeaderSize;
 642         uint8_t TotalAttributes = 0;
 643
 644         /* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
 645         uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 646         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 647         while (AttributeIDListLength)
 648         {
 649                 /* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
 650                 uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
 651                 uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 652
 653                 /* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
 654                 memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
 655
 656                 /* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
 657                 if (AttributeLength == 2)
 658                   CurrentAttributeRange[1] = CurrentAttributeRange[0];
 659
 660                 /* Swap the endianness of the attribute range values */
 661                 CurrentAttributeRange[0] = SwapEndian_16(CurrentAttributeRange[0]);
 662                 CurrentAttributeRange[1] = SwapEndian_16(CurrentAttributeRange[1]);
 663
 664                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
 665
 666                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 667                 *CurrentParameter     += AttributeLength;
 668         }
 669
 670         return TotalAttributes;
 671 }
 672
 673 /** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given
 674  *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 675  *
 676  *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 677  *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 678  *
 679  *  \return Total number of UUIDs stored in the Data Element Sequence
 680  */
 681 static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
 682 {
 683         uint8_t ElementHeaderSize;
 684         uint8_t TotalUUIDs = 0;
 685
 686         /* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
 687         uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 688         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 689         while (ServicePatternLength)
 690         {
 691                 /* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
 692                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 693                 uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 694
 695                 /* Copy over the base UUID value to the free UUID slot in the list */
 696                 memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 697
 698                 /* Copy over UUID from the container to the free slot - if a short UUID (<= 4 bytes) it replaces the lower
 699                    4 bytes of the base UUID, otherwise it replaces the UUID completely */
 700                 if (UUIDLength <= 4)
 701                 {
 702                         memcpy(&CurrentUUID[UUID_SIZE_BYTES - 4], *CurrentParameter, UUIDLength);
 703                         SwapEndian_n(&CurrentUUID[UUID_SIZE_BYTES - 4], UUIDLength);
 704                 }
 705                 else
 706                 {
 707                         memcpy(&CurrentUUID[0], *CurrentParameter, UUIDLength);
 708                 }
 709
 710                 BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 711                                 UUIDLength,
 712                                 CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
 713                                 CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
 714                                 CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
 715                                 CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
 716
 717                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 718                 *CurrentParameter    += UUIDLength;
 719         }
 720
 721         return TotalUUIDs;
 722 }
 723
 724 /** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 725  *
 726  *  \param[in] AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 727  *
 728  *  \return Size in bytes of the entire attribute container, including the header
 729  */
 730 static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
 731 {
 732         /* Fetch the size of the Data Element structure from the header */
 733         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 734
 735         /* Convert the Data Element size index into a size in bytes */
 736         switch (SizeIndex)
 737         {
 738                 case SDP_DATASIZE_Variable8Bit:
 739                         *HeaderSize = (1 + sizeof(uint8_t));
 740                         return pgm_read_byte(AttributeData + 1);
 741                 case SDP_DATASIZE_Variable16Bit:
 742                         *HeaderSize = (1 + sizeof(uint16_t));
 743                         return SwapEndian_16(pgm_read_word(AttributeData + 1));
 744                 case SDP_DATASIZE_Variable32Bit:
 745                         *HeaderSize = (1 + sizeof(uint32_t));
 746                         return SwapEndian_32(pgm_read_dword(AttributeData + 1));
 747                 default:
 748                         *HeaderSize = 1;
 749                         return (1 << SizeIndex);
 750         }
 751
 752         return 0;
 753 }
 754
 755 /** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 756  *  pointer to the start of the Data Element's contents.
 757  *
 758  *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 759  *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 760  *
 761  *  \return Size in bytes of the Data Element container's contents, minus the header
 762  */
 763 static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
 764 {
 765         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 766         uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
 767         *DataElementHeader += sizeof(uint8_t);
 768
 769         uint32_t ElementValueSize;
 770
 771         /* Convert the Data Element size index into a size in bytes */
 772         switch (SizeIndex)
 773         {
 774                 case SDP_DATASIZE_Variable8Bit:
 775                         ElementValueSize    = *((uint8_t*)*DataElementHeader);
 776                         *DataElementHeader += sizeof(uint8_t);
 777                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 778                         break;
 779                 case SDP_DATASIZE_Variable16Bit:
 780                         ElementValueSize    = SwapEndian_16(*((uint16_t*)*DataElementHeader));
 781                         *DataElementHeader += sizeof(uint16_t);
 782                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 783                         break;
 784                 case SDP_DATASIZE_Variable32Bit:
 785                         ElementValueSize    = SwapEndian_32(*((uint32_t*)*DataElementHeader));
 786                         *DataElementHeader += sizeof(uint32_t);
 787                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 788                         break;
 789                 default:
 790                         ElementValueSize    = (1 << SizeIndex);
 791                         *ElementHeaderSize  = 1;
 792                         break;
 793         }
 794
 795         return ElementValueSize;
 796 }