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


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