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         BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
 228         BT_SDP_DEBUG(2, "-- Total Parameter Length: 0x%04X", ResponsePacket.SDPHeader.ParameterLength);
 229
 230         Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
 231                              Channel);
 232 }
 233
 234 static void* ServiceDiscovery_GetAttributeValue(ServiceAttributeTable_t* AttributeTable, uint16_t AttributeID)
 235 {
 236         void* CurrTableItemData;
 237         while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
 238         {
 239                 if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
 240                   return CurrTableItemData;
 241
 242                 AttributeTable++;
 243         }
 244
 245         return NULL;
 246 }
 247
 248 static ServiceAttributeTable_t* ServiceDiscovery_GetAttributeTable(uint8_t* UUID)
 249 {
 250         for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
 251         {
 252                 if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
 253                   return (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
 254         }
 255
 256         return NULL;
 257 }
 258
 259 static uint8_t ServiceDiscovery_GetAttributeList(uint16_t AttributeList[][2], const void** CurrentParameter)
 260 {
 261         uint8_t ElementHeaderSize;
 262         uint8_t TotalAttributes = 0;
 263
 264         uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 265         BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
 266         while (AttributeIDListLength)
 267         {
 268                 uint8_t AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 269
 270                 memcpy(&AttributeList[TotalAttributes][0], *CurrentParameter, AttributeLength);
 271
 272                 if (AttributeLength == 2)
 273                   memcpy(&AttributeList[TotalAttributes][1], *CurrentParameter, 2);
 274
 275                 BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", AttributeList[TotalAttributes][0], AttributeList[TotalAttributes][1]);
 276
 277                 TotalAttributes++;
 278
 279                 AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
 280                 *CurrentParameter     += AttributeLength;
 281         }
 282
 283         return TotalAttributes;
 284 }
 285
 286 static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** CurrentParameter)
 287 {
 288         uint8_t ElementHeaderSize;
 289         uint8_t TotalUUIDs = 0;
 290
 291         uint16_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 292         BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
 293         while (ServicePatternLength)
 294         {
 295                 uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
 296                 uint8_t  UUIDLength  = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
 297
 298                 memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
 299                 memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
 300
 301                 BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
 302                                 UUIDLength,
 303                                 CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
 304                                 CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
 305                                 CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
 306                                 CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);
 307
 308                 ServicePatternLength -= (UUIDLength + ElementHeaderSize);
 309                 *CurrentParameter    += UUIDLength;
 310         }
 311
 312         return TotalUUIDs;
 313 }
 314
 315 static uint32_t ServiceDiscovery_GetLocalAttributeSize(const void* AttributeData)
 316 {
 317         /* Fetch the size of the Data Element structure from the header */
 318         uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
 319
 320         /* Convert the Data Element size index into a size in bytes */
 321         switch (SizeIndex)
 322         {
 323                 case 5:
 324                         return (1 + sizeof(uint8_t)) + pgm_read_byte(AttributeData + 1);
 325                 case 6:
 326                         return (1 + sizeof(uint16_t)) + pgm_read_word(AttributeData + 1);
 327                 case 7:
 328                         return (1 + sizeof(uint32_t)) + pgm_read_dword(AttributeData + 1);
 329                 default:
 330                         return (1 + (1 << SizeIndex));
 331         }
 332
 333         return 0;
 334 }
 335
 336 static uint32_t ServiceDiscovery_GetDataElementSize(const void** DataElementHeader, uint8_t* ElementHeaderSize)
 337 {
 338         /* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
 339         uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
 340         *DataElementHeader += sizeof(uint8_t);
 341
 342         uint32_t ElementValue;
 343
 344         /* Convert the Data Element size index into a size in bytes */
 345         switch (SizeIndex)
 346         {
 347                 case 5:
 348                         ElementValue = *((uint8_t*)*DataElementHeader);
 349                         *DataElementHeader += sizeof(uint8_t);
 350                         *ElementHeaderSize  = (1 + sizeof(uint8_t));
 351                         break;
 352                 case 6:
 353                         ElementValue = *((uint16_t*)*DataElementHeader);
 354                         *DataElementHeader += sizeof(uint16_t);
 355                         *ElementHeaderSize  = (1 + sizeof(uint16_t));
 356                         break;
 357                 case 7:
 358                         ElementValue = *((uint32_t*)*DataElementHeader);
 359                         *DataElementHeader += sizeof(uint32_t);
 360                         *ElementHeaderSize  = (1 + sizeof(uint32_t));
 361                         break;
 362                 default:
 363                         ElementValue = (1 << SizeIndex);
 364                         *ElementHeaderSize = 1;
 365                         break;
 366         }
 367
 368         return ElementValue;
 369 }