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