ServiceDiscoveryProtocol.c 27.7 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
/*
             LUFA Library
     Copyright (C) Dean Camera, 2010.
              
  dean [at] fourwalledcubicle [dot] com
      www.fourwalledcubicle.com
*/

/*
  Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)

  Permission to use, copy, modify, distribute, and sell this 
  software and its documentation for any purpose is hereby granted
  without fee, provided that the above copyright notice appear in 
  all copies and that both that the copyright notice and this
  permission notice and warranty disclaimer appear in supporting 
  documentation, and that the name of the author not be used in 
  advertising or publicity pertaining to distribution of the 
  software without specific, written prior permission.

  The author disclaim all warranties with regard to this
  software, including all implied warranties of merchantability
  and fitness.  In no event shall the author be liable for any
  special, indirect or consequential damages or any damages
  whatsoever resulting from loss of use, data or profits, whether
  in an action of contract, negligence or other tortious action,
  arising out of or in connection with the use or performance of
  this software.
*/

#define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
#include "ServiceDiscoveryProtocol.h"

34
35
36
/** Master service table, listing all supported services (and their attribute tables) of the device, including
 *  each service's UUID.
 */
37
const ServiceTable_t SDP_Services_Table[] PROGMEM =
38
	{
39
40
41
		{ .UUID  = SDP_UUID   , .AttributeTable = SDP_Attribute_Table     },
		{ .UUID  = RFCOMM_UUID, .AttributeTable = RFCOMM_Attribute_Table  },
		{ .UUID  = L2CAP_UUID , .AttributeTable = L2CAP_Attribute_Table   },
42
43
	};

44
45
/** Base UUID value common to all standardized Bluetooth services */
const UUID_t BaseUUID PROGMEM = {BASE_80BIT_UUID, {0, 0, 0, 0, 0, 0}};
46

47
48
49
50
51
52
53
/** Main Service Discovery Protocol packet processing routine. This function processes incomming SDP packets from
 *  a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
 *  services the local device exposes.
 *
 *  \param[in]  Data     Incomming packet data containing the SDP request
 *  \param[in]  Channel  Channel the request was issued to by the remote device
 */
54
void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
55
56
57
58
59
60
61
62
63
64
65
{
	SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
	SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);

	BT_SDP_DEBUG(1, "SDP Packet Received");
	BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
	BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);

	switch (SDPHeader->PDU)
	{
		case SDP_PDU_SERVICESEARCHREQUEST:
66
			SDP_ProcessServiceSearch(SDPHeader, Channel);
67
68
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
69
			SDP_ProcessServiceAttribute(SDPHeader, Channel);
70
71
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
72
			SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
73
74
75
76
			break;
	}
}

77
78
79
80
81
82
/** Internal processing routine for SDP Service Search Requests.
 *
 *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 */
static void SDP_ProcessServiceSearch(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
83
{
84
85
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

86
	BT_SDP_DEBUG(1, "<< Service Search");
87
88
89
90
91
92
93

	/* Retrieve the list of search UUIDs from the request */
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
	uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
	
	/* Retrieve the maximum service record reponse count from the request */
94
	uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
	BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
	
	struct
	{
		SDP_PDUHeader_t SDPHeader;
		uint16_t        TotalServiceRecordCount;
		uint16_t        CurrentServiceRecordCount;
		uint8_t         ResponseData[100];
	} ResponsePacket;

	/* Create a pointer to the buffer to indicate the current location for response data to be added */
	void* CurrResponsePos = ResponsePacket.ResponseData;
	
	uint8_t AddedServiceHandles = 0;

	/* Search through the list of UUIDs one at a time looking for matching search Attributes */
	for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
	{
113
114
		ServiceAttributeTable_t* AttributeTable;

115
		/* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
116
		if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
		  continue;
		  
		BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);

		/* Retrieve a PROGMEM pointer to the value of the service's record handle */
		const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);

		/* Copy over the service record handle to the response list */
		uint8_t AttrHeaderSize;
		SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
		memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, sizeof(uint32_t));
		CurrResponsePos += AttrHeaderSize + sizeof(uint32_t);
		
		/* Increment the total number of service records added to the list */
		AddedServiceHandles++;
	}

	/* Continuation state - always zero */
135
	SDP_WriteData8(&CurrResponsePos, 0);
136
137
138
139
140

	/* Fill out the service record count values in the returned packet */
	ResponsePacket.TotalServiceRecordCount   = SwapEndian_16(AddedServiceHandles);
	ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;

141
142
143
144
145
146
147
	/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created service
	   handle list and the SDP continuation state */
	uint16_t ParamLength = (ResponsePacket.CurrentServiceRecordCount << 2) +
	                        sizeof(ResponsePacket.CurrentServiceRecordCount) +
	                        sizeof(ResponsePacket.TotalServiceRecordCount) +
	                        sizeof(uint8_t);

148
149
150
	/* Fill in the response packet's header */
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
151
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
152
153
154
155

	BT_SDP_DEBUG(1, ">> Service Search Response");

	/* Send the completed response packet to the sender */
156
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
157
158
}

159
160
161
162
163
164
/** Internal processing routine for SDP Service Attribute Requests.
 *
 *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 */
static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
165
{
166
167
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

168
	BT_SDP_DEBUG(1, "<< Service Attribute");
169
170

	/* Retrieve the service handle whose attributes are to be examined */
171
	uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
172
173
174
	BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
	
	/* Retrieve the maximum Attribute reponse size from the request */
175
	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
	
	/* Retrieve the list of Attributes from the request */
	uint16_t AttributeList[15][2];
	uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);

	struct
	{
		SDP_PDUHeader_t SDPHeader;
		uint16_t        AttributeListByteCount;
		uint8_t         ResponseData[100];
	} ResponsePacket;

	/* Create a pointer to the buffer to indicate the current location for response data to be added */
	void* CurrResponsePos = ResponsePacket.ResponseData;

	/* Clamp the maximum attribute size to the size of the allocated buffer */
	if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
	  MaxAttributeSize = sizeof(ResponsePacket.ResponseData);

197
	uint16_t TotalResponseSize = 0;
198
199
200
201
202
203
204
205
206
207

	/* Search through the global UUID list an item at a time */
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
	{
		/* Read in a pointer to the current UUID table entry's Attribute table */
		ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
		
		/* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
		const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
		
208
209
210
		/* Get the size of the header for the Service Record Handle */
		uint8_t AttrHeaderSize;
		SDP_GetLocalAttributeContainerSize(ServiceRecord, &AttrHeaderSize);
211
212
213
214
		
		/* Retrieve the endian-swapped service handle of the current service being examined */
		uint32_t CurrServiceHandle = SwapEndian_32(pgm_read_dword(ServiceRecord + AttrHeaderSize));
		
215
		/* Check if the current service in the service table has the requested service handle */
216
		if (ServiceHandle == CurrServiceHandle)
217
		{
218
			/* Add the listed attributes for the found UUID to the response */
219
220
			TotalResponseSize = SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
		                                                          &CurrResponsePos);
221
222
223
224
225
226
227
			
			/* Requested service found, abort the search through the service table */
			break;
		}
	}

	/* Continuation state - always zero */
228
	SDP_WriteData8(&CurrResponsePos, 0);
229
230

	/* Set the total response list size to the size of the outer container plus its header size and continuation state */
231
	ResponsePacket.AttributeListByteCount    = SwapEndian_16(TotalResponseSize);
232
233
234

	/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
	   value list and the SDP continuation state */
235
236
	uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + TotalResponseSize + sizeof(uint8_t));
	
237
238
239
240
241
242
243
244
245
246
	/* Fill in the response packet's header */
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICEATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);

	BT_SDP_DEBUG(1, ">> Service Attribute Response");
	BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);

	/* Send the completed response packet to the sender */
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
247
248
}

249
250
251
252
253
254
/** Internal processing routine for SDP Service Search Attribute Requests.
 *
 *  \param[in] SDPHeader  Pointer to the start of the issued SDP request
 *  \param[in] Channel    Pointer to the Bluetooth channel structure the request was issued to
 */
static void SDP_ProcessServiceSearchAttribute(const SDP_PDUHeader_t* const SDPHeader, Bluetooth_Channel_t* const Channel)
255
{
256
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
257
258
259
	
	BT_SDP_DEBUG(1, "<< Service Search Attribute");

260
	/* Retrieve the list of search UUIDs from the request */
261
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
262
	uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
263
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
264
	
265
	/* Retrieve the maximum Attribute reponse size from the request */
266
	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
267
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
268
	
269
	/* Retrieve the list of Attributes from the request */
270
	uint16_t AttributeList[15][2];
271
	uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
272
273
	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
	
274
275
276
	struct
	{
		SDP_PDUHeader_t SDPHeader;
277
		uint16_t        AttributeListByteCount;
278
279
280
		uint8_t         ResponseData[100];
	} ResponsePacket;
	
281
	/* Create a pointer to the buffer to indicate the current location for response data to be added */
282
	void* CurrResponsePos = ResponsePacket.ResponseData;
283

284
	/* Clamp the maximum attribute size to the size of the allocated buffer */
285
286
	if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
	  MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
287

288
	/* Add the outer Data Element Sequence header for all of the retrieved Attributes */
289
	uint16_t* TotalResponseSize = SDP_AddSequence16(&CurrResponsePos);
290
291
	
	/* Search through the list of UUIDs one at a time looking for matching search Attributes */
292
293
	for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
	{
294
295
		ServiceAttributeTable_t* AttributeTable;

296
		/* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
297
		if ((AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem])) == NULL)
298
299
		  continue;
		  
300
301
		BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);

302
		/* Add the listed attributes for the found UUID to the response */
303
		*TotalResponseSize += SDP_AddListedAttributesToResponse(AttributeTable, AttributeList, TotalAttributes, 
304
		                                                        &CurrResponsePos);
305
	}
306
307
	
	/* Continuation state - always zero */
308
	SDP_WriteData8(&CurrResponsePos, 0);
309

310
	/* Set the total response list size to the size of the outer container plus its header size and continuation state */
311
	ResponsePacket.AttributeListByteCount    = SwapEndian_16(3 + *TotalResponseSize);
312

313
314
315
316
317
318
	/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
	   value list and the SDP continuation state */
	uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + 
	                        (3 + *TotalResponseSize) +
	                        sizeof(uint8_t));

319
320
321
	/* Flip the endianness of the container's size */
	*TotalResponseSize = SwapEndian_16(*TotalResponseSize);

322
	/* Fill in the response packet's header */
323
324
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
325
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
326

327
	BT_SDP_DEBUG(1, ">> Service Search Attribute Response");
328
	BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
329

330
	/* Send the completed response packet to the sender */
331
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
332
333
}

334
335
336
337
338
339
340
341
342
/** Adds all the Attributes in the given service table to the response that appear in the Attribute table.
 *
 *  \param[in]  AttributeTable   Pointer to an Attribute table for the service to examine
 *  \param[in]  AttributeList    Pointer to a list of Attribute ranges
 *  \param[in]  TotalAttributes  Number of Attributes stored in the Attribute list
 *  \param[out] BufferPos       Pointer to the output buffer position where the retrieved attributes are to be stored
 *
 *  \return Number of bytes added to the output buffer
 */
343
344
static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable, uint16_t AttributeList[][2],
                                                  const uint8_t TotalAttributes, void** const BufferPos)
345
{
346
	uint16_t TotalResponseSize;
347
348

	/* Add an inner Data Element Sequence header for the current services's found Attributes */
349
	uint16_t* AttributeListSize = SDP_AddSequence16(BufferPos);
350
351
352
353
354

	/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
	for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
	{
		uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
355
356
357
358
		void*     AttributeValue;
		
		/* Look through the current service's attribute list, examining all the attributes */
		while ((AttributeValue = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
359
		{
360
361
362
363
364
365
366
367
368
369
370
			/* Get the current Attribute's ID from the current attribute table entry */
			uint16_t CurrAttributeID = pgm_read_word(&AttributeTable->AttributeID);

			/* Check if the current Attribute's ID is within the current Attribute range */
			if ((CurrAttributeID >= AttributeIDRange[0]) && (CurrAttributeID <= AttributeIDRange[1]))
			{
				/* Increment the current UUID's returned Attribute container size by the number of added bytes */
				*AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos);			
			}
			
			AttributeTable++;
371
372
373
		}
	}

374
375
376
	/* Record the total number of added bytes to the buffer */
	TotalResponseSize = 3 + *AttributeListSize;

377
378
379
380
381
382
	/* Fix endianness of the added attribute data element sequence */
	*AttributeListSize = SwapEndian_16(*AttributeListSize);

	return TotalResponseSize;
}

383
384
385
386
387
388
389
390
391
392
393
/** Adds the given attribute ID and value to the reponse buffer, and advances the response buffer pointer past the added data.
 *
 *  \param[in] AttributeID          Attribute ID to add to the response buffer
 *  \param[in] AttributeValue       Pointer to the start of the Attribute's value, located in PROGMEM
 *  \param[in, out] ResponseBuffer  Pointer to a buffer where the Attribute and Attribute Value is to be added
 *
 *  \return Number of bytes added to the response buffer
 */
static uint16_t SDP_AddAttributeToResponse(const uint16_t AttributeID, const void* AttributeValue, void** ResponseBuffer)
{
	/* Retrieve the size of the attribute value from its container header */
394
	uint8_t  AttributeHeaderLength;
395
	uint16_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
396
	
397
398
	BT_SDP_DEBUG(2, " -- Add Attribute (0x%04X) 0x%04X", (AttributeHeaderLength + AttributeValueLength), AttributeID);

399
	/* Add a Data Element header to the response for the Attribute ID */
400
	SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
401
402
	
	/* Add the Attribute ID to the created Data Element */
403
	SDP_WriteData16(ResponseBuffer, AttributeID);
404
405
	
	/* Copy over the Attribute value Data Element container to the response */
406
407
	memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
	*ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
408
	
409
	return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
410
411
}

412
413
414
415
416
417
418
419
/** Retrieves a pointer to the value of the given Attribute ID from the given Attribute table.
 *
 *  \param[in] AttributeTable  Pointer to the Attribute table to search in
 *  \param[in] AttributeID     Attribute ID to search for within the table
 *
 *  \return Pointer to the start of the Attribute's value if found within the table, NULL otherwise
 */
static void* SDP_GetAttributeValue(const ServiceAttributeTable_t* AttributeTable, const uint16_t AttributeID)
420
{
421
	void* CurrTableItemData;
422
423
	
	/* Search through the current Attribute table, abort when the terminator item has been reached */
424
	while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
425
	{
426
		/* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
427
		if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
428
		  return CurrTableItemData;
429
430
431
432
433
434
435
		
		AttributeTable++;
	}
			
	return NULL;
}

436
437
438
439
440
441
442
/** Retrieves the Attribute table for the given UUID if it exists.
 *
 *  \param[in] UUID  UUID to search for
 *
 *  \return Pointer to the UUID's associated Attribute table if found in the global UUID table, NULL otherwise
 */
static ServiceAttributeTable_t* SDP_GetAttributeTable(const uint8_t* const UUID)
443
{
444
	/* Search through the global UUID list an item at a time */
445
446
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
	{
447
448
449
		/* Read in a pointer to the current UUID table entry's Attribute table */
		ServiceAttributeTable_t* CurrAttributeTable = (ServiceAttributeTable_t*)pgm_read_word(&SDP_Services_Table[CurrTableItem].AttributeTable);
	
450
		/* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
451
		if (!(memcmp_P(UUID, &SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
452
453
454
455
456
457
458
459
460
461
462
		  return CurrAttributeTable;
		
		/* Retrieve the list of the service's Class UUIDs from its Attribute table */
		void* ClassUUIDs = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICECLASSIDS);
		
		/* Go to the next UUID in the table if the current item does not have a list of Class UUIDs */
		if (ClassUUIDs == NULL)
		  continue;
		  
		/* Retrieve the size of the Class UUID list and skip past the header to the first Class UUID in the list */ 
		uint8_t  ClassUUIDListHeaderSize;
463
		uint16_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize);
464
465
466
467
468
469
		ClassUUIDs += ClassUUIDListHeaderSize;
		
		/* Check each class UUID in turn for a match */
		while (ClassUUIDListSize)
		{
			/* Current Service UUID's Class UUID list has a matching entry, return the Attribute table */
470
			if (!(memcmp_P(UUID, &((ItemUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
471
472
			  return CurrAttributeTable;
		
473
474
			ClassUUIDListSize -= sizeof(ItemUUID_t);
			ClassUUIDs        += sizeof(ItemUUID_t);
475
		}	
476
477
478
479
480
	}
	
	return NULL;
}

481
482
483
484
485
486
487
488
489
/** Reads in the collection of Attribute ranges from the input buffer's Data Element Sequence container, into the given 
 *  Attribute list for later use. Once complete, the input buffer pointer is advanced to the end of the Attribute container.
 *
 *  \param[out] AttributeList     Pointer to a buffer where the list of Attribute ranges are to be stored
 *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of Attribute and Attribute Range elements
 *
 *  \return Total number of Attribute ranges stored in the Data Element Sequence
 */
static uint8_t SDP_GetAttributeList(uint16_t AttributeList[][2], const void** const CurrentParameter)
490
{
491
492
	uint8_t ElementHeaderSize;
	uint8_t TotalAttributes = 0;
493

494
495
	/* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
	uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
496
497
498
	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
	while (AttributeIDListLength)
	{
499
500
501
		/* Retrieve the size of the next Attribute in the container and get a pointer to the next free Attribute element in the list */
		uint16_t* CurrentAttributeRange = AttributeList[TotalAttributes++];
		uint8_t   AttributeLength       = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
502
		
503
504
		/* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
		memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
505
		
506
		/* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
507
		if (AttributeLength == 2)
508
509
510
511
512
		  CurrentAttributeRange[1] = CurrentAttributeRange[0];

		/* Swap the endianness of the attribute range values */
		CurrentAttributeRange[0] = SwapEndian_16(CurrentAttributeRange[0]);
		CurrentAttributeRange[1] = SwapEndian_16(CurrentAttributeRange[1]);
513

514
		BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
515
		
516
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
517
		*CurrentParameter     += AttributeLength;
518
	}
519
	
520
	return TotalAttributes;
521
522
}

523
524
525
526
527
528
529
530
531
/** Reads in the collection of UUIDs from the input buffer's Data Element Sequence container, into the given 
 *  UUID list for later use. Once complete, the input buffer pointer is advanced to the end of the UUID container.
 *
 *  \param[out] UUIDList          Pointer to a buffer where the list of UUIDs are to be stored
 *  \param[in]  CurrentParameter  Pointer to a Buffer containing a Data Element Sequence of UUID elements
 *
 *  \return Total number of UUIDs stored in the Data Element Sequence
 */
static uint8_t SDP_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** const CurrentParameter)
532
533
534
535
{
	uint8_t ElementHeaderSize;
	uint8_t TotalUUIDs = 0;

536
537
	/* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
	uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
538
539
540
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
	while (ServicePatternLength)
	{
541
		/* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
542
		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
543
		uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
544
		
545
		/* Copy over the base UUID value to the free UUID slot in the list */
546
		memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID));
547

548
549
		/* Copy over UUID from the container to the free slot */
		memcpy(&CurrentUUID[UUID_SIZE_BYTES - UUIDLength], *CurrentParameter, UUIDLength);
550
		
551
		BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
552
		                UUIDLength,
553
554
555
556
557
		                CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
		                CurrentUUID[4], CurrentUUID[5],
						CurrentUUID[6],  CurrentUUID[7],
		                CurrentUUID[8],  CurrentUUID[9],
						CurrentUUID[10],  CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
558
559
560
561
562
563
564
565

		ServicePatternLength -= (UUIDLength + ElementHeaderSize);
		*CurrentParameter    += UUIDLength;
	}
	
	return TotalUUIDs;
}

566
567
/** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 *
568
569
 *  \param[in]  AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 *  \param[out] HeaderSize     Pointer to a location where the header size of the data element is to be stored
570
571
572
 *
 *  \return Size in bytes of the entire attribute container, including the header
 */
573
static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
574
575
576
577
{
	/* Fetch the size of the Data Element structure from the header */
	uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
	
578
579
	uint32_t ElementValueSize;

580
581
582
	/* Convert the Data Element size index into a size in bytes */
	switch (SizeIndex)
	{
583
		case SDP_DATASIZE_Variable8Bit:
584
			*HeaderSize = (1 + sizeof(uint8_t));
585
586
			ElementValueSize = pgm_read_byte(AttributeData + 1);
			break;
587
		case SDP_DATASIZE_Variable16Bit:
588
			*HeaderSize = (1 + sizeof(uint16_t));
589
590
			ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
			break;
591
		case SDP_DATASIZE_Variable32Bit:
592
			*HeaderSize = (1 + sizeof(uint32_t));
593
594
			ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
			break;
595
		default:
596
			*HeaderSize = 1;
597
598
			ElementValueSize = (1 << SizeIndex);
			break;
599
600
	}

601
	return ElementValueSize;
602
603
}

604
605
606
607
608
609
610
611
612
/** Retrieves the size of a Data Element container from the current input buffer, and advances the input buffer
 *  pointer to the start of the Data Element's contents.
 *
 *  \param[in, out] DataElementHeader  Pointer to the start of a Data Element header
 *  \param[out]     ElementHeaderSize  Size in bytes of the header that was skipped
 *
 *  \return Size in bytes of the Data Element container's contents, minus the header
 */
static uint32_t SDP_GetDataElementSize(const void** const DataElementHeader, uint8_t* const ElementHeaderSize)
613
{
614
	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
615
616
	uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);	

617
	uint32_t ElementValueSize;
618
619

	/* Convert the Data Element size index into a size in bytes */
620
621
	switch (SizeIndex)
	{
622
		case SDP_DATASIZE_Variable8Bit:
623
			ElementValueSize    = SDP_ReadData8(DataElementHeader);
624
625
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
			break;
626
		case SDP_DATASIZE_Variable16Bit:
627
			ElementValueSize    = SDP_ReadData16(DataElementHeader);
628
629
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
			break;
630
		case SDP_DATASIZE_Variable32Bit:
631
			ElementValueSize    = SDP_ReadData32(DataElementHeader);
632
633
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
			break;
634
		default:
635
636
			ElementValueSize    = (1 << SizeIndex);
			*ElementHeaderSize  = 1;
637
			break;
638
639
	}
	
640
	return ElementValueSize;
641
}