ServiceDiscoveryProtocol.c 24.8 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
/** Service Discovery Protocol attribute, indicating the service's availability. */
35
36
37
38
const struct
{
	uint8_t Header;
	uint8_t Data;
39
} PROGMEM SDP_Attribute_Availability = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_8Bit), 0xFF};
40
41
42
43

const struct
{
	uint8_t  Header;
44
	uint32_t Data;
45
} PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), 0x00010000};
46

47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
const struct
{
	uint8_t   Header;
	uint8_t   Size;
	Version_t VersionList[];
} PROGMEM SDP_Attribute_Version =
	{
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit),
		.Size   = (sizeof(Version_t) * 1),
		.VersionList =
			{
				{.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Version = 0x0100}
			}
	};

62
63
64
const struct
{
	uint8_t     Header;
65
	uint16_t    Size;
66
67
	ClassUUID_t UUIDList[];
} PROGMEM SDP_Attribute_ServiceClassIDs =
68
	{
69
70
71
72
73
74
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),
		.Size   = (sizeof(ClassUUID_t) * 1),
		.UUIDList =
			{
				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00}}
			}
75
76
	};

77
78
/** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
79
	{
80
81
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &SDP_Attribute_ServiceHandle    },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &SDP_Attribute_ServiceClassIDs  },
82
		{.AttributeID = SDP_ATTRIBUTE_ID_VERSION,             .Data = &SDP_Attribute_Version          },
83

84
85
		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
	};
86

87
88
89
/** Master service table, listing all supported services (and their attribute tables) of the device, including
 *  each service's UUID.
 */
90
const ServiceTable_t SDP_Services_Table[] PROGMEM =
91
	{
92
93
		{   // 128-bit UUID for the SDP service
			.UUID  = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x01},
94
			.AttributeTable = SDP_Attribute_Table,
95
96
97
		},
	};

98
/** Base UUID value common to all standardized Bluetooth services */
99
const uint8_t BaseUUID[] PROGMEM = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};
100

101

102
103
104
105
106
107
108
/** 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
 */
109
void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
110
111
112
113
114
115
116
117
118
119
120
{
	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:
121
			SDP_ProcessServiceSearch(SDPHeader, Channel);
122
123
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
124
			SDP_ProcessServiceAttribute(SDPHeader, Channel);
125
126
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
127
			SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
128
129
130
131
			break;
	}
}

132
133
134
135
136
137
/** 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)
138
{
139
140
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

141
	BT_SDP_DEBUG(1, "<< Service Search");
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210

	/* 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 */
	uint16_t MaxServiceRecordCount = SwapEndian_16(*((uint16_t*)CurrentParameter));
	CurrentParameter += sizeof(uint16_t);
	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++)
	{
		/* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
		ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
		
		/* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
		if (AttributeTable == NULL)
		  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 */
	*((uint8_t*)CurrResponsePos) = 0;

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

	/* Fill in the response packet's header */
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16((ResponsePacket.CurrentServiceRecordCount << 2) +
	                                                         sizeof(ResponsePacket.CurrentServiceRecordCount) +
	                                                         sizeof(ResponsePacket.TotalServiceRecordCount) +
	                                                         sizeof(uint8_t));

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

	/* Send the completed response packet to the sender */
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
	                     Channel);
211
212
}

213
214
215
216
217
218
/** 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)
219
220
221
222
{
	BT_SDP_DEBUG(1, "<< Service Attribute");
}

223
224
225
226
227
228
/** 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)
229
{
230
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
231
232
233
	
	BT_SDP_DEBUG(1, "<< Service Search Attribute");

234
	/* Retrieve the list of search UUIDs from the request */
235
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
236
	uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
237
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
238
	
239
	/* Retrieve the maximum Attribute reponse size from the request */
240
	uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
241
	CurrentParameter += sizeof(uint16_t);
242
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
243
	
244
	/* Retrieve the list of Attributes from the request */
245
	uint16_t AttributeList[15][2];
246
	uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
247
248
	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
	
249
250
251
	struct
	{
		SDP_PDUHeader_t SDPHeader;
252
		uint16_t        AttributeListByteCount;
253
254
255
		uint8_t         ResponseData[100];
	} ResponsePacket;
	
256
	/* Create a pointer to the buffer to indicate the current location for response data to be added */
257
	void* CurrResponsePos = ResponsePacket.ResponseData;
258

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

263
	/* Add the outer Data Element Sequence header for all of the retrieved Attributes */
264
265
266
	uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
	
	/* Search through the list of UUIDs one at a time looking for matching search Attributes */
267
268
	for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
	{
269
270
		/* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
		ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
271
		
272
		/* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
273
274
275
		if (AttributeTable == NULL)
		  continue;
		  
276
277
		BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);

278
279
280
281
		/* Add an inner Data Element Sequence header for the current UUID's found Attributes */
		uint16_t* CurrentUUIDResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
		
		/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
282
283
284
285
		for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
		{
			uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
		
286
			/* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
287
			for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
288
			{
289
290
				/* Retrieve a PROGMEM pointer to the value of the current Attribute ID, if it exists in the UUID's Attribute table */
				const void* AttributeValue = SDP_GetAttributeValue(AttributeTable, CurrAttributeID);
291
				
292
				/* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
293
294
				if (AttributeValue == NULL)
				  continue;
295
				
296
297
				BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);

298
				/* Increment the current UUID's returned Attribute container size by the number of added bytes */
299
				*CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
300
			}
301

302
			/* Increment the outer container size by the number of added bytes */
303
			*TotalResponseSize += 3 + *CurrentUUIDResponseSize;
304
		}
305
306
307

		/* Flip the endianness of the container's size */
		*CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize);
308
	}
309
310
311
	
	/* Continuation state - always zero */
	*((uint8_t*)CurrResponsePos) = 0;
312

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

	/* Fill in the response packet's header */
317
318
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
319
320
321
322
323
324
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(sizeof(ResponsePacket.AttributeListByteCount) + 
	                                                         (3 + *TotalResponseSize) +
	                                                         sizeof(uint8_t));

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

326
327
	BT_SDP_DEBUG(1, ">> Service Search Attribute Response");

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

333
334
335
336
337
338
339
340
341
342
343
/** 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 */
344
345
	uint8_t  AttributeHeaderLength;
	uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
346
347
348
349
350
351

	/* Add a Data Element header to the response for the Attribute ID */
	*((uint8_t*)*ResponseBuffer) = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit);
	*ResponseBuffer += sizeof(uint8_t);
	
	/* Add the Attribute ID to the created Data Element */
352
	*((uint16_t*)*ResponseBuffer) = SwapEndian_16(AttributeID);
353
354
355
	*ResponseBuffer += sizeof(uint16_t);
	
	/* Copy over the Attribute value Data Element container to the response */
356
	memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
357
	SwapEndian_n(*ResponseBuffer, AttributeHeaderLength);
358
	*ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
359
	
360
	return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
361
362
}

363
364
365
366
367
368
369
370
/** 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)
371
{
372
	void* CurrTableItemData;
373
374
	
	/* Search through the current Attribute table, abort when the terminator item has been reached */
375
	while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
376
	{
377
		/* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
378
		if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
379
		  return CurrTableItemData;
380
381
382
383
384
385
386
		
		AttributeTable++;
	}
			
	return NULL;
}

387
388
389
390
391
392
393
/** 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)
394
{
395
	/* Search through the global UUID list an item at a time */
396
397
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
	{
398
399
400
		/* 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);
	
401
		/* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
402
		if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
		  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;
		uint32_t ClassUUIDListSize = SDP_GetLocalAttributeContainerSize(ClassUUIDs, &ClassUUIDListHeaderSize);
		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 */
421
			if (!(memcmp_P(UUID, &((ClassUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
422
423
			  return CurrAttributeTable;
		
424
425
			ClassUUIDListSize -= sizeof(ClassUUID_t);
			ClassUUIDs        += sizeof(ClassUUID_t);
426
		}	
427
428
429
430
431
	}
	
	return NULL;
}

432
433
434
435
436
437
438
439
440
/** 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)
441
{
442
443
	uint8_t ElementHeaderSize;
	uint8_t TotalAttributes = 0;
444

445
446
	/* Retrieve the total size of the Attribute container, and unwrap the outer Data Element Sequence container */
	uint16_t AttributeIDListLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
447
448
449
	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
	while (AttributeIDListLength)
	{
450
451
452
		/* 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);
453
		
454
455
		/* Copy over the starting Attribute ID and (if it the current element is a range) the ending Attribute ID */
		memcpy(&CurrentAttributeRange[0], *CurrentParameter, AttributeLength);
456
		
457
		/* If the element is not an Attribute Range, copy over the starting ID to the ending ID to make a range of 1 */
458
		if (AttributeLength == 2)
459
460
461
462
463
		  CurrentAttributeRange[1] = CurrentAttributeRange[0];

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

465
		BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
466
		
467
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
468
		*CurrentParameter     += AttributeLength;
469
	}
470
	
471
	return TotalAttributes;
472
473
}

474
475
476
477
478
479
480
481
482
/** 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)
483
484
485
486
{
	uint8_t ElementHeaderSize;
	uint8_t TotalUUIDs = 0;

487
488
	/* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
	uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
489
490
491
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
	while (ServicePatternLength)
	{
492
		/* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
493
		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
494
		uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
495
		
496
		/* Copy over the base UUID value to the free UUID slot in the list */
497
		memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
498
499
500

		/* Copy over UUID from the container to the free slot - if a short UUID (<= 4 bytes) it replaces the lower
		   4 bytes of the base UUID, otherwise it replaces the UUID completely */
501
		memcpy(&CurrentUUID[(UUIDLength <= 4) ? (UUID_SIZE_BYTES - 4) : 0], *CurrentParameter, UUIDLength);
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
		
		BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
		                UUIDLength,
		                CurrentUUID[15], CurrentUUID[14], CurrentUUID[13], CurrentUUID[12],
		                CurrentUUID[11], CurrentUUID[10], CurrentUUID[9],  CurrentUUID[8],
		                CurrentUUID[7],  CurrentUUID[6],  CurrentUUID[5],  CurrentUUID[4],
		                CurrentUUID[3],  CurrentUUID[2],  CurrentUUID[1],  CurrentUUID[0]);

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

517
518
519
520
521
522
/** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
 *
 *  \param[in] AttributeData  Pointer to the start of the Attribute container, located in PROGMEM
 *
 *  \return Size in bytes of the entire attribute container, including the header
 */
523
static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
524
525
526
527
528
529
530
{
	/* Fetch the size of the Data Element structure from the header */
	uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
	
	/* Convert the Data Element size index into a size in bytes */
	switch (SizeIndex)
	{
531
		case SDP_DATASIZE_Variable8Bit:
532
533
			*HeaderSize = (1 + sizeof(uint8_t));
			return pgm_read_byte(AttributeData + 1);
534
		case SDP_DATASIZE_Variable16Bit:
535
536
			*HeaderSize = (1 + sizeof(uint16_t));
			return pgm_read_word(AttributeData + 1);
537
		case SDP_DATASIZE_Variable32Bit:
538
539
			*HeaderSize = (1 + sizeof(uint32_t));
			return pgm_read_dword(AttributeData + 1);
540
		default:
541
542
			*HeaderSize = 1;
			return (1 << SizeIndex);
543
544
545
546
547
	}

	return 0;
}

548
549
550
551
552
553
554
555
556
/** 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)
557
{
558
	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
559
560
561
	uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
	*DataElementHeader += sizeof(uint8_t);
	
562
	uint32_t ElementValueSize;
563
564

	/* Convert the Data Element size index into a size in bytes */
565
566
	switch (SizeIndex)
	{
567
		case SDP_DATASIZE_Variable8Bit:
568
			ElementValueSize    = *((uint8_t*)*DataElementHeader);
569
570
571
			*DataElementHeader += sizeof(uint8_t);
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
			break;
572
		case SDP_DATASIZE_Variable16Bit:
573
			ElementValueSize    = SwapEndian_16(*((uint16_t*)*DataElementHeader));
574
575
576
			*DataElementHeader += sizeof(uint16_t);
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
			break;
577
		case SDP_DATASIZE_Variable32Bit:
578
			ElementValueSize    = SwapEndian_32(*((uint32_t*)*DataElementHeader));
579
580
581
			*DataElementHeader += sizeof(uint32_t);
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
			break;
582
		default:
583
584
			ElementValueSize    = (1 << SizeIndex);
			*ElementHeaderSize  = 1;
585
			break;
586
587
	}
	
588
	return ElementValueSize;
589
}