ServiceDiscoveryProtocol.c 6.88 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
#define  INCLUDE_FROM_SERVICEDISCOVERYPROTOCOL_C
32
33
#include "ServiceDiscoveryProtocol.h"

34
35
36
37
38
39
40
41
SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Name,             "SDP");
SERVICE_ATTRIBUTE_TEXT(SDP_Attribute_Description,      "BT Service Discovery");
SERVICE_ATTRIBUTE_8BIT_LEN(SDP_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
const ServiceAttributeTable_t SDP_Attribute_Table[] PROGMEM =
	{
		{.AttributeID = SDP_ATTRIBUTE_NAME        , .AttributeData = &SDP_Attribute_Name},
		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .AttributeData = &SDP_Attribute_Description},
		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .AttributeData = &SDP_Attribute_Availability},
42
		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
43
44
45
46
47
48
49
50
51
52
	};

SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Name,             "RFCOMM");
SERVICE_ATTRIBUTE_TEXT(RFCOMM_Attribute_Description,      "Virtual Serial");
SERVICE_ATTRIBUTE_8BIT_LEN(RFCOMM_Attribute_Availability, SDP_DATATYPE_UNSIGNED_INT, 1, {0xFF});
const ServiceAttributeTable_t RFCOMM_Attribute_Table[] PROGMEM =
	{
		{.AttributeID = SDP_ATTRIBUTE_NAME        , .AttributeData = &RFCOMM_Attribute_Name},
		{.AttributeID = SDP_ATTRIBUTE_DESCRIPTION , .AttributeData = &RFCOMM_Attribute_Description},
		{.AttributeID = SDP_ATTRIBUTE_AVAILABILITY, .AttributeData = &RFCOMM_Attribute_Availability},
53
		SERVICE_ATTRIBUTE_TABLE_TERMINATOR
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
	};
	
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,
		},
	};


69
void ServiceDiscovery_ProcessPacket(void* Data, Bluetooth_Channel_t* Channel)
70
71
72
73
{
	SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
	SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);

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

78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
	switch (SDPHeader->PDU)
	{
		case SDP_PDU_SERVICESEARCHREQUEST:
			ServiceDiscovery_ProcessServiceSearch(SDPHeader);
			break;		
		case SDP_PDU_SERVICEATTRIBUTEREQUEST:
			ServiceDiscovery_ProcessServiceAttribute(SDPHeader);
			break;
		case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
			ServiceDiscovery_ProcessServiceSearchAttribute(SDPHeader);
			break;
	}
}

static void ServiceDiscovery_ProcessServiceSearch(SDP_PDUHeader_t* SDPHeader)
{
94
	BT_SDP_DEBUG(1, "<< Service Search");
95
96
97
98
}

static void ServiceDiscovery_ProcessServiceAttribute(SDP_PDUHeader_t* SDPHeader)
{
99
	BT_SDP_DEBUG(1, "<< Service Attribute");
100
101
102
103
}

static void ServiceDiscovery_ProcessServiceSearchAttribute(SDP_PDUHeader_t* SDPHeader)
{
104
	void* CurrentParameter = ((void*)SDPHeader + sizeof(SDP_PDUHeader_t));
105
	
106
	BT_SDP_DEBUG(1, "<< Service Search Attribute");
107
108
	
	uint8_t ElementHeaderSize;
109

110
111
	uint16_t ServicePatternLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);
	BT_SDP_DEBUG(2, "-- Total UUID Length: 0x%04X", ServicePatternLength);
112
113
	while (ServicePatternLength)
	{
114
		uint8_t UUIDLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);
115
116
117
118
119
120
		uint8_t UUID[16] = {BASE_96BIT_UUID, 0x00, 0x00, 0x00, 0x00};

		if (UUIDLength <= 32)
		  memcpy(&UUID[sizeof(UUID) - sizeof(uint32_t)], CurrentParameter, UUIDLength);
		else
		  memcpy(UUID, CurrentParameter, UUIDLength);
121
		
122
		CurrentParameter += UUIDLength;
123
		
124
		BT_SDP_DEBUG(2, "-- UUID (%d): 0x%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
125
126
127
						UUIDLength,
		                UUID[15], UUID[14], UUID[13], UUID[12], UUID[11], UUID[10], UUID[9], UUID[8],
						UUID[7],  UUID[6],  UUID[5],  UUID[4],  UUID[3],  UUID[2],  UUID[1], UUID[0]);
128
	
129
		ServicePatternLength -= (UUIDLength + ElementHeaderSize);
130
131
	}
	
132
133
	uint16_t MaxAttributeSize = ServiceDiscovery_Read16BitParameter(&CurrentParameter);
	BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
134

135
136
	uint16_t AttributeIDListLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);
	BT_SDP_DEBUG(2, "-- Total Attribute Length: 0x%04X", AttributeIDListLength);
137
138
	while (AttributeIDListLength)
	{
139
140
		uint8_t  AttributeLength = ServiceDiscovery_GetDataElementSize(&CurrentParameter, &ElementHeaderSize);
		uint32_t Attribute = 0;
141
		
142
143
144
145
		memcpy(&Attribute, CurrentParameter, AttributeLength);
		CurrentParameter += AttributeLength;
		
		BT_SDP_DEBUG(2, "-- Attribute(%d): 0x%08lX", AttributeLength, Attribute);
146
	
147
		AttributeIDListLength -= (AttributeLength + ElementHeaderSize);
148
149
150
	}
}

151
static uint32_t ServiceDiscovery_GetDataElementSize(void** DataElementHeader, uint8_t* ElementHeaderSize)
152
{
153
154
155
156
157
158
159
	uint8_t SizeIndex = (*((uint8_t*)*DataElementHeader) & 0x07);
	*DataElementHeader += sizeof(uint8_t);
	
	*ElementHeaderSize = 1;
	
	uint32_t ElementValue;
	
160
161
162
	switch (SizeIndex)
	{
		case 0:
163
164
			ElementValue = 1;
			break;
165
		case 1:
166
167
			ElementValue = 2;
			break;
168
		case 2:
169
170
			ElementValue = 4;
			break;
171
		case 3:
172
173
			ElementValue = 8;
			break;
174
		case 4:
175
176
			ElementValue = 16;
			break;
177
		case 5:
178
179
180
181
			ElementValue = *((uint8_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint8_t);
			*ElementHeaderSize  = (1 + sizeof(uint8_t));
			break;
182
		case 6:
183
184
185
186
			ElementValue = *((uint16_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint16_t);
			*ElementHeaderSize  = (1 + sizeof(uint16_t));
			break;
187
		default:
188
189
190
191
			ElementValue = *((uint32_t*)*DataElementHeader);
			*DataElementHeader += sizeof(uint32_t);
			*ElementHeaderSize  = (1 + sizeof(uint32_t));
			break;
192
	}
193
194
	
	return ElementValue;
195
}