ServiceDiscoveryProtocol.c 27.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
35
const struct
{
36
37
38
	uint8_t  Header;
	uint32_t Data;
} PROGMEM SDP_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), SWAPENDIAN_32(0x00010000)};
39
40
41

const struct
{
42
43
44
45
46
47
48
49
50
51
52
53
	uint8_t     Header;
	uint16_t    Size;
	ClassUUID_t UUIDList[];
} PROGMEM SDP_Attribute_ServiceClassIDs =
	{
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),
		.Size   = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1),
		.UUIDList =
			{
				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x00, 0x10, 0x00, 0x00}}
			}
	};
54

55
56
const struct
{
57
58
59
	uint8_t     Header;
	uint8_t     Size;
	Item16Bit_t VersionList[];
60
61
62
} PROGMEM SDP_Attribute_Version =
	{
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit),
63
		.Size   = (sizeof(Item16Bit_t) * 1),
64
65
		.VersionList =
			{
66
				{.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Value = SWAPENDIAN_16(0x0100)}
67
68
69
			}
	};

70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
const struct
{
	uint8_t     Header;
	uint8_t     Size;
	Item16Bit_t OffsetList[];
} PROGMEM SDP_Attribute_LangOffset =
	{
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable8Bit),
		.Size   = (sizeof(Item16Bit_t) * 1),
		.OffsetList =
			{
				{.Header = (SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_16Bit), .Value = SWAPENDIAN_16(0x0100)}
			}
	};

const struct
{
	uint8_t     Header;
	uint8_t     Size;
	char        Text[];
} PROGMEM SDP_Attribute_ServiceName =
	{
		.Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit),
		.Size   = sizeof("SDP") - 1,
		.Text   = "SDP",
	};

const struct
{
	uint8_t     Header;
	uint8_t     Size;
	char        Text[];
} PROGMEM SDP_Attribute_ServiceDescription =
	{
		.Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit),
		.Size   = sizeof("Service Discovery Protocol Server") - 1,
		.Text   = "Service Discovery Protocol Server",
	};

/** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
	{
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &SDP_Attribute_ServiceHandle      },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &SDP_Attribute_ServiceClassIDs    },
		{.AttributeID = SDP_ATTRIBUTE_ID_VERSION,             .Data = &SDP_Attribute_Version            },
		{.AttributeID = SDP_ATTRIBUTE_ID_LANGIDOFFSET,        .Data = &SDP_Attribute_LangOffset         },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICENAME,         .Data = &SDP_Attribute_ServiceName        },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICEDESCRIPTION,  .Data = &SDP_Attribute_ServiceDescription },

		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
	};

const struct
{
	uint8_t  Header;
	uint32_t Data;
} PROGMEM RFCOMM_Attribute_ServiceHandle = {(SDP_DATATYPE_UnsignedInt | SDP_DATASIZE_32Bit), SWAPENDIAN_32(0x00010001)};

128
129
130
const struct
{
	uint8_t     Header;
131
	uint16_t    Size;
132
	ClassUUID_t UUIDList[];
133
} PROGMEM RFCOMM_Attribute_ServiceClassIDs =
134
	{
135
		.Header = (SDP_DATATYPE_Sequence | SDP_DATASIZE_Variable16Bit),
136
		.Size   = SWAPENDIAN_16(sizeof(ClassUUID_t) * 1),
137
138
		.UUIDList =
			{
139
				{.Header = (SDP_DATATYPE_UUID | SDP_DATASIZE_128Bit), .UUID = {BASE_96BIT_UUID, 0x01, 0x11, 0x00, 0x00}}
140
			}
141
142
	};

143
144
145
146
147
148
const struct
{
	uint8_t     Header;
	uint8_t     Size;
	char        Text[];
} PROGMEM RFCOMM_Attribute_ServiceName =
149
	{
150
151
152
153
		.Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit),
		.Size   = sizeof("Serial Port") - 1,
		.Text   = "Serial Port",
	};
154

155
156
157
158
159
160
161
162
163
164
const struct
{
	uint8_t     Header;
	uint8_t     Size;
	char        Text[];
} PROGMEM RFCOMM_Attribute_ServiceDescription =
	{
		.Header = (SDP_DATATYPE_String | SDP_DATASIZE_Variable8Bit),
		.Size   = sizeof("Wireless Serial Port Service") - 1,
		.Text   = "Wireless Serial Port Service",
165
	};
166

167
168
169
170
171
172
173
174
175
176
const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM =
	{
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE, .Data = &RFCOMM_Attribute_ServiceHandle      },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICECLASSIDS,     .Data = &RFCOMM_Attribute_ServiceClassIDs    },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICENAME,         .Data = &RFCOMM_Attribute_ServiceName        },
		{.AttributeID = SDP_ATTRIBUTE_ID_SERVICEDESCRIPTION,  .Data = &RFCOMM_Attribute_ServiceDescription },

		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
	};
	
177
178
179
/** Master service table, listing all supported services (and their attribute tables) of the device, including
 *  each service's UUID.
 */
180
const ServiceTable_t SDP_Services_Table[] PROGMEM =
181
	{
182
		{   // 128-bit UUID for the SDP service
183
			.UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
184
			.AttributeTable = SDP_Attribute_Table,
185
		},
186
187
188
189
		{   // 128-bit UUID for the RFCOMM service
			.UUID  = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00},
			.AttributeTable = RFCOMM_Attribute_Table,
		},
190
191
	};

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

195

196
197
198
199
200
201
202
/** 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
 */
203
void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
204
205
206
207
208
209
210
211
212
213
214
{
	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:
215
			SDP_ProcessServiceSearch(SDPHeader, Channel);
216
217
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
218
			SDP_ProcessServiceAttribute(SDPHeader, Channel);
219
220
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
221
			SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
222
223
224
225
			break;
	}
}

226
227
228
229
230
231
/** 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)
232
{
233
234
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));

235
	BT_SDP_DEBUG(1, "<< Service Search");
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291

	/* 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;

292
293
294
295
296
297
298
	/* 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);

299
300
301
	/* Fill in the response packet's header */
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHRESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
302
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
303
304
305
306

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

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

310
311
312
313
314
315
/** 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)
316
317
318
319
{
	BT_SDP_DEBUG(1, "<< Service Attribute");
}

320
321
322
323
324
325
/** 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)
326
{
327
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
328
329
330
	
	BT_SDP_DEBUG(1, "<< Service Search Attribute");

331
	/* Retrieve the list of search UUIDs from the request */
332
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
333
	uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
334
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
335
	
336
	/* Retrieve the maximum Attribute reponse size from the request */
337
	uint16_t MaxAttributeSize = SwapEndian_16(*((uint16_t*)CurrentParameter));
338
	CurrentParameter += sizeof(uint16_t);
339
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
340
	
341
	/* Retrieve the list of Attributes from the request */
342
	uint16_t AttributeList[15][2];
343
	uint8_t  TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
344
345
	BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
	
346
347
348
	struct
	{
		SDP_PDUHeader_t SDPHeader;
349
		uint16_t        AttributeListByteCount;
350
351
352
		uint8_t         ResponseData[100];
	} ResponsePacket;
	
353
	/* Create a pointer to the buffer to indicate the current location for response data to be added */
354
	void* CurrResponsePos = ResponsePacket.ResponseData;
355

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

360
	/* Add the outer Data Element Sequence header for all of the retrieved Attributes */
361
362
363
	uint16_t* TotalResponseSize = SDP_AddDataElementHeader16(&CurrResponsePos, SDP_DATATYPE_Sequence);
	
	/* Search through the list of UUIDs one at a time looking for matching search Attributes */
364
365
	for (uint8_t CurrUUIDItem = 0; CurrUUIDItem < TotalUUIDs; CurrUUIDItem++)
	{
366
367
		/* Retrieve the attribute table of the current search UUID from the global UUID table if it exists */
		ServiceAttributeTable_t* AttributeTable = SDP_GetAttributeTable(UUIDList[CurrUUIDItem]);
368
		
369
		/* If the UUID does not exist in the global UUID table, continue on to the next search UUID */
370
371
372
		if (AttributeTable == NULL)
		  continue;
		  
373
374
		BT_SDP_DEBUG(2, " -- Found UUID %d in table", CurrUUIDItem);

375
376
377
378
		/* 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 */
379
380
381
382
		for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
		{
			uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
		
383
			/* Look in the current Attribute Range for a matching Attribute ID in the UUID's Attribute table */
384
			for (uint32_t CurrAttributeID = AttributeIDRange[0]; CurrAttributeID <= AttributeIDRange[1]; CurrAttributeID++)
385
			{
386
387
				/* 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);
388
				
389
				/* If the Attribute does not exist in the current UUID's Attribute table, continue to the next Attribute ID */
390
391
				if (AttributeValue == NULL)
				  continue;
392
				
393
394
				BT_SDP_DEBUG(2, " -- Add Attribute 0x%04X", CurrAttributeID);

395
				/* Increment the current UUID's returned Attribute container size by the number of added bytes */
396
				*CurrentUUIDResponseSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, &CurrResponsePos);
397
			}
398

399
			/* Increment the outer container size by the number of added bytes */
400
			*TotalResponseSize += 3 + *CurrentUUIDResponseSize;
401
		}
402
403
404

		/* Flip the endianness of the container's size */
		*CurrentUUIDResponseSize = SwapEndian_16(*CurrentUUIDResponseSize);
405
	}
406
407
408
	
	/* Continuation state - always zero */
	*((uint8_t*)CurrResponsePos) = 0;
409

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

413
414
415
416
417
418
	/* 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));

419
	/* Fill in the response packet's header */
420
421
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
422
	ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
423
424
425

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

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

430
	/* Send the completed response packet to the sender */
431
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
432
433
}

434
435
436
437
438
439
440
441
442
443
444
/** 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 */
445
446
	uint8_t  AttributeHeaderLength;
	uint32_t AttributeValueLength = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttributeHeaderLength);
447
	
448
449
450
451
452
	/* 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 */
453
	*((uint16_t*)*ResponseBuffer) = SwapEndian_16(AttributeID);
454
455
456
	*ResponseBuffer += sizeof(uint16_t);
	
	/* Copy over the Attribute value Data Element container to the response */
457
458
	memcpy_P(*ResponseBuffer, AttributeValue, AttributeHeaderLength + AttributeValueLength);
	*ResponseBuffer += AttributeHeaderLength + AttributeValueLength;
459
	
460
	return (sizeof(uint8_t) + sizeof(uint16_t) + AttributeHeaderLength + AttributeValueLength);
461
462
}

463
464
465
466
467
468
469
470
/** 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)
471
{
472
	void* CurrTableItemData;
473
474
	
	/* Search through the current Attribute table, abort when the terminator item has been reached */
475
	while ((CurrTableItemData = (void*)pgm_read_word(&AttributeTable->Data)) != NULL)
476
	{
477
		/* Check if the current Attribute ID matches the search ID - if so return a pointer to it */
478
		if (pgm_read_word(&AttributeTable->AttributeID) == AttributeID)
479
		  return CurrTableItemData;
480
481
482
483
484
485
486
		
		AttributeTable++;
	}
			
	return NULL;
}

487
488
489
490
491
492
493
/** 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)
494
{
495
	/* Search through the global UUID list an item at a time */
496
497
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
	{
498
499
500
		/* 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);
	
501
		/* If the current table item's UUID matches the search UUID, return a pointer the table item's Attribute table */
502
		if (!(memcmp_P(UUID, SDP_Services_Table[CurrTableItem].UUID, UUID_SIZE_BYTES)))
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
		  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 */
521
			if (!(memcmp_P(UUID, &((ClassUUID_t*)ClassUUIDs)->UUID, UUID_SIZE_BYTES)))
522
523
			  return CurrAttributeTable;
		
524
525
			ClassUUIDListSize -= sizeof(ClassUUID_t);
			ClassUUIDs        += sizeof(ClassUUID_t);
526
		}	
527
528
529
530
531
	}
	
	return NULL;
}

532
533
534
535
536
537
538
539
540
/** 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)
541
{
542
543
	uint8_t ElementHeaderSize;
	uint8_t TotalAttributes = 0;
544

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

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

565
		BT_SDP_DEBUG(2, "-- Attribute: 0x%04X-0x%04X", CurrentAttributeRange[0], CurrentAttributeRange[1]);
566
		
567
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
568
		*CurrentParameter     += AttributeLength;
569
	}
570
	
571
	return TotalAttributes;
572
573
}

574
575
576
577
578
579
580
581
582
/** 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)
583
584
585
586
{
	uint8_t ElementHeaderSize;
	uint8_t TotalUUIDs = 0;

587
588
	/* Retrieve the total size of the UUID container, and unwrap the outer Data Element Sequence container */
	uint16_t ServicePatternLength = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
589
590
591
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
	while (ServicePatternLength)
	{
592
		/* Retrieve the size of the next UUID in the container and get a pointer to the next free UUID element in the list */
593
		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
594
		uint8_t  UUIDLength  = SDP_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
595
		
596
		/* Copy over the base UUID value to the free UUID slot in the list */
597
		memcpy_P(CurrentUUID, BaseUUID, sizeof(BaseUUID));
598
599
600

		/* 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 */
601
602
603
604
605
606
607
608
609
		if (UUIDLength <= 4)
		{
			memcpy(&CurrentUUID[UUID_SIZE_BYTES - 4], *CurrentParameter, UUIDLength);
			SwapEndian_n(&CurrentUUID[UUID_SIZE_BYTES - 4], UUIDLength);				
		}
		else
		{
			memcpy(&CurrentUUID[0], *CurrentParameter, UUIDLength);		
		}
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
		
		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;
}

625
626
627
628
629
630
/** 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
 */
631
static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData, uint8_t* const HeaderSize)
632
633
634
635
636
637
638
{
	/* 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)
	{
639
		case SDP_DATASIZE_Variable8Bit:
640
641
			*HeaderSize = (1 + sizeof(uint8_t));
			return pgm_read_byte(AttributeData + 1);
642
		case SDP_DATASIZE_Variable16Bit:
643
			*HeaderSize = (1 + sizeof(uint16_t));
644
			return SwapEndian_16(pgm_read_word(AttributeData + 1));
645
		case SDP_DATASIZE_Variable32Bit:
646
			*HeaderSize = (1 + sizeof(uint32_t));
647
			return SwapEndian_32(pgm_read_dword(AttributeData + 1));
648
		default:
649
650
			*HeaderSize = 1;
			return (1 << SizeIndex);
651
652
653
654
655
	}

	return 0;
}

656
657
658
659
660
661
662
663
664
/** 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)
665
{
666
	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
667
668
669
	uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
	*DataElementHeader += sizeof(uint8_t);
	
670
	uint32_t ElementValueSize;
671
672

	/* Convert the Data Element size index into a size in bytes */
673
674
	switch (SizeIndex)
	{
675
		case SDP_DATASIZE_Variable8Bit:
676
			ElementValueSize    = *((uint8_t*)*DataElementHeader);
677
678
679
			*DataElementHeader += sizeof(uint8_t);
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
			break;
680
		case SDP_DATASIZE_Variable16Bit:
681
			ElementValueSize    = SwapEndian_16(*((uint16_t*)*DataElementHeader));
682
683
684
			*DataElementHeader += sizeof(uint16_t);
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
			break;
685
		case SDP_DATASIZE_Variable32Bit:
686
			ElementValueSize    = SwapEndian_32(*((uint32_t*)*DataElementHeader));
687
688
689
			*DataElementHeader += sizeof(uint32_t);
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
			break;
690
		default:
691
692
			ElementValueSize    = (1 << SizeIndex);
			*ElementHeaderSize  = 1;
693
			break;
694
695
	}
	
696
	return ElementValueSize;
697
}