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