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

   1 /*
   2              LUFA Library
   3      Copyright (C) Dean Camera, 2010.
   4
   5   dean [at] fourwalledcubicle [dot] com
   6       www.fourwalledcubicle.com
   7 */
   8
   9 /*
  10   Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)
  11
  12   Permission to use, copy, modify, distribute, and sell this
  13   software and its documentation for any purpose is hereby granted
  14   without fee, provided that the above copyright notice appear in
  15   all copies and that both that the copyright notice and this
  16   permission notice and warranty disclaimer appear in supporting
  17   documentation, and that the name of the author not be used in
  18   advertising or publicity pertaining to distribution of the
  19   software without specific, written prior permission.
  20
  21   The author disclaim all warranties with regard to this
  22   software, including all implied warranties of merchantability
  23   and fitness.  In no event shall the author be liable for any
  24   special, indirect or consequential damages or any damages
  25   whatsoever resulting from loss of use, data or profits, whether
  26   in an action of contract, negligence or other tortious action,
  27   arising out of or in connection with the use or performance of
  28   this software.
  29 */
  30
  31 #define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
  32 #include "ServiceDiscoveryProtocol.h"
  33
  34 /** Service Discovery Protocol attribute, indicationg the service's name. */
  35 const struct
  36 {
  37         uint8_t Header;
  38         uint8_t Length;
  39         uint8_t Data[];
  40 } PROGMEM SDP_Attribute_Name = {(SDP_DATATYPE_String | 5), sizeof("SDP"), "SDP"};
  41
  42 /** Service Discovery Protocol attribute, indicationg the service's description. */
  43 const struct
  44 {
  45         uint8_t Header;
  46         uint8_t Length;
  47         uint8_t Data[];
  48 } PROGMEM SDP_Attribute_Description = {(SDP_DATATYPE_String | 5), sizeof("BT Service Discovery"), "BT Service Discovery"};
  49
  50 /** Service Discovery Protocol attribute, indicationg the service's availability. */
  51 const struct
  52 {
  53         uint8_t Header;
  54         uint8_t Data;
  55 } PROGMEM SDP_Attribute_Availability = {(SDP_DATATYPE_UnsignedInt | 0), 0xFF};
  56
  57 const struct
  58 {
  59         uint8_t  Header;
  60         uint16_t Data;
  61 } PROGMEM SDP_Attribute_LanguageOffset = {(SDP_DATATYPE_UnsignedInt | 1), 0x0100};
  62
  63 const struct
  64 {
  65         uint8_t  Header;
  66         uint16_t Data;
  67 } PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | 1), 0x0001};
  68
  69 const struct
  70 {
  71         uint8_t     Header;
  72         uint8_t     Size;
  73         ClassUUID_t UUIDList[];
  74 } PROGMEM SDP_Attribute_ServiceClassIDs =
  75         {
  76                 (SDP_DATATYPE_Sequence | 5), (sizeof(ClassUUID_t) * 1),
  77                 {
  78                         {.Header = (SDP_DATATYPE_UUID | 5), .Size = UUID_SIZE_BYTES, .UUID = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00}}
  79                 }
  80         };
  81
  82 /** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
  83 const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
  84         {
  85                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE,   .Data = &SDP_Attribute_ServiceHandle},
  86                 {.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,       .Data = &SDP_Attribute_ServiceClassIDs},
  87                 {.AttributeID = SDP_ATTRIBUTE_ID_LANGIDOFFSET,          .Data = &SDP_Attribute_LanguageOffset},
  88                 {.AttributeID = SDP_ATTRIBUTE_IDO_PROVIDER    | 0x0100, .Data = &SDP_Attribute_Name},
  89                 {.AttributeID = SDP_ATTRIBUTE_IDO_DESCRIPTION | 0x0100, .Data = &SDP_Attribute_Description},
  90
  91                 SERVICE_ATTRIBUTE_TABLE_TERMINATOR
  92         };
  93
  94 /** Master service table, listing all supported services (and their attribute tables) of the device, including
  95  *  each service's UUID.
  96  */
  97 const ServiceTable_t SDP_Services_Table[] PROGMEM =
  98         {
  99                 {   // 128-bit UUID for the SDP service
 100                         .UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
 101                         .AttributeTable = SDP_Attribute_Table,
 102                 },
 103 #if 0
 104                 {   // 128-bit UUID for the RFCOMM service
 105                         .UUID  = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00},
 106                         .AttributeTable = RFCOMM_Attribute_Table,
 107                 },
 108 #endif
 109         };
 110
 111 /** Base UUID value common to all standardized Bluetooth services */
 112 const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
 113
 114
 115 /** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
 116  *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
 117  *  services the local device exposes.
 118  *
 119  *  \param[in]  Data     Incomming packet data containing the SDP request
 120  *  \param[in]  Channel  Channel the request was issued to by the remote device
 121  */
 122 void ServiceDiscovery_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
 123 {
 124         SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
 125         SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);
 126
 127         BT_SDP_DEBUG(1, "SDP Packet Received");
 128         BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
 129         BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);
 130
 131         switch (SDPHeader->PDU)
 132         {
 133                 case SDP_PDU_SERVICESEARCHREQUEST:
 134                         ServiceDiscovery_ProcessServiceSearch(SDPHeader, Channel);
 135                         break;
 136                 case SDP_PDU_SERVICEATTRIBUTEREQUEST:
 137                         ServiceDiscovery_ProcessServiceAttribute(SDPHeader, Channel);
 138                         break;
 139                 case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
 140                         ServiceDiscovery_ProcessServiceSearchAttribute(SDPHeader, Channel);
 141                         break;
 142         }
 143 }
 144
 145 static void ServiceDiscovery_ProcessServiceSearch(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
 146 {
 147         BT_SDP_DEBUG(1, "<< Service Search");
 148 }
 149
 150 static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
 151 {
 152         BT_SDP_DEBUG(1, "<< Service Attribute");
 153 }
 154
 155 static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
 156 {
 157         const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
 158
 159         BT_SDP_DEBUG(1, "<< Service Search Attribute");
 160
 161         uint8_t UUIDList[12][UUID_SIZE_BYTES];
 162         uint8_t TotalUUIDs = ServiceDiscovery_GetUUIDList(UUIDList, &CurrentParameter);
 163         BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
 164
 165         uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(&CurrentParameter);
 166         BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
 167
 168         uint16_t AttributeList[15][2];
 169         uint8_t  TotalAttributes = ServiceDiscovery_GetAttributeList(AttributeList, &CurrentParameter);
 170         BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
 171
 172         struct
 173         {
 174                 SDP_PDUHeader_t SDPHeader;
 175                 uint16_t        AttributeListByteCount;
 176                 uint8_t         ResponseData[100];
 177         } ResponsePacket;
 178
 179         uint8_t* CurrResponsePos = ResponsePacket.ResponseData;
 180
 181         if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
 182           MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
 183
 184         uint16_t* TotalResponseSize = ServiceDiscovery_AddDataElementHeader(&CurrResponsePos, SDP_DATATYPE_Sequence);
 185         for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
 186         {
 187                 ServiceAttributeTable_t* AttributeTable = ServiceDiscovery_GetAttributeTable(UUIDList[CurrUUIDItem]);
 188
 189                 if (AttributeTable == NULL)
 190                   continue;
 191
 192                 uint16_t* CurrentUUIDResponseSize = ServiceDiscovery_AddDataElementHeader(&CurrResponsePos, SDP_DATATYPE_Sequence);
 193                 for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
 194                 {
 195                         uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
 196
 197                         for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
 198                         {
 199                                 const void* AttributeValue = ServiceDiscovery_GetAttributeValue(AttributeTable, CurrAttributeID);
 200
 201                                 if (AttributeValue == NULL)
 202                                   continue;
 203
 204                                 uint32_t AttributeValueLength = ServiceDiscovery_GetLocalAttributeSize(AttributeValue);
 205
 206                                 BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);
 207
 208                                 *((uint8_t*)CurrResponsePos) = (1 | SDP_DATATYPE_UnsignedInt);
 209                                 CurrResponsePos += sizeof(uint8_t);
 210                                 *((uint16_t*)CurrResponsePos) = CurrAttributeID;
 211                                 CurrResponsePos += sizeof(uint16_t);
 212                                 memcpy_P(CurrResponsePos, AttributeValue, AttributeValueLength);
 213                                 CurrResponsePos += AttributeValueLength;
 214
 215                                 *CurrentUUIDResponseSize += sizeof(uint8_t) + sizeof(uint16_t) + AttributeValueLength;
 216                         }
 217
 218                         *TotalResponseSize += 3 + *CurrentUUIDResponseSize;
 219                 }
 220         }
 221
 222         ResponsePacket.AttributeListByteCount    = (*TotalResponseSize + 3);
 223         ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
 224         ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
 225         ResponsePacket.SDPHeader.ParameterLength = (ResponsePacket.AttributeListByteCount + sizeof(ResponsePacket.AttributeListByteCount));
 226
 227         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
 228                              Channel);
 229 }
 230
 231 static void* ServiceDiscovery_GetAttributeValue(ServiceAttributeTable_t* AttributeTable, uint16_t AttributeID)
 232 {
 233         void* CurrTableItemData;
 234         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 235         {
 236                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 237                   return CurrTableItemData;
 238
 239                 AttributeTable++;
 240         }
 241
 242         return NULL;
 243 }
 244
 245 static ServiceAttributeTable_t* ServiceDiscovery_GetAttributeTable(uint8_t* UUID)
 246 {
 247         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 248         {
 249                 if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 250                   return (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 251         }
 252
 253         return NULL;
 254 }
 255
 256 static uint8_t ServiceDiscovery_GetAttributeList(uint16_t AttributeList[][2], const void** CurrentParameter)
 257 {
 258         uint8_t ElementHeaderSize;
 259         uint8_t TotalAttributes = 0;
 260
 261         uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 262         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 263         while (AttributeIDListLength)
 264         {
 265                 uint8_t AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 266
 267                 memcpy(&AttributeList[TotalAttributes][0], *CurrentParameter, AttributeLength);
 268
 269                 if (AttributeLength == 2)
 270                   memcpy(&AttributeList[TotalAttributes][1], *CurrentParameter, 2);
 271
 272                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", AttributeList[TotalAttributes][0], AttributeList[TotalAttributes][1]);
 273
 274                 TotalAttributes++;
 275
 276                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 277                 *CurrentParameter     += AttributeLength;
 278         }
 279
 280         return TotalAttributes;
 281 }
 282
 283 static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** CurrentParameter)
 284 {
 285         uint8_t ElementHeaderSize;
 286         uint8_t TotalUUIDs = 0;
 287
 288         uint16_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 289         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 290         while (ServicePatternLength)
 291         {
 292                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 293                 uint8_t  UUIDLength  = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 294
 295                 memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 296                 memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
 297
 298                 BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 299                                 UUIDLength,
 300                                 CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
 301                                 CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
 302                                 CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
 303                                 CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
 304
 305                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 306                 *CurrentParameter    += UUIDLength;
 307         }
 308
 309         return TotalUUIDs;
 310 }
 311
 312 static uint32_t ServiceDiscovery_GetLocalAttributeSize(const void* AttributeData)
 313 {
 314         /* Fetch the size of the Data Element structure from the header */
 315         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 316
 317         /* Convert the Data Element size index into a size in bytes */
 318         switch (SizeIndex)
 319         {
 320                 case 5:
 321                         return (1 + sizeof(uint8_t)) + pgm_read_byte(AttributeData + 1);
 322                 case 6:
 323                         return (1 + sizeof(uint16_t)) + pgm_read_word(AttributeData + 1);
 324                 case 7:
 325                         return (1 + sizeof(uint32_t)) + pgm_read_dword(AttributeData + 1);
 326                 default:
 327                         return (1 + (1 << SizeIndex));
 328         }
 329
 330         return 0;
 331 }
 332
 333 static uint32_t ServiceDiscovery_GetDataElementSize(const void** DataElementHeader, uint8_t* ElementHeaderSize)
 334 {
 335         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 336         uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
 337         *DataElementHeader += sizeof(uint8_t);
 338
 339         uint32_t ElementValue;
 340
 341         /* Convert the Data Element size index into a size in bytes */
 342         switch (SizeIndex)
 343         {
 344                 case 5:
 345                         ElementValue = *((uint8_t*)*DataElementHeader);
 346                         *DataElementHeader += sizeof(uint8_t);
 347                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 348                         break;
 349                 case 6:
 350                         ElementValue = *((uint16_t*)*DataElementHeader);
 351                         *DataElementHeader += sizeof(uint16_t);
 352                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 353                         break;
 354                 case 7:
 355                         ElementValue = *((uint32_t*)*DataElementHeader);
 356                         *DataElementHeader += sizeof(uint32_t);
 357                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 358                         break;
 359                 default:
 360                         ElementValue = (1 << SizeIndex);
 361                         *ElementHeaderSize = 1;
 362                         break;
 363         }
 364
 365         return ElementValue;
 366 }