projects / pub / USBasp.git / blob
? search:


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