SDP.c 29.6 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
/*
             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.
*/

31
32
33
34
35
36
37
38
/** \file
 *
 *  SDP layer module. This module implements a simple Service Discovery
 *  Protocol server, which can broadcast the device's supported services
 *  to other Bluetooth devices upon request, so that they can determine
 *  what services are available.
 */

39
40
41
42
/*
	TODO: Honor remote device's buffer size constraints via continuation state
 */

43
#define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
44
#include "SDP.h"
45

46
47
/** Service attribute table list, containing a pointer to each service attribute table the device contains */
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
56
57
58
59
60
/** Initializes the SDP service, ready for new connections from a SDP client. */
void SDP_Initialize(void)
{
	/* Not currently used */
}

61
62
63
64
65
66
67
/** 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
 */
68
void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
69
70
71
72
73
74
75
76
{
	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);

77
	/* Dispatch to the correct processing routine for the given SDP packet type */
78
79
80
	switch (SDPHeader->PDU)
	{
		case SDP_PDU_SERVICESEARCHREQUEST:
81
			SDP_ProcessServiceSearch(SDPHeader, Channel);
82
83
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
84
			SDP_ProcessServiceAttribute(SDPHeader, Channel);
85
86
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
87
			SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
88
89
90
91
			break;
	}
}

92
93
94
95
96
97
/** 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)
98
{
99
100
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

101
	BT_SDP_DEBUG(1, "<< Service Search");
102
103
104
105
106
107
108

	/* 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 */
109
	uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
110
111
112
113
114
115
116
117
118
	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;
119
120
	
	uint8_t AddedServiceHandles = 0;
121
122
123
124

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

125
126
	/* Search through the global service list an item at a time */
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++)
127
	{
128
129
		/* Read in a pointer to the current UUID table entry's Attribute table */
		ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
130

131
		if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable)))
132
		  continue;
133
134

		BT_SDP_DEBUG(2, " -- Found search match in table");
135
136

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

		/* Copy over the service record handle to the response list */
		uint8_t AttrHeaderSize;
141
142
143
		uint8_t AttrSize = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
		memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, AttrSize);
		CurrResponsePos += AttrHeaderSize + AttrSize;
144
145
146
147
148
		
		AddedServiceHandles++;
	}

	/* Continuation state - always zero */
149
	SDP_WriteData8(&CurrResponsePos, 0);
150
151
152
153
154

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

155
156
157
158
159
160
161
	/* 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);

162
163
164
	/* Fill in the response packet's header */
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
165
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
166
167
168
169

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

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

173
174
175
176
177
178
/** 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)
179
{
180
181
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

182
	BT_SDP_DEBUG(1, "<< Service Attribute");
183
184

	/* Retrieve the service handle whose attributes are to be examined */
185
	uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
186
187
188
	BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
	
	/* Retrieve the maximum Attribute reponse size from the request */
189
	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
190
191
192
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
	
	/* Retrieve the list of Attributes from the request */
193
	uint16_t AttributeList[8][2];
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
	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);

211
	uint16_t TotalResponseSize = 0;
212
213

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

	/* Continuation state - always zero */
242
	SDP_WriteData8(&CurrResponsePos, 0);
243
244

	/* Set the total response list size to the size of the outer container plus its header size and continuation state */
245
	ResponsePacket.AttributeListByteCount    = SwapEndian_16(TotalResponseSize);
246
247
248

	/* 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 */
249
250
	uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + TotalResponseSize + sizeof(uint8_t));
	
251
252
253
254
255
256
257
258
259
260
	/* 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);
261
262
}

263
264
265
266
267
268
/** 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)
269
{
270
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
271
272
273
	
	BT_SDP_DEBUG(1, "<< Service Search Attribute");

274
	/* Retrieve the list of search UUIDs from the request */
275
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
276
	uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
277
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
278
	
279
	/* Retrieve the maximum Attribute reponse size from the request */
280
	uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
281
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
282
	
283
	/* Retrieve the list of Attributes from the request */
284
	uint16_t AttributeList[8][2];
285
	uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
286
287
	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
	
288
289
290
	struct
	{
		SDP_PDUHeader_t SDPHeader;
291
		uint16_t        AttributeListByteCount;
292
293
294
		uint8_t         ResponseData[100];
	} ResponsePacket;
	
295
	/* Create a pointer to the buffer to indicate the current location for response data to be added */
296
	void* CurrResponsePos = ResponsePacket.ResponseData;
297

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

302
	/* Add the outer Data Element Sequence header for all of the retrieved Attributes */
303
	uint16_t* TotalResponseSize = SDP_AddSequence16(&CurrResponsePos);
304
	
305
306
	/* Search through the global service list an item at a time */
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++)
307
	{
308
309
		/* Read in a pointer to the current UUID table entry's Attribute table */
		ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
310

311
		if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable)))
312
313
		  continue;
		  
314
		BT_SDP_DEBUG(2, " -- Found search match in table");
315

316
		/* Add the listed attributes for the found UUID to the response */
317
		*TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes, 
318
		                                                        &CurrResponsePos);
319
	}
320
321
	
	/* Continuation state - always zero */
322
	SDP_WriteData8(&CurrResponsePos, 0);
323

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

327
328
329
330
331
332
	/* 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));

333
334
335
	/* Flip the endianness of the container's size */
	*TotalResponseSize = SwapEndian_16(*TotalResponseSize);

336
	/* Fill in the response packet's header */
337
338
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
339
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
340

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

344
	/* Send the completed response packet to the sender */
345
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
346
347
}

348
349
350
351
352
353
354
355
356
/** 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
 */
357
358
static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable, uint16_t AttributeList[][2],
                                                  const uint8_t TotalAttributes, void** const BufferPos)
359
{
360
	uint16_t TotalResponseSize;
361
362

	/* Add an inner Data Element Sequence header for the current services's found Attributes */
363
	uint16_t* AttributeListSize = SDP_AddSequence16(BufferPos);
364
365
366
367
368

	/* 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];
369
370
371
		void*     AttributeValue;
		
		/* Look through the current service's attribute list, examining all the attributes */
372
		while ((AttributeValue = pgm_read_ptr(&AttributeTable->Data)) != NULL)
373
		{
374
375
376
377
378
379
380
381
382
383
384
			/* 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++;
385
386
387
		}
	}

388
389
390
	/* Record the total number of added bytes to the buffer */
	TotalResponseSize = 3 + *AttributeListSize;

391
392
393
394
395
396
	/* Fix endianness of the added attribute data element sequence */
	*AttributeListSize = SwapEndian_16(*AttributeListSize);

	return TotalResponseSize;
}

397
398
399
400
401
402
403
404
405
406
407
/** 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 */
408
	uint8_t  AttributeHeaderLength;
409
	uint16_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
410
	
411
412
	BT_SDP_DEBUG(2, " -- Add Attribute (0x%04X) 0x%04X", (AttributeHeaderLength + AttributeValueLength), AttributeID);

413
	/* Add a Data Element header to the response for the Attribute ID */
414
	SDP_WriteData8(ResponseBuffer, (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit));
415
416
	
	/* Add the Attribute ID to the created Data Element */
417
	SDP_WriteData16(ResponseBuffer, AttributeID);
418
419
	
	/* Copy over the Attribute value Data Element container to the response */
420
421
	memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
	*ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
422
	
423
	return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
424
425
}

426
427
428
429
430
431
432
433
/** 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)
434
{
435
	void* CurrTableItemData;
436
437
	
	/* Search through the current Attribute table, abort when the terminator item has been reached */
438
	while ((CurrTableItemData = pgm_read_ptr(&AttributeTable->Data)) != NULL)
439
	{
440
		/* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
441
		if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
442
		  return CurrTableItemData;
443
444
445
446
447
448
449
		
		AttributeTable++;
	}
			
	return NULL;
}

450
/** Retrieves the Attribute table for the given UUID list if it exists.
451
 *
452
453
454
 *  \param[in] UUIDList            List of UUIDs which must be matched within the service attribute table
 *  \param[in] TotalUUIDs          Total number of UUIDs stored in the UUID list
 *  \param[in] CurrAttributeTable  Pointer to the service attribute table to search through
455
 *
456
 *  \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise
457
 */
458
459
static bool SDP_SearchServiceTable(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs,
			                       const ServiceAttributeTable_t* CurrAttributeTable)
460
{
461
462
463
464
465
466
467
468
469
	bool UUIDMatch[TotalUUIDs];	
	
	/* Set all the match flags to false (not matched) before starting the search */
	memset(UUIDMatch, false, sizeof(UUIDMatch));

	const void* CurrAttribute;
	
	/* Search through the current attribute table, checking each attribute value for UUID matches */
	while ((CurrAttribute = pgm_read_ptr(&CurrAttributeTable->Data)) != NULL)
470
	{
471
472
473
474
475
476
477
478
479
480
481
		SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute);
		CurrAttributeTable++;
	}

	/* Determine how many UUID matches in the list we have found */
	uint8_t UUIDMatches = 0;
	for (uint8_t i = 0; i < TotalUUIDs; i++)
	{
		if (UUIDMatch[i])
		  UUIDMatches++;
	}
482
	
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
	/* If all UUIDs have been matched to the current service, return true */
	return (UUIDMatches == TotalUUIDs);
}

/** Recursively upwraps the given locally stored attribute (in PROGMEM space), searching for UUIDs to match against
 *  the given UUID list. As matches are found, they are indicated in the UUIDMatch flag list.
 *
 *  \param[in]      UUIDList       List of UUIDs which must be matched within the service attribute table
 *  \param[in]      TotalUUIDs     Total number of UUIDs stored in the UUID list
 *  \param[in, out] UUIDMatch      Array of flags indicating which UUIDs in the list have already been matched
 *  \param[in]      CurrAttribute  Pointer to the current attribute to search through
 *
 *  \return True if all the UUIDs given in the UUID list appear in the given attribute table, false otherwise
 */
static void SDP_CheckUUIDMatch(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, bool UUIDMatch[],
                               const void* CurrAttribute)
{
	uint8_t CurrAttributeType = (pgm_read_byte(CurrAttribute) & ~0x07);

	/* Check the data type of the current attribute value - if UUID, compare, if Sequence, unwrap and recurse */
	if (CurrAttributeType == SDP_DATATYPE_UUID)
	{
		/* Look for matches in the UUID list against the current attribute UUID value */
		for (uint8_t i = 0; i < TotalUUIDs; i++)
		{
508
			/* Check if the current unmatched UUID is identical to the search UUID */
509
510
511
512
513
514
515
516
517
518
519
520
			if (!(UUIDMatch[i]) && !(memcmp_P(UUIDList[i], (CurrAttribute + 1), UUID_SIZE_BYTES)))
			{
				/* Indicate match found for the current attribute UUID and early-abort */
				UUIDMatch[i] = true;
				break;
			}
		}
	}
	else if (CurrAttributeType == SDP_DATATYPE_Sequence)
	{
		uint8_t  SequenceHeaderSize;
		uint16_t SequenceSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &SequenceHeaderSize);
521
		
522
		CurrAttribute += SequenceHeaderSize;
523
		
524
525
		/* Recursively unwrap the sequence container, and re-search its contents for UUIDs */
		while (SequenceSize)
526
		{
527
528
529
530
531
532
533
534
535
			uint8_t  InnerHeaderSize;
			uint16_t InnerSize = SDP_GetLocalAttributeContainerSize(CurrAttribute, &InnerHeaderSize);
			
			SDP_CheckUUIDMatch(UUIDList, TotalUUIDs, UUIDMatch, CurrAttribute);
						
			SequenceSize  -= InnerHeaderSize + InnerSize;
			CurrAttribute += InnerHeaderSize + InnerSize;
		}
	}	
536
537
}

538
539
540
541
542
543
544
545
546
/** 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)
547
{
548
549
	uint8_t ElementHeaderSize;
	uint8_t TotalAttributes = 0;
550

551
552
	/* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
	uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
553
554
555
	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
	while (AttributeIDListLength)
	{
556
557
558
		/* 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);
559
		
560
561
		/* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
		memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
562
		
563
		/* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
564
		if (AttributeLength == 2)
565
566
567
568
569
		  CurrentAttributeRange[1] = CurrentAttributeRange[0];

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

571
		BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
572
		
573
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
574
		*CurrentParameter     += AttributeLength;
575
	}
576
	
577
	return TotalAttributes;
578
579
}

580
581
582
583
584
585
586
587
588
/** 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)
589
590
591
592
{
	uint8_t ElementHeaderSize;
	uint8_t TotalUUIDs = 0;

593
594
	/* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
	uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
595
596
597
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
	while (ServicePatternLength)
	{
598
		/* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
599
		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
600
		uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
601
		
602
		/* Copy over UUID from the container to the free slot */
603
604
605
606
607
608
609
610
611
612
613
614
615
		if (UUIDLength <= 4)
		{
			/* Copy over the base UUID value to the free UUID slot in the list */
			memcpy_P(CurrentUUID, &BaseUUID, sizeof(BaseUUID));

			/* Copy over short UUID */
			memcpy(CurrentUUID + (4 - UUIDLength), *CurrentParameter, UUIDLength);
		}
		else
		{
			/* Copy over full UUID */
			memcpy(CurrentUUID, *CurrentParameter, UUIDLength);		
		}
616
		
617
		BT_SDP_DEBUG(2, "-- UUID (%d): %02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
618
		                UUIDLength,
619
620
		                CurrentUUID[0], CurrentUUID[1], CurrentUUID[2], CurrentUUID[3],
		                CurrentUUID[4], CurrentUUID[5],
621
622
623
						CurrentUUID[6], CurrentUUID[7],
		                CurrentUUID[8], CurrentUUID[9],
						CurrentUUID[10], CurrentUUID[11], CurrentUUID[12],  CurrentUUID[13],  CurrentUUID[14],  CurrentUUID[15]);
624
625
626
627
628
629
630
631

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

632
633
/** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 *
634
635
 *  \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
636
637
638
 *
 *  \return Size in bytes of the entire attribute container, including the header
 */
639
static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
640
641
642
643
{
	/* Fetch the size of the Data Element structure from the header */
	uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
	
644
645
	uint32_t ElementValueSize;

646
647
648
	/* Convert the Data Element size index into a size in bytes */
	switch (SizeIndex)
	{
649
		case SDP_DATASIZE_Variable8Bit:
650
			*HeaderSize = (1 + sizeof(uint8_t));
651
652
			ElementValueSize = pgm_read_byte(AttributeData + 1);
			break;
653
		case SDP_DATASIZE_Variable16Bit:
654
			*HeaderSize = (1 + sizeof(uint16_t));
655
656
			ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
			break;
657
		case SDP_DATASIZE_Variable32Bit:
658
			*HeaderSize = (1 + sizeof(uint32_t));
659
660
			ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
			break;
661
		default:
662
			*HeaderSize = 1;
663
664
			ElementValueSize = (1 << SizeIndex);
			break;
665
666
	}

667
	return ElementValueSize;
668
669
}

670
671
672
673
674
675
676
677
678
/** 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)
679
{
680
	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
681
682
	uint8_t  SizeIndex = (SDP_ReadData8(DataElementHeader) & 0x07);	

683
	uint32_t ElementValueSize;
684
685

	/* Convert the Data Element size index into a size in bytes */
686
687
	switch (SizeIndex)
	{
688
		case SDP_DATASIZE_Variable8Bit:
689
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
690
			ElementValueSize    = SDP_ReadData8(DataElementHeader);
691
			break;
692
		case SDP_DATASIZE_Variable16Bit:
693
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
694
			ElementValueSize    = SDP_ReadData16(DataElementHeader);
695
			break;
696
		case SDP_DATASIZE_Variable32Bit:
697
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
698
			ElementValueSize    = SDP_ReadData32(DataElementHeader);
699
			break;
700
		default:
701
			*ElementHeaderSize  = 1;
702
			ElementValueSize    = (1 << SizeIndex);
703
			break;
704
705
	}
	
706
	return ElementValueSize;
707
}