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


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