ServiceDiscoveryProtocol.c 10.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
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, indicationg the service's name. */
35
SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Name,         "SDP");
36
/** Service Discovery Protocol attribute, indicationg the service's description. */
37
SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Description,  "BT Service Discovery");
38
39
40
/** Service Discovery Protocol attribute, indicationg the service's availability. */
SERVICE_ATTRIBUTE_LEN8(SDP_Attribute_Availability, SDP_DATATYPE_UnsignedInt, 1, {0xFF});
/** Service Discovery Protocol attribute table, listing all supported attributes of the service. */
41
42
const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
	{
43
44
45
		{.AttributeID = SDP_ATTRIBUTE_NAME        , .Data = &SDP_Attribute_Name},
		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .Data = &SDP_Attribute_Description},
		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .Data = &SDP_Attribute_Availability},
46
47
48
		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
	};

49
/** RFCOMM Service attribute, indicationg the service's name. */
50
SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Name,         "RFCOMM");
51
/** RFCOMM Service attribute, indicationg the service's description. */
52
SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Description,  "Virtual Serial");
53
54
55
/** RFCOMM Service attribute, indicationg the service's availability. */
SERVICE_ATTRIBUTE_LEN8(RFCOMM_Attribute_Availability, SDP_DATATYPE_UnsignedInt, 1, {0xFF});
/** RFCOMM Service attribute table, listing all supported attributes of the service. */
56
57
const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM =
	{
58
59
60
		{.AttributeID = SDP_ATTRIBUTE_NAME        , .Data = &RFCOMM_Attribute_Name},
		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .Data = &RFCOMM_Attribute_Description},
		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .Data = &RFCOMM_Attribute_Availability},
61
62
		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
	};
63

64
65
66
/** Master service table, listing all supported services (and their attribute tables) of the device, including
 *  each service's UUID.
 */
67
68
69
70
71
72
73
74
75
76
77
78
const ServiceTable_t SDP_Services_Table[] =
	{
		{   // 128-bit UUID for the SDP service
			.UUID  = {BASE_96BIT_UUID, 0x01, 0x00, 0x00, 0x00},
			.AttributeTable = &SDP_Attribute_Table,
		},
		{   // 128-bit UUID for the RFCOMM service
			.UUID  = {BASE_96BIT_UUID, 0x03, 0x00, 0x00, 0x00},
			.AttributeTable = &RFCOMM_Attribute_Table,
		},
	};

79
/* Base UUID value common to all standardized Bluetooth services */
80
81
const uint8_t BaseUUID[] = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};

82
83
84
85
86
87
88
89
90
91
92
93
94

void ServiceDiscovery_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
{
	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:
95
			ServiceDiscovery_ProcessServiceSearch(SDPHeader, Channel);
96
97
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
98
			ServiceDiscovery_ProcessServiceAttribute(SDPHeader, Channel);
99
100
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
101
			ServiceDiscovery_ProcessServiceSearchAttribute(SDPHeader, Channel);
102
103
104
105
			break;
	}
}

106
static void ServiceDiscovery_ProcessServiceSearch(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
107
108
109
110
{
	BT_SDP_DEBUG(1, "<< Service Search");
}

111
static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
112
113
114
115
{
	BT_SDP_DEBUG(1, "<< Service Attribute");
}

116
static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader, Bluetooth_Channel_t* Channel)
117
{
118
	const void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
119
120
121
	
	BT_SDP_DEBUG(1, "<< Service Search Attribute");

122
	uint8_t UUIDList[12][UUID_SIZE_BYTES];
123
124
	uint8_t TotalUUIDs = ServiceDiscovery_GetUUIDList(UUIDList, &CurrentParameter);
	BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
125
126
127
128
	
	uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(&CurrentParameter);
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);

129
130
131
	struct
	{
		SDP_PDUHeader_t SDPHeader;
132
		uint16_t        AttributeListByteCount;
133
134
135
136
137
138
		uint8_t         ResponseData[100];
	} ResponsePacket;
	
	if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
	  MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
	  
139
	ResponsePacket.AttributeListByteCount    = ServiceDiscovery_ProcessAttributes(UUIDList, TotalUUIDs,
140
141
	                                                                              ResponsePacket.ResponseData,
	                                                                              MaxAttributeSize, &CurrentParameter);
142
143
144
	ResponsePacket.SDPHeader.PDU             = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
	ResponsePacket.SDPHeader.TransactionID   = SDPHeader->TransactionID;
	ResponsePacket.SDPHeader.ParameterLength = (ResponsePacket.AttributeListByteCount + sizeof(ResponsePacket.AttributeListByteCount));
145
146
147
148
149

	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ResponsePacket.SDPHeader.ParameterLength),
	                     Channel);
}

150
static uint8_t ServiceDiscovery_ProcessAttributes(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, uint8_t* ResponseBuffer,
151
152
                                                  uint8_t MaxResponseSize, const void** CurrentParameter)
{
153
154
	uint16_t* AttributeResponseSize = ServiceDiscovery_AddDataElementHeader(&ResponseBuffer, SDP_DATATYPE_UnsignedInt);
	uint8_t   ElementHeaderSize;
155
156

	uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
157
158
159
	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
	while (AttributeIDListLength)
	{
160
		uint8_t  AttributeLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
161
		uint32_t Attribute       = 0;
162
		
163
164
		memcpy(&((char*)&Attribute)[sizeof(uint32_t) - AttributeLength], *CurrentParameter, AttributeLength);

165
		BT_SDP_DEBUG(2, "-- Attribute(%d): 0x%08lX", AttributeLength, Attribute);
166
		
167
168
		*AttributeResponseSize += ServiceDiscovery_GetAttribute(UUIDList, TotalUUIDs, Attribute, &ResponseBuffer,
		                                                        (MaxResponseSize - *AttributeResponseSize));
169
170
	
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
171
		*CurrentParameter     += AttributeLength;
172
	}
173
	
174
	return *AttributeResponseSize;
175
176
}

177
static uint8_t ServiceDiscovery_GetAttribute(uint8_t UUIDList[][UUID_SIZE_BYTES], const uint8_t TotalUUIDs, const uint32_t Attribute,
178
179
180
                                             uint8_t** DataBuffer, uint8_t BufferLen)
{
	for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(ServiceTable_t)); CurrTableItem++)
181
	{
182
183
		for (uint8_t CurrUUIDIndex = 0; CurrUUIDIndex < TotalUUIDs; CurrUUIDIndex++)
		{
184
			if (!(memcmp(SDP_Services_Table[CurrTableItem].UUID, UUIDList[CurrUUIDIndex], UUID_SIZE_BYTES)))
185
186
187
			{
				const ServiceAttributeTable_t* AttributeTable = SDP_Services_Table[CurrTableItem].AttributeTable;

188
				// TODO: Process attribute table
189
190
191
192
193
194
195
196
				BT_SDP_DEBUG(2, "FOUND UUID IN TABLE");

				break;
			}
		}
	}
		
	return 0;
197
198
}

199
static uint8_t ServiceDiscovery_GetUUIDList(uint8_t UUIDList[][UUID_SIZE_BYTES], const void** CurrentParameter)
200
201
202
203
204
205
206
207
208
{
	uint8_t ElementHeaderSize;
	uint8_t TotalUUIDs = 0;

	uint16_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
	while (ServicePatternLength)
	{
		uint8_t* CurrentUUID = UUIDList[TotalUUIDs++];
209
		uint8_t  UUIDLength  = ServiceDiscovery_GetDataElementSize(CurrentParameter, &ElementHeaderSize);
210
211
		
		memcpy(CurrentUUID, BaseUUID, sizeof(BaseUUID));
212
		memcpy(&CurrentUUID[(UUIDLength <= 32) ? (sizeof(BaseUUID) - 32) : 0], *CurrentParameter, UUIDLength);
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
		
		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;
}

static uint32_t ServiceDiscovery_GetDataElementSize(const void** DataElementHeader, uint8_t* ElementHeaderSize)
229
{
230
	/* Fetch the size of the Data Element structure from the header, increment the current buffer pos */
231
232
233
234
	uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
	*DataElementHeader += sizeof(uint8_t);
	
	uint32_t ElementValue;
235
236

	/* Convert the Data Element size index into a size in bytes */
237
238
239
240
241
242
243
244
245
246
247
248
	switch (SizeIndex)
	{
		case 5:
			ElementValue = *((uint8_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint8_t);
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
			break;
		case 6:
			ElementValue = *((uint16_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint16_t);
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
			break;
249
		case 7:
250
251
252
253
			ElementValue = *((uint32_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint32_t);
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
			break;
254
		default:
255
256
			ElementValue = (1 << SizeIndex);
			*ElementHeaderSize = 1;
257
			break;
258
259
260
261
	}
	
	return ElementValue;
}