BluetoothACLPackets.c 16.1 KB
Newer Older
1
2
/*
             LUFA Library
Dean Camera's avatar
Dean Camera committed
3
     Copyright (C) Dean Camera, 2010.
4
5
6
7
8
9
              
  dean [at] fourwalledcubicle [dot] com
      www.fourwalledcubicle.com
*/

/*
Dean Camera's avatar
Dean Camera committed
10
  Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)
11

12
13
14
15
16
17
18
  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 
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
  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_BLUETOOTH_ACLPACKETS_C
#include "BluetoothACLPackets.h"

void Bluetooth_ProcessACLPackets(void)
{
36
	Bluetooth_ACL_Header_t        ACLPacketHeader;
37
	Bluetooth_DataPacket_Header_t DataHeader;
38
39
40
41
42
43
44
45
46
47
48
49
50

	Pipe_SelectPipe(BLUETOOTH_DATA_IN_PIPE);
	Pipe_Unfreeze();
	
	if (!(Pipe_IsReadWriteAllowed()))
	{
		Pipe_Freeze();
		return;
	}
	  
	Pipe_Read_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));
	Pipe_Read_Stream_LE(&DataHeader, sizeof(DataHeader));

51
#if (ACL_DEBUG_LEVEL > 1)
52
53
54
55
56
	BT_ACL_DEBUG("Packet Received", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader.DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader.DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader.PayloadLength);
57
#endif
58

59
	if (DataHeader.DestinationChannel == BT_CHANNEL_SIGNALING)
60
61
62
63
64
65
	{
		Bluetooth_SignalCommand_Header_t SignalCommandHeader;
		Pipe_Read_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader));

		switch (SignalCommandHeader.Code)
		{
66
			case BT_SIGNAL_CONNECTION_REQUEST:
67
				Bluetooth_SignalPacket_ConnectionRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);
68
				break;
69
			case BT_SIGNAL_CONFIGURATION_REQUEST:
70
71
				Bluetooth_SignalPacket_ConfigurationRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);
				break;
72
			case BT_SIGNAL_DISCONNECTION_REQUEST:
73
74
				Bluetooth_SignalPacket_DisconnectionRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);
				break;			
75
			case BT_SIGNAL_ECHO_REQUEST:
76
				Bluetooth_SignalPacket_EchoRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);
77
				break;
78
79
			case BT_SIGNAL_INFORMATION_REQUEST:
				Bluetooth_SignalPacket_InformationRequest(&ACLPacketHeader, &DataHeader, &SignalCommandHeader);
80
				break;
81
			default:
82
				#if (ACL_DEBUG_LEVEL > 0)
83
				BT_ACL_DEBUG("<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);
84
85
				#endif
	
86
87
88
89
90
91
92
93
94
95
96
97
				Pipe_Discard_Stream(ACLPacketHeader.DataLength);
				Pipe_ClearIN();		
				Pipe_Freeze();
				break;
		}
	}
	else
	{
		uint8_t DataPayload[DataHeader.PayloadLength];
		Pipe_Read_Stream_LE(&DataPayload, sizeof(DataPayload));
		DataHeader.PayloadLength = 0;
	
98
		BT_ACL_DEBUG("-- Data Payload: ", NULL);
99
100
		for (uint16_t B = 0; B < sizeof(DataPayload); B++)
		  printf("0x%02X ", DataPayload[B]);
101
		printf("\r\n");
102
103
104
105
106
107
108

		Pipe_Discard_Stream(ACLPacketHeader.DataLength);
		Pipe_ClearIN();		
		Pipe_Freeze();
	}
}

109
110
111
static inline void Bluetooth_SignalPacket_ConnectionRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,
                                                            Bluetooth_DataPacket_Header_t* DataHeader,
                                                            Bluetooth_SignalCommand_Header_t* SignalCommandHeader)
112
113
114
115
116
{
	Bluetooth_SignalCommand_ConnectionRequest_t ConnectionRequest;
	
	Pipe_Read_Stream_LE(&ConnectionRequest, sizeof(ConnectionRequest));

117
#if (ACL_DEBUG_LEVEL > 0)
118
	BT_ACL_DEBUG("<< L2CAP Connection Request", NULL);
119
#endif
120
#if (ACL_DEBUG_LEVEL > 1)
121
122
	BT_ACL_DEBUG("-- PSM: 0x%04X", ConnectionRequest.PSM);
	BT_ACL_DEBUG("-- Source Channel: 0x%04X", ConnectionRequest.SourceChannel);
123
#endif
124
125
126
127
128
129
130
131
132
133
	
	Pipe_ClearIN();
	Pipe_Freeze();
	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
	Bluetooth_SignalCommand_ConnectionResponse_t ConnectionResponse;

	ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse);
	DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(ConnectionResponse);
134
135
	DataHeader->DestinationChannel        = BT_CHANNEL_SIGNALING;
	SignalCommandHeader->Code             = BT_SIGNAL_CONNECTION_RESPONSE;
136
137
138
139
	SignalCommandHeader->Length           = sizeof(ConnectionResponse);
	
	Bluetooth_Channel_t* ChannelData      = Bluetooth_InitChannelData(ConnectionRequest.SourceChannel, ConnectionRequest.PSM);
	
140
	ConnectionResponse.Result             = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES : BT_CONNECTION_SUCCESSFUL;
141
142
143
144
145
146
147
148
149
150
151
152
	ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;
	ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;
	ConnectionResponse.Status             = 0x00;

	Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));
	Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));
	Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));
	Pipe_Write_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse));
	
	Pipe_ClearOUT();		
	Pipe_Freeze();
	
153
#if (ACL_DEBUG_LEVEL > 1)
154
155
156
157
158
	BT_ACL_DEBUG("Packet Sent", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength);			
159
#endif
160
#if (ACL_DEBUG_LEVEL > 0)
161
	BT_ACL_DEBUG(">> L2CAP Connection Response", NULL);
162
#endif
163
#if (ACL_DEBUG_LEVEL > 1)
164
165
	BT_ACL_DEBUG("-- Source Channel: 0x%04X", ConnectionResponse.SourceChannel);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel);
166
#endif
167
168
}

169
170
171
static inline void Bluetooth_SignalPacket_ConfigurationRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,
                                                               Bluetooth_DataPacket_Header_t* DataHeader,
                                                               Bluetooth_SignalCommand_Header_t* SignalCommandHeader)
172
173
174
{
	Bluetooth_SignalCommand_ConfigurationRequest_t ConfigurationRequest;
	Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));
175
176
	
	// TODO: Process/Discard configuration options here
177

178
#if (ACL_DEBUG_LEVEL > 0)
179
	BT_ACL_DEBUG("<< L2CAP Configuration Request", NULL);
180
#endif
181
#if (ACL_DEBUG_LEVEL > 1)
182
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);
183
#endif
184
185
186
187
188
189
190
191
192
193
	
	Pipe_ClearIN();
	Pipe_Freeze();
	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
	Bluetooth_SignalCommand_ConfigurationResponse_t ConfigurationResponse;

	ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse);
	DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(ConfigurationResponse);
194
195
	DataHeader->DestinationChannel        = BT_CHANNEL_SIGNALING;
	SignalCommandHeader->Code             = BT_SIGNAL_CONFIGURATION_RESPONSE;
196
197
	SignalCommandHeader->Length           = sizeof(ConfigurationResponse);
	
198
	Bluetooth_Channel_t* ChannelData      = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);
199
200
201
202

	if (ChannelData != NULL)
	  ChannelData->State = Channel_Open;
	  
203
	// TODO: Add channel config data to the tail of ConfigurationResponse
204
205
206

	ConfigurationResponse.SourceChannel   = ChannelData->RemoteNumber;
	ConfigurationResponse.Flags           = 0x00;
207
	ConfigurationResponse.Result          = (ChannelData != NULL) ? BT_CONFIGURATION_SUCCESSFUL : BT_CONFIGURATION_REJECTED;
208
209
210
211
212
213

	Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));
	Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));
	Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));
	Pipe_Write_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse));
	
214
	Pipe_ClearOUT();
215
216
	Pipe_Freeze();
	
217
#if (ACL_DEBUG_LEVEL > 1)
218
219
220
221
222
	BT_ACL_DEBUG("Packet Sent", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength);			
223
#endif
224
#if (ACL_DEBUG_LEVEL > 0)
225
	BT_ACL_DEBUG(">> L2CAP Configuration Response", NULL);
226
227
228
229
#endif
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("-- Result: 0x%02X", ConfigurationResponse.Result);
#endif
230
}
231
232
233
234
235
236
237
238
239

static inline void Bluetooth_SignalPacket_DisconnectionRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,
                                                               Bluetooth_DataPacket_Header_t* DataHeader,
                                                               Bluetooth_SignalCommand_Header_t* SignalCommandHeader)
{
	Bluetooth_SignalCommand_DisconnectionRequest_t DisconnectionRequest;
	
	Pipe_Read_Stream_LE(&DisconnectionRequest, sizeof(DisconnectionRequest));

240
#if (ACL_DEBUG_LEVEL > 0)
241
	BT_ACL_DEBUG("<< L2CAP Disconnection Request", NULL);
242
#endif
243
244
245
246
247
248
249
250
251
252
253
254
255
256
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DisconnectionRequest.DestinationChannel);
	BT_ACL_DEBUG("-- Source Channel: 0x%04X", DisconnectionRequest.SourceChannel);
#endif
	
	Pipe_ClearIN();
	Pipe_Freeze();
	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
	Bluetooth_SignalCommand_DisconnectionResponse_t DisconnectionResponse;

	ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(DisconnectionResponse);
	DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(DisconnectionResponse);
257
258
	DataHeader->DestinationChannel        = BT_CHANNEL_SIGNALING;
	SignalCommandHeader->Code             = BT_SIGNAL_DISCONNECTION_RESPONSE;
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
	SignalCommandHeader->Length           = sizeof(DisconnectionResponse);
	
	Bluetooth_Channel_t* ChannelData      = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, true);

	if (ChannelData != NULL)
	  ChannelData->State = Channel_Closed;
	
	DisconnectionResponse.DestinationChannel = ChannelData->LocalNumber;
	DisconnectionResponse.SourceChannel      = ChannelData->RemoteNumber;

	Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));
	Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));
	Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));
	Pipe_Write_Stream_LE(&DisconnectionResponse, sizeof(DisconnectionResponse));
	
	Pipe_ClearOUT();
	Pipe_Freeze();
	
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("Packet Sent", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength);			
#endif
284
#if (ACL_DEBUG_LEVEL > 0)
285
	BT_ACL_DEBUG(">> L2CAP Disconnection Response", NULL);
286
#endif
287
288
289
290
291
292
293
294
295
296
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("-- Source Channel: 0x%04X", DisconnectionResponse.SourceChannel);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DisconnectionResponse.DestinationChannel);
#endif
}

static inline void Bluetooth_SignalPacket_EchoRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,
                                                      Bluetooth_DataPacket_Header_t* DataHeader,
                                                      Bluetooth_SignalCommand_Header_t* SignalCommandHeader)
{
297
#if (ACL_DEBUG_LEVEL > 0)
298
	BT_ACL_DEBUG("<< L2CAP Echo Request", NULL);
299
#endif
300
301
302
303
304
305
306
307
	
	Pipe_ClearIN();
	Pipe_Freeze();
	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
	ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader);
	DataHeader->PayloadLength             = sizeof(*SignalCommandHeader);
308
309
	DataHeader->DestinationChannel        = BT_CHANNEL_SIGNALING;
	SignalCommandHeader->Code             = BT_SIGNAL_ECHO_RESPONSE;
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
	SignalCommandHeader->Length           = 0;
	
	Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));
	Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));
	Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));
	
	Pipe_ClearOUT();		
	Pipe_Freeze();
	
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("Packet Sent", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength);			
#endif
326
#if (ACL_DEBUG_LEVEL > 0)
327
	BT_ACL_DEBUG(">> L2CAP Echo Response", NULL);
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
#endif
}

static inline void Bluetooth_SignalPacket_InformationRequest(Bluetooth_ACL_Header_t* ACLPacketHeader,
                                                             Bluetooth_DataPacket_Header_t* DataHeader,
                                                             Bluetooth_SignalCommand_Header_t* SignalCommandHeader)
{
	Bluetooth_SignalCommand_InformationRequest_t InformationRequest;

	Pipe_Read_Stream_LE(&InformationRequest, sizeof(InformationRequest));

#if (ACL_DEBUG_LEVEL > 0)
	BT_ACL_DEBUG("<< Information Request", NULL);
#endif
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("-- Info Type: 0x%04X", InformationRequest.InfoType);
#endif
	
	Pipe_ClearIN();
	Pipe_Freeze();
	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
	Bluetooth_SignalCommand_InformationResponse_t InformationResponse;
	uint8_t ResponseData[4];
	uint8_t ResponseLen;

	switch (InformationRequest.InfoType)
	{
		case BT_INFOREQ_MTU:		
			InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;
			ResponseLen = 2;
			
			*((uint16_t*)&ResponseData) = 65533;
			break;
		case BT_INFOREQ_EXTENDEDFEATURES:
			InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;
			ResponseLen = 4;
			
			*((uint32_t*)&ResponseData) = 0;
			break;
		default:
			InformationResponse.Result = BT_INFORMATION_NOTSUPPORTED;
			ResponseLen = 0;
			break;
	}
	
	ACLPacketHeader->DataLength           = sizeof(*DataHeader) + sizeof(*SignalCommandHeader) + sizeof(InformationResponse) +
	                                        ResponseLen;
	DataHeader->PayloadLength             = sizeof(*SignalCommandHeader) + sizeof(InformationResponse) + ResponseLen;
	DataHeader->DestinationChannel        = BT_CHANNEL_SIGNALING;
	SignalCommandHeader->Code             = BT_SIGNAL_INFORMATION_RESPONSE;
	SignalCommandHeader->Length           = sizeof(InformationResponse) + ResponseLen;
	
	Pipe_Write_Stream_LE(ACLPacketHeader, sizeof(*ACLPacketHeader));
	Pipe_Write_Stream_LE(DataHeader, sizeof(*DataHeader));
	Pipe_Write_Stream_LE(SignalCommandHeader, sizeof(*SignalCommandHeader));
	Pipe_Write_Stream_LE(&InformationResponse, sizeof(InformationResponse));
	Pipe_Write_Stream_LE(ResponseData, ResponseLen);
	
	Pipe_ClearOUT();		
	Pipe_Freeze();

#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("Packet Sent", NULL);
	BT_ACL_DEBUG("-- Connection Handle: 0x%04X", (ACLPacketHeader->ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG("-- Data Length: 0x%04X", ACLPacketHeader->DataLength);
	BT_ACL_DEBUG("-- Destination Channel: 0x%04X", DataHeader->DestinationChannel);
	BT_ACL_DEBUG("-- Payload Length: 0x%04X", DataHeader->PayloadLength);			
#endif
#if (ACL_DEBUG_LEVEL > 0)
	BT_ACL_DEBUG(">> L2CAP Information Response", NULL);	
#endif
#if (ACL_DEBUG_LEVEL > 1)
	BT_ACL_DEBUG("-- Result: 0x%02X", InformationResponse.Result);
#endif
404
}