BluetoothACLPackets.c 29.7 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
  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"

34
35
36
/** Bluetooth ACL processing task. This task should be called repeatedly the main Bluetooth
 *  stack task to manage the ACL processing state.
 */
37
void Bluetooth_ACLTask(void)
38
{
39
40
	/* Process incomming ACL packets, if any */
	Bluetooth_ProcessIncommingACLPackets();
41
	
42
	/* Check for any half-open channels, send configuration details to the remote device if found */
43
44
45
46
47
48
	for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)
	{
		Bluetooth_Channel_t* ChannelData = &Bluetooth_Connection.Channels[i];
	
		bool MustSendConfigReq = true;
	
49
		/* Check if we are in a channel state which requires a configuration request to be sent */
50
51
52
53
54
55
56
57
58
59
60
61
62
		switch (ChannelData->State)
		{
			case Channel_Config_WaitConfig:
				ChannelData->State = Channel_Config_WaitReqResp;
				break;
			case Channel_Config_WaitSendConfig:
				ChannelData->State = Channel_Config_WaitResp;
				break;
			default:
				MustSendConfigReq  = false;
				break;
		}
		
63
		/* Only send a configuration request if it the channel was in a state which required it */
64
65
		if (MustSendConfigReq)
		{
66
67
68
69
70
71
72
73
74
75
76
			struct
			{
				BT_Signal_Header_t           SignalCommandHeader;
				BT_Signal_ConfigurationReq_t ConfigurationRequest;
				
				struct
				{
					BT_Config_Option_Header_t Header;
					uint16_t Value;
				} Option_LocalMTU;
			} PacketData;
77
			
78
			/* Fill out the Signal Command header in the response packet */
79
80
81
82
			PacketData.SignalCommandHeader.Code            = BT_SIGNAL_CONFIGURATION_REQUEST;
			PacketData.SignalCommandHeader.Identifier      = ++Bluetooth_Connection.SignallingIdentifier;
			PacketData.SignalCommandHeader.Length          = sizeof(PacketData.ConfigurationRequest) +
			                                                 sizeof(PacketData.Option_LocalMTU);
83
84

			/* Fill out the Configuration Request in the response packet, including local MTU information */
85
86
87
88
89
90
91
			PacketData.ConfigurationRequest.DestinationChannel = ChannelData->RemoteNumber;
			PacketData.ConfigurationRequest.Flags          = 0;
			PacketData.Option_LocalMTU.Header.Type         = BT_CONFIG_OPTION_MTU;
			PacketData.Option_LocalMTU.Header.Length       = sizeof(PacketData.Option_LocalMTU.Value);
			PacketData.Option_LocalMTU.Value               = ChannelData->LocalMTU;

			Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);
92
			
93
94
			BT_ACL_DEBUG(1, ">> L2CAP Configuration Request", NULL);
			BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.ConfigurationRequest.DestinationChannel);
95
96
97
98
		}
	}
}

99
100
101
102
103
/** Incomming ACL packet processing task. This task is called by the main ACL processing task to read in and process
 *  any incomming ACL packets to the device, handling signal requests as they are received or passing along channel
 *  data to the user application.
 */
static void Bluetooth_ProcessIncommingACLPackets(void)
104
105
106
{
	BT_ACL_Header_t        ACLPacketHeader;
	BT_DataPacket_Header_t DataHeader;
107
108
109
110
111
112
113
114
115
116

	Pipe_SelectPipe(BLUETOOTH_DATA_IN_PIPE);
	Pipe_Unfreeze();
	
	if (!(Pipe_IsReadWriteAllowed()))
	{
		Pipe_Freeze();
		return;
	}
	  
117
	/* Read in the received ACL packet headers when it has been discovered that a packet has been received */
118
119
120
	Pipe_Read_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));
	Pipe_Read_Stream_LE(&DataHeader, sizeof(DataHeader));

121
122
123
124
125
126
	BT_ACL_DEBUG(2, "", NULL);
	BT_ACL_DEBUG(2, "Packet Received", NULL);
	BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel);
	BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength);
127

128
	/* Check the packet's destination channel - signalling channel should be processed by the stack internally */
129
	if (DataHeader.DestinationChannel == BT_CHANNEL_SIGNALING)
130
	{
131
		/* Read in the Signal Command header of the incomming packet */
132
		BT_Signal_Header_t SignalCommandHeader;
133
		Pipe_Read_Stream_LE(&SignalCommandHeader, sizeof(SignalCommandHeader));
134
		
135
		/* Dispatch to the appropriate handler function based on the Signal message code */
136
137
		switch (SignalCommandHeader.Code)
		{
138
			case BT_SIGNAL_CONNECTION_REQUEST:
139
				Bluetooth_Signal_ConnectionReq(&SignalCommandHeader);
140
				break;
141
			case BT_SIGNAL_CONNECTION_RESPONSE:
142
				Bluetooth_Signal_ConnectionResp(&SignalCommandHeader);
143
				break;
144
			case BT_SIGNAL_CONFIGURATION_REQUEST:
145
				Bluetooth_Signal_ConfigurationReq(&SignalCommandHeader);
146
				break;
147
			case BT_SIGNAL_CONFIGURATION_RESPONSE:
148
				Bluetooth_Signal_ConfigurationResp(&SignalCommandHeader);
149
				break;
150
			case BT_SIGNAL_DISCONNECTION_REQUEST:
151
				Bluetooth_Signal_DisconnectionReq(&SignalCommandHeader);
152
153
				break;
			case BT_SIGNAL_DISCONNECTION_RESPONSE:
154
				Bluetooth_Signal_DisconnectionResp(&SignalCommandHeader);
155
				break;
156
			case BT_SIGNAL_ECHO_REQUEST:
157
				Bluetooth_Signal_EchoReq(&SignalCommandHeader);
158
				break;
159
			case BT_SIGNAL_INFORMATION_REQUEST:
160
				Bluetooth_Signal_InformationReq(&SignalCommandHeader);
161
				break;
162
163
164
165
166
167
168
169
170
171
172
			case BT_SIGNAL_COMMAND_REJECT:
				BT_ACL_DEBUG(1, "<< Command Reject", NULL);
				
				uint16_t RejectReason;
				Pipe_Read_Stream_LE(&RejectReason, sizeof(RejectReason));
				Pipe_Discard_Stream(ACLPacketHeader.DataLength - sizeof(RejectReason));
				Pipe_ClearIN();
				Pipe_Freeze();				
			
				BT_ACL_DEBUG(2, "-- Reason: %d", RejectReason);
				break;
173
			default:
174
				BT_ACL_DEBUG(1, "<< Unknown Signaling Command 0x%02X", SignalCommandHeader.Code);
175
	
176
177
178
179
180
181
182
183
				Pipe_Discard_Stream(ACLPacketHeader.DataLength);
				Pipe_ClearIN();		
				Pipe_Freeze();
				break;
		}
	}
	else
	{
184
185
186
		/* Non-signalling packet received, read in the packet contents and pass to the user application */
		uint8_t PacketData[DataHeader.PayloadLength];
		Pipe_Read_Stream_LE(PacketData, DataHeader.PayloadLength);
187
		Pipe_ClearIN();
188
		Pipe_Freeze();
189
190

		Bluetooth_PacketReceived(PacketData, DataHeader.PayloadLength, Bluetooth_GetChannelData(DataHeader.DestinationChannel, false));
191
192
193
	}
}

194
195
196
197
198
199
200
201
202
/** Sends a packet to the remote device on the specified channel.
 *
 * \param Data     Pointer to a buffer where the data is to be sourced from
 * \param DataLen  Length of the data to send
 * \param Channel  Channel information structure containing the destination channel's information, NULL to send
 *                 to the remote device's signalling channel
 *
 * \return A value from the \ref BT_SendPacket_ErrorCodes_t enum
 */
203
uint8_t Bluetooth_SendPacket(void* Data, uint16_t DataLen, Bluetooth_Channel_t* Channel)
204
{
205
206
207
	BT_ACL_Header_t        ACLPacketHeader;
	BT_DataPacket_Header_t DataHeader;

208
	/* A remote device must be connected before a packet transmission is attempted */
209
	if (!(Bluetooth_Connection.IsConnected))
210
211
	  return BT_SENDPACKET_NotConnected;

212
	/* If the destination channel is not the signalling channel and it is not currently fully open, abort */
213
	if ((Channel != NULL) && (Channel->State != Channel_Open))
214
	  return BT_SENDPACKET_ChannelNotOpen;
215

216
	/* Fill out the packet's header from the remote device connection information structure */
217
	ACLPacketHeader.ConnectionHandle      = (Bluetooth_Connection.ConnectionHandle | BT_ACL_FIRST_AUTOFLUSH);
218
	ACLPacketHeader.DataLength            = sizeof(DataHeader) + DataLen;
219
	DataHeader.PayloadLength              = DataLen;
220
	DataHeader.DestinationChannel         = (Channel == NULL) ? BT_CHANNEL_SIGNALING : Channel->RemoteNumber;
221
222
223
224

	Pipe_SelectPipe(BLUETOOTH_DATA_OUT_PIPE);
	Pipe_Unfreeze();
	
225
	/* Write the packet contents to the pipe so that it can be sent to the remote device */
226
227
228
	Pipe_Write_Stream_LE(&ACLPacketHeader, sizeof(ACLPacketHeader));
	Pipe_Write_Stream_LE(&DataHeader, sizeof(DataHeader));
	Pipe_Write_Stream_LE(Data, DataLen);
229
	Pipe_ClearOUT();
230
	
231
	Pipe_Freeze();
232
	
233
234
235
236
237
238
239
	BT_ACL_DEBUG(2, "", NULL);
	BT_ACL_DEBUG(2, "Packet Sent", NULL);
	BT_ACL_DEBUG(2, "-- Connection Handle: 0x%04X", (ACLPacketHeader.ConnectionHandle & 0x0FFF));
	BT_ACL_DEBUG(2, "-- Data Length: 0x%04X", ACLPacketHeader.DataLength);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DataHeader.DestinationChannel);
	BT_ACL_DEBUG(2, "-- Payload Length: 0x%04X", DataHeader.PayloadLength);

240
	return BT_SENDPACKET_NoError;
241
242
}

243
244
245
246
247
248
249
250
251
252
253
/** Opens a bluetooth channel to the currently connected remote device, so that data can be exchanged.
 *
 *  \note The channel is not immediately opened when this function returns - it must undergo a two way
 *        connection and configuration process first as the main Bluetooth stack processing task is
 *        repeatedly called. The returned channel is unusable by the user application until its State
 *        element has progressed to the Open state.
 *
 *  \param PSM  PSM of the service that the channel is to be opened for
 *
 *  \return Pointer to the channel information structure of the opened channel, or NULL if no free channels
 */
254
255
256
257
Bluetooth_Channel_t* Bluetooth_OpenChannel(uint16_t PSM)
{
	Bluetooth_Channel_t* ChannelData = NULL;

258
	/* Search through the channel information list for a free channel item */
259
260
261
262
	for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)
	{
		if (Bluetooth_Connection.Channels[i].State == Channel_Closed)
		{
263
264
265
266
			ChannelData = &Bluetooth_Connection.Channels[i];
			
			/* Set the new channel structure's local channel number to a unique value within the connection orientated
			   channel address space */
267
			ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i);
268
269
270
271
			break;
		}
	}

272
	/* If no free channel item was found in the list, all channels are occupied - abort */
273
274
275
	if (ChannelData == NULL)
	  return NULL;

276
	/* Reset and fill out the allocated channel's information structure with defaults */
277
278
279
	ChannelData->RemoteNumber = 0;
	ChannelData->PSM          = PSM;
	ChannelData->LocalMTU     = MAXIMUM_CHANNEL_MTU;
280
	ChannelData->State        = Channel_WaitConnectRsp;
281
282
283
284
285
286
287
	  
	struct
	{
		BT_Signal_Header_t        SignalCommandHeader;
		BT_Signal_ConnectionReq_t ConnectionRequest;
	} PacketData;

288
	/* Fill out the Signal Command header in the response packet */
289
290
291
	PacketData.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_REQUEST;
	PacketData.SignalCommandHeader.Identifier        = ++Bluetooth_Connection.SignallingIdentifier;
	PacketData.SignalCommandHeader.Length            = sizeof(PacketData.ConnectionRequest);
292
293
	
	/* Fill out the Connection Request in the response packet */
294
295
296
297
298
299
300
301
302
303
304
305
	PacketData.ConnectionRequest.PSM                 = PSM;
	PacketData.ConnectionRequest.SourceChannel       = ChannelData->LocalNumber;
	
	Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);

	BT_ACL_DEBUG(1, ">> L2CAP Connection Request", NULL);
	BT_ACL_DEBUG(2, "-- PSM 0x%04X", PacketData.ConnectionRequest.PSM);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.ConnectionRequest.SourceChannel);

	return ChannelData;
}

306
307
308
309
310
311
312
313
314
315
/** Closes a bluetooth channel that is open to the currently connected remote device, so that no further data
 *  can be exchanged.
 *
 *  \note The channel is not immediately closed when this function returns - it must undergo an asynchronous
 *        disconnection process first as the main Bluetooth stack processing task is repeatedly called. The
 *        returned channel is unusable by the user application upon return however the channel is not completely
 *        closed until its State element has progressed to the Closed state.
 *
 * \param Channel  Channel information structure of the channel to close
 */
316
317
void Bluetooth_CloseChannel(Bluetooth_Channel_t* Channel)
{
318
	/* Don't try to close a non-existing or already closed channel */
319
320
	if ((Channel == NULL) || (Channel->State == Channel_Closed))
	  return;
321
322

	/* Set the channel's state to the start of the teardown process */
323
324
325
326
327
328
329
330
	Channel->State = Channel_WaitDisconnect;

	struct
	{
		BT_Signal_Header_t           SignalCommandHeader;
		BT_Signal_DisconnectionReq_t DisconnectionRequest;
	} PacketData;
	
331
	/* Fill out the Signal Command header in the response packet */
332
333
334
	PacketData.SignalCommandHeader.Code            = BT_SIGNAL_DISCONNECTION_REQUEST;
	PacketData.SignalCommandHeader.Identifier      = ++Bluetooth_Connection.SignallingIdentifier;
	PacketData.SignalCommandHeader.Length          = sizeof(PacketData.DisconnectionRequest);
335
336

	/* Fill out the Disconnection Request in the response packet */
337
338
339
340
341
342
343
344
345
346
	PacketData.DisconnectionRequest.DestinationChannel = Channel->RemoteNumber;
	PacketData.DisconnectionRequest.SourceChannel = Channel->LocalNumber;

	Bluetooth_SendPacket(&PacketData, sizeof(PacketData), NULL);
	
	BT_ACL_DEBUG(1, ">> L2CAP Disconnection Request", NULL);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", PacketData.DisconnectionRequest.DestinationChannel);	
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", PacketData.DisconnectionRequest.SourceChannel);	
}

347
348
349
350
351
/** Internal Bluetooth stack Signal Command processing routine for a Connection Request command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_ConnectionReq(BT_Signal_Header_t* SignalCommandHeader)
352
{
353
	BT_Signal_ConnectionReq_t ConnectionRequest;
354
355
356
357
358
359
	
	Pipe_Read_Stream_LE(&ConnectionRequest, sizeof(ConnectionRequest));

	Pipe_ClearIN();
	Pipe_Freeze();

360
361
362
	BT_ACL_DEBUG(1, "<< L2CAP Connection Request", NULL);
	BT_ACL_DEBUG(2, "-- PSM: 0x%04X", ConnectionRequest.PSM);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionRequest.SourceChannel);
363
	
364
	/* Try to retrieve the existing channel's information structure if it exists */
365
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionRequest.SourceChannel, true);
366

367
	/* If an existing channel item with the correct remote channel number was not found, find a free channel entry */
368
369
	if (ChannelData == NULL)
	{
370
		/* Look through the channel information list for a free entry */
371
372
373
374
		for (uint8_t i = 0; i < BLUETOOTH_MAX_OPEN_CHANNELS; i++)
		{
			if (Bluetooth_Connection.Channels[i].State == Channel_Closed)
			{
375
376
377
378
				ChannelData = &Bluetooth_Connection.Channels[i];

				/* Set the new channel structure's local channel number to a unique value within the connection orientated
				   channel address space */
379
				ChannelData->LocalNumber = (BT_CHANNELNUMBER_BASEOFFSET + i);
380
381
382
383
384
				break;
			}
		}
	}

385
	/* Reset the channel item contents only if a channel entry was found for it */
386
387
388
389
390
391
392
393
	if (ChannelData != NULL)
	{
		ChannelData->RemoteNumber = ConnectionRequest.SourceChannel;
		ChannelData->PSM          = ConnectionRequest.PSM;
		ChannelData->LocalMTU     = MAXIMUM_CHANNEL_MTU;
		ChannelData->State        = Channel_Config_WaitConfig;
	}
	
394
395
396
397
398
399
	struct
	{
		BT_Signal_Header_t         SignalCommandHeader;
		BT_Signal_ConnectionResp_t ConnectionResponse;
	} ResponsePacket;

400
	/* Fill out the Signal Command header in the response packet */
401
402
403
	ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConnectionResponse);
404
405

	/* Fill out the Connection Response in the response packet */
406
407
408
409
	ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;
	ResponsePacket.ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;
	ResponsePacket.ConnectionResponse.Result             = (ChannelData == NULL) ? BT_CONNECTION_REFUSED_RESOURCES :
	                                                                               BT_CONNECTION_SUCCESSFUL;
410
411
412
413
414
415
416
	ResponsePacket.ConnectionResponse.Status             = 0x00;
	
	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);

	BT_ACL_DEBUG(1, ">> L2CAP Connection Response", NULL);
	BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConnectionResponse.Result);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.ConnectionResponse.DestinationChannel);
417
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConnectionResponse.SourceChannel);
418
419
}

420
421
422
423
424
/** Internal Bluetooth stack Signal Command processing routine for a Connection Response command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_ConnectionResp(BT_Signal_Header_t* SignalCommandHeader)
425
426
427
428
429
430
431
432
433
434
435
436
437
{
	BT_Signal_ConnectionResp_t ConnectionResponse;
	
	Pipe_Read_Stream_LE(&ConnectionResponse, sizeof(ConnectionResponse));

	Pipe_ClearIN();
	Pipe_Freeze();

	BT_ACL_DEBUG(1, "<< L2CAP Connection Response", NULL);
	BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConnectionResponse.Result);	
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConnectionResponse.SourceChannel);	
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConnectionResponse.DestinationChannel);	

438
	/* Search for the referenced channel in the channel information list */
439
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionResponse.SourceChannel, false);
440

441
	/* Only progress if the referenced channel data was found */
442
443
	if (ChannelData != NULL)
	{
444
		/* Set the channel structure's remote channel number to the channel allocated on the remote device */
445
446
447
448
449
450
		ChannelData->RemoteNumber = ConnectionResponse.SourceChannel;
		ChannelData->State        = (ConnectionResponse.Result == BT_CONNECTION_SUCCESSFUL) ?
		                             Channel_Config_WaitConfig : Channel_Closed;
	}
}

451
452
453
454
455
/** Internal Bluetooth stack Signal Command processing routine for a Configuration Request command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_ConfigurationReq(BT_Signal_Header_t* SignalCommandHeader)
456
{
457
	BT_Signal_ConfigurationReq_t ConfigurationRequest;
458
459
	
	/* Allocate a buffer large enough to hold the variable number of configuration options in the request */
460
461
	uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest));
	uint8_t Options[OptionsLen];
462

463
464
	Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));	
	Pipe_Read_Stream_LE(&Options, sizeof(Options));
465

466
467
468
	Pipe_ClearIN();
	Pipe_Freeze();

469
	/* Search for the referenced channel in the channel information list */
470
471
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);

472
473
474
	BT_ACL_DEBUG(1, "<< L2CAP Configuration Request", NULL);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ConfigurationRequest.DestinationChannel);
	BT_ACL_DEBUG(2, "-- Remote MTU: 0x%04X", ChannelData->RemoteMTU);
475
476
	BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen);

477
478
	/* Only look at the channel configuration options if a valid channel entry for the local channel number was found */
	if (ChannelData != NULL)
479
	{
480
481
482
483
484
485
		/* Iterate through each option in the configuration request to look for ones which can be processed */
		uint8_t OptionPos = 0;
		while (OptionPos < OptionsLen)
		{
			BT_Config_Option_Header_t* OptionHeader = (BT_Config_Option_Header_t*)&Options[OptionPos];
			void*                      OptionData   = &Options[OptionPos + sizeof(*OptionHeader)];
486

487
488
489
490
491
492
			BT_ACL_DEBUG(2, "-- Option Type: 0x%04X", OptionHeader->Type);
			BT_ACL_DEBUG(2, "-- Option Length: 0x%04X", (sizeof(*OptionHeader) + OptionHeader->Length));
			
			/* Store the remote MTU option's value if present */
			if (OptionHeader->Type == BT_CONFIG_OPTION_MTU)
			  ChannelData->RemoteMTU = *((uint16_t*)OptionData);
493

494
495
496
			/* Progress to the next option in the packet */
			OptionPos += (sizeof(*OptionHeader) + OptionHeader->Length);
		}
497
	}
498
	
499
500
501
502
503
504
	struct
	{
		BT_Signal_Header_t            SignalCommandHeader;
		BT_Signal_ConfigurationResp_t ConfigurationResponse;
	} ResponsePacket;

505
	/* Fill out the Signal Command header in the response packet */
506
507
508
	ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONFIGURATION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConfigurationResponse);
509
510

	/* Fill out the Configuration Response in the response packet */
511
512
513
514
515
	ResponsePacket.ConfigurationResponse.SourceChannel   = ChannelData->RemoteNumber;
	ResponsePacket.ConfigurationResponse.Flags           = 0x00;
	ResponsePacket.ConfigurationResponse.Result          = (ChannelData != NULL) ? BT_CONFIGURATION_SUCCESSFUL : BT_CONFIGURATION_REJECTED;

	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);
516
517

	if (ChannelData != NULL)
518
519
520
521
522
523
524
525
526
527
528
529
530
531
	{
		switch (ChannelData->State)
		{
			case Channel_Config_WaitConfig:
				ChannelData->State = Channel_Config_WaitSendConfig;
				break;
			case Channel_Config_WaitReqResp:
				ChannelData->State = Channel_Config_WaitResp;
				break;
			case Channel_Config_WaitReq:
				ChannelData->State = Channel_Open;
				break;
		}
	}
532

533
534
535
	BT_ACL_DEBUG(1, ">> L2CAP Configuration Response", NULL);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConfigurationResponse.SourceChannel);
	BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.ConfigurationResponse.Result);
536
}
537

538
539
540
541
542
/** Internal Bluetooth stack Signal Command processing routine for a Configuration Response command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_ConfigurationResp(BT_Signal_Header_t* SignalCommandHeader)
543
544
545
546
547
548
549
550
551
552
553
554
{
	BT_Signal_ConfigurationResp_t ConfigurationResponse;

	Pipe_Read_Stream_LE(&ConfigurationResponse, sizeof(ConfigurationResponse));

	Pipe_ClearIN();
	Pipe_Freeze();
	
	BT_ACL_DEBUG(1, "<< L2CAP Configuration Response", NULL);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ConfigurationResponse.SourceChannel);
	BT_ACL_DEBUG(2, "-- Result: 0x%02X", ConfigurationResponse.Result);

555
556
557
558
559
	/* Search for the referenced channel in the channel information list */
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationResponse.SourceChannel, true);
	
	/* Only update the channel's state if it was found in the channel list */
	if (ChannelData != NULL)
560
	{
561
562
		/* Check if the channel configuration completed successfuly */
		if (ConfigurationResponse.Result == BT_CONFIGURATION_SUCCESSFUL)
563
564
565
566
567
568
569
570
571
		{
			switch (ChannelData->State)
			{
				case Channel_Config_WaitReqResp:
					ChannelData->State = Channel_Config_WaitReq;
					break;
				case Channel_Config_WaitResp:
					ChannelData->State = Channel_Open;
					break;
572
573
574
575
576
577
578
			}	
		}
		else
		{
			/* Configuration failed - close the channel */
			ChannelData->State = Channel_Closed;
		}
579
580
581
	}
}

582
583
584
585
586
/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Request command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_DisconnectionReq(BT_Signal_Header_t* SignalCommandHeader)
587
{
588
	BT_Signal_DisconnectionReq_t DisconnectionRequest;
589
590
591
	
	Pipe_Read_Stream_LE(&DisconnectionRequest, sizeof(DisconnectionRequest));

592
593
594
	BT_ACL_DEBUG(1, "<< L2CAP Disconnection Request", NULL);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionRequest.DestinationChannel);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionRequest.SourceChannel);
595
596
597
598
	
	Pipe_ClearIN();
	Pipe_Freeze();
	
599
	/* Search for the referenced channel in the channel information list */
600
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, true);
601

602
603
604
605
606
607
	struct
	{
		BT_Signal_Header_t            SignalCommandHeader;
		BT_Signal_DisconnectionResp_t DisconnectionResponse;
	} ResponsePacket;

608
	/* Fill out the Signal Command header in the response packet */
609
610
611
	ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_DISCONNECTION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length               = sizeof(ResponsePacket.DisconnectionResponse);
612
613

	/* Fill out the Disconnection Response in the response packet */
614
615
	ResponsePacket.DisconnectionResponse.DestinationChannel = ChannelData->RemoteNumber;
	ResponsePacket.DisconnectionResponse.SourceChannel      = ChannelData->LocalNumber;
616
617
618

	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);

619
	/* If the channel was found in the channel list, close it */
620
621
622
	if (ChannelData != NULL)
	  ChannelData->State = Channel_Closed;

623
624
625
	BT_ACL_DEBUG(1, ">> L2CAP Disconnection Response", NULL);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.DisconnectionResponse.SourceChannel);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", ResponsePacket.DisconnectionResponse.DestinationChannel);
626
627
}

628
629
630
631
632
/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Response command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_DisconnectionResp(BT_Signal_Header_t* SignalCommandHeader)
633
634
635
636
637
638
639
640
641
642
643
644
{
	BT_Signal_DisconnectionResp_t DisconnectionResponse;
	
	Pipe_Read_Stream_LE(&DisconnectionResponse, sizeof(DisconnectionResponse));

	BT_ACL_DEBUG(1, "<< L2CAP Disconnection Response", NULL);
	BT_ACL_DEBUG(2, "-- Destination Channel: 0x%04X", DisconnectionResponse.DestinationChannel);
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", DisconnectionResponse.SourceChannel);
	
	Pipe_ClearIN();
	Pipe_Freeze();
	
645
	/* Search for the referenced channel in the channel information list */
646
647
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionResponse.SourceChannel, true);
	
648
649
	/* If the channel was found in the channel list, close it */	
	if (ChannelData != NULL)
650
651
652
	  ChannelData->State = Channel_Closed;
}

653
654
655
656
657
/** Internal Bluetooth stack Signal Command processing routine for an Echo Request command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_EchoReq(BT_Signal_Header_t* SignalCommandHeader)
658
{
659
	BT_ACL_DEBUG(1, "<< L2CAP Echo Request", NULL);
660
661
662
663
	
	Pipe_ClearIN();
	Pipe_Freeze();
	
664
665
666
667
668
	struct
	{
		BT_Signal_Header_t SignalCommandHeader;
	} ResponsePacket;

669
	/* Fill out the Signal Command header in the response packet */
670
671
672
	ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_ECHO_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length               = 0;
673
	
674
675
676
	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);

	BT_ACL_DEBUG(1, ">> L2CAP Echo Response", NULL);
677
678
}

679
680
681
682
683
/** Internal Bluetooth stack Signal Command processing routine for an Information Request command.
 *
 *  \param  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
 */
static inline void Bluetooth_Signal_InformationReq(BT_Signal_Header_t* SignalCommandHeader)
684
{
685
	BT_Signal_InformationReq_t InformationRequest;
686
687
688

	Pipe_Read_Stream_LE(&InformationRequest, sizeof(InformationRequest));

689
690
	BT_ACL_DEBUG(1, "<< L2CAP Information Request", NULL);
	BT_ACL_DEBUG(2, "-- Info Type: 0x%04X", InformationRequest.InfoType);
691
692
693
694
	
	Pipe_ClearIN();
	Pipe_Freeze();

695
696
697
698
699
700
701
702
703
704
	struct
	{
		BT_Signal_Header_t SignalCommandHeader;
		BT_Signal_InformationResp_t InformationResponse;
		
		uint8_t Data[4];
	} ResponsePacket;
	
	uint8_t DataLen = 0;
	
705
	/* Retrieve the requested information and store it in the outgoing packet, if found */
706
707
708
	switch (InformationRequest.InfoType)
	{
		case BT_INFOREQ_MTU:		
709
710
			ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;
			DataLen = 2;
711
			
712
			*((uint16_t*)&ResponsePacket.Data) = MAXIMUM_CHANNEL_MTU;
713
714
			break;
		case BT_INFOREQ_EXTENDEDFEATURES:
715
716
			ResponsePacket.InformationResponse.Result = BT_INFORMATION_SUCCESSFUL;
			DataLen = 4;
717
			
718
			*((uint32_t*)&ResponsePacket.Data) = 0;
719
720
			break;
		default:
721
722
			ResponsePacket.InformationResponse.Result = BT_INFORMATION_NOTSUPPORTED;
			DataLen = 0;
723
724
725
			break;
	}
	
726
	/* Fill out the Signal Command header in the response packet */
727
728
729
730
	ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_INFORMATION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length               = sizeof(ResponsePacket.InformationResponse) + DataLen;

731
732
733
	/* Fill out the Information Response in the response packet */
	ResponsePacket.InformationResponse.InfoType = InformationRequest.InfoType;
	
734
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket) - sizeof(ResponsePacket.Data) + DataLen), NULL);
735

736
737
	BT_ACL_DEBUG(1, ">> L2CAP Information Response", NULL);	
	BT_ACL_DEBUG(2, "-- Result: 0x%02X", ResponsePacket.InformationResponse.Result);
738
}