BluetoothACLPackets.c 30.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
  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
/** Sends a packet to the remote device on the specified channel.
 *
196
197
198
199
 * \param[in] Data     Pointer to a buffer where the data is to be sourced from
 * \param[in] DataLen  Length of the data to send
 * \param[in] Channel  Channel information structure containing the destination channel's information, NULL to send
 *                     to the remote device's signalling channel
200
201
202
 *
 * \return A value from the \ref BT_SendPacket_ErrorCodes_t enum
 */
203
uint8_t Bluetooth_SendPacket(void* Data, const uint16_t DataLen, Bluetooth_Channel_t* const 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
/** 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.
 *
250
 *  \param[in] PSM  PSM of the service that the channel is to be opened for
251
252
253
 *
 *  \return Pointer to the channel information structure of the opened channel, or NULL if no free channels
 */
254
Bluetooth_Channel_t* Bluetooth_OpenChannel(const uint16_t PSM)
255
256
257
{
	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
/** 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.
 *
314
 * \param[in,out] Channel  Channel information structure of the channel to close
315
 */
316
void Bluetooth_CloseChannel(Bluetooth_Channel_t* const Channel)
317
{
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
/** Internal Bluetooth stack Signal Command processing routine for a Connection Request command.
 *
349
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
350
 */
351
static inline void Bluetooth_Signal_ConnectionReq(const BT_Signal_Header_t* const 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
				break;
			}
		}
	}
384
385
	
	uint8_t ChannelStatus = BT_CONNECTION_REFUSED_RESOURCES;
386

387
	/* Reset the channel item contents only if a channel entry was found for it */
388
389
	if (ChannelData != NULL)
	{
390
391
392
393
394
395
396
397
398
399
400
401
402
403
		/* Check if the user application will allow the connection based on its PSM */
		if (Bluetooth_ChannelConnectionRequest(ConnectionRequest.PSM))
		{
			ChannelData->RemoteNumber = ConnectionRequest.SourceChannel;
			ChannelData->PSM          = ConnectionRequest.PSM;
			ChannelData->LocalMTU     = MAXIMUM_CHANNEL_MTU;
			ChannelData->State        = Channel_Config_WaitConfig;
			
			ChannelStatus = BT_CONNECTION_SUCCESSFUL;
		}
		else
		{
			ChannelStatus = BT_CONNECTION_REFUSED_PSM;		
		}
404
405
	}
	
406
407
408
409
410
411
	struct
	{
		BT_Signal_Header_t         SignalCommandHeader;
		BT_Signal_ConnectionResp_t ConnectionResponse;
	} ResponsePacket;

412
	/* Fill out the Signal Command header in the response packet */
413
414
415
	ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONNECTION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConnectionResponse);
416
417

	/* Fill out the Connection Response in the response packet */
418
419
	ResponsePacket.ConnectionResponse.DestinationChannel = ChannelData->LocalNumber;
	ResponsePacket.ConnectionResponse.SourceChannel      = ChannelData->RemoteNumber;
420
	ResponsePacket.ConnectionResponse.Result             = ChannelStatus;
421
422
423
424
425
426
427
	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);
428
	BT_ACL_DEBUG(2, "-- Source Channel: 0x%04X", ResponsePacket.ConnectionResponse.SourceChannel);
429
430
}

431
432
/** Internal Bluetooth stack Signal Command processing routine for a Connection Response command.
 *
433
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
434
 */
435
static inline void Bluetooth_Signal_ConnectionResp(const BT_Signal_Header_t* const SignalCommandHeader)
436
437
438
439
440
441
442
443
444
445
446
447
448
{
	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);	

449
	/* Search for the referenced channel in the channel information list */
450
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConnectionResponse.SourceChannel, false);
451

452
	/* Only progress if the referenced channel data was found */
453
454
	if (ChannelData != NULL)
	{
455
		/* Set the channel structure's remote channel number to the channel allocated on the remote device */
456
457
458
459
460
461
		ChannelData->RemoteNumber = ConnectionResponse.SourceChannel;
		ChannelData->State        = (ConnectionResponse.Result == BT_CONNECTION_SUCCESSFUL) ?
		                             Channel_Config_WaitConfig : Channel_Closed;
	}
}

462
463
/** Internal Bluetooth stack Signal Command processing routine for a Configuration Request command.
 *
464
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
465
 */
466
static inline void Bluetooth_Signal_ConfigurationReq(const BT_Signal_Header_t* const SignalCommandHeader)
467
{
468
	BT_Signal_ConfigurationReq_t ConfigurationRequest;
469
470
	
	/* Allocate a buffer large enough to hold the variable number of configuration options in the request */
471
472
	uint8_t OptionsLen = (SignalCommandHeader->Length - sizeof(ConfigurationRequest));
	uint8_t Options[OptionsLen];
473

474
475
	Pipe_Read_Stream_LE(&ConfigurationRequest, sizeof(ConfigurationRequest));	
	Pipe_Read_Stream_LE(&Options, sizeof(Options));
476

477
478
479
	Pipe_ClearIN();
	Pipe_Freeze();

480
	/* Search for the referenced channel in the channel information list */
481
482
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(ConfigurationRequest.DestinationChannel, false);

483
484
485
	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);
486
487
	BT_ACL_DEBUG(2, "-- Options Len: 0x%04X", OptionsLen);

488
489
	/* Only look at the channel configuration options if a valid channel entry for the local channel number was found */
	if (ChannelData != NULL)
490
	{
491
492
493
494
495
496
		/* 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)];
497

498
499
500
501
502
503
			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);
504

505
506
507
			/* Progress to the next option in the packet */
			OptionPos += (sizeof(*OptionHeader) + OptionHeader->Length);
		}
508
	}
509
	
510
511
512
513
514
515
	struct
	{
		BT_Signal_Header_t            SignalCommandHeader;
		BT_Signal_ConfigurationResp_t ConfigurationResponse;
	} ResponsePacket;

516
	/* Fill out the Signal Command header in the response packet */
517
518
519
	ResponsePacket.SignalCommandHeader.Code              = BT_SIGNAL_CONFIGURATION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier        = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length            = sizeof(ResponsePacket.ConfigurationResponse);
520
521

	/* Fill out the Configuration Response in the response packet */
522
523
524
525
526
	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);
527
528

	if (ChannelData != NULL)
529
530
531
532
533
534
535
536
537
538
539
540
541
542
	{
		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;
		}
	}
543

544
545
546
	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);
547
}
548

549
550
/** Internal Bluetooth stack Signal Command processing routine for a Configuration Response command.
 *
551
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
552
 */
553
static inline void Bluetooth_Signal_ConfigurationResp(const BT_Signal_Header_t* const SignalCommandHeader)
554
555
556
557
558
559
560
561
562
563
564
565
{
	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);

566
567
568
569
570
	/* 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)
571
	{
572
573
		/* Check if the channel configuration completed successfuly */
		if (ConfigurationResponse.Result == BT_CONFIGURATION_SUCCESSFUL)
574
575
576
577
578
579
580
581
582
		{
			switch (ChannelData->State)
			{
				case Channel_Config_WaitReqResp:
					ChannelData->State = Channel_Config_WaitReq;
					break;
				case Channel_Config_WaitResp:
					ChannelData->State = Channel_Open;
					break;
583
584
585
586
587
588
589
			}	
		}
		else
		{
			/* Configuration failed - close the channel */
			ChannelData->State = Channel_Closed;
		}
590
591
592
	}
}

593
594
/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Request command.
 *
595
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
596
 */
597
static inline void Bluetooth_Signal_DisconnectionReq(const BT_Signal_Header_t* const SignalCommandHeader)
598
{
599
	BT_Signal_DisconnectionReq_t DisconnectionRequest;
600
601
602
	
	Pipe_Read_Stream_LE(&DisconnectionRequest, sizeof(DisconnectionRequest));

603
604
605
	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);
606
607
608
609
	
	Pipe_ClearIN();
	Pipe_Freeze();
	
610
	/* Search for the referenced channel in the channel information list */
611
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionRequest.SourceChannel, true);
612

613
614
615
616
617
618
	struct
	{
		BT_Signal_Header_t            SignalCommandHeader;
		BT_Signal_DisconnectionResp_t DisconnectionResponse;
	} ResponsePacket;

619
	/* Fill out the Signal Command header in the response packet */
620
621
622
	ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_DISCONNECTION_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length               = sizeof(ResponsePacket.DisconnectionResponse);
623
624

	/* Fill out the Disconnection Response in the response packet */
625
626
	ResponsePacket.DisconnectionResponse.DestinationChannel = ChannelData->RemoteNumber;
	ResponsePacket.DisconnectionResponse.SourceChannel      = ChannelData->LocalNumber;
627
628
629

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

630
	/* If the channel was found in the channel list, close it */
631
632
633
	if (ChannelData != NULL)
	  ChannelData->State = Channel_Closed;

634
635
636
	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);
637
638
}

639
640
/** Internal Bluetooth stack Signal Command processing routine for a Disconnection Response command.
 *
641
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
642
 */
643
static inline void Bluetooth_Signal_DisconnectionResp(const BT_Signal_Header_t* const SignalCommandHeader)
644
645
646
647
648
649
650
651
652
653
654
655
{
	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();
	
656
	/* Search for the referenced channel in the channel information list */
657
658
	Bluetooth_Channel_t* ChannelData = Bluetooth_GetChannelData(DisconnectionResponse.SourceChannel, true);
	
659
660
	/* If the channel was found in the channel list, close it */	
	if (ChannelData != NULL)
661
662
663
	  ChannelData->State = Channel_Closed;
}

664
665
/** Internal Bluetooth stack Signal Command processing routine for an Echo Request command.
 *
666
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
667
 */
668
static inline void Bluetooth_Signal_EchoReq(const BT_Signal_Header_t* const SignalCommandHeader)
669
{
670
	BT_ACL_DEBUG(1, "<< L2CAP Echo Request", NULL);
671
672
673
674
	
	Pipe_ClearIN();
	Pipe_Freeze();
	
675
676
677
678
679
	struct
	{
		BT_Signal_Header_t SignalCommandHeader;
	} ResponsePacket;

680
	/* Fill out the Signal Command header in the response packet */
681
682
683
	ResponsePacket.SignalCommandHeader.Code                 = BT_SIGNAL_ECHO_RESPONSE;
	ResponsePacket.SignalCommandHeader.Identifier           = SignalCommandHeader->Identifier;
	ResponsePacket.SignalCommandHeader.Length               = 0;
684
	
685
686
687
	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), NULL);

	BT_ACL_DEBUG(1, ">> L2CAP Echo Response", NULL);
688
689
}

690
691
/** Internal Bluetooth stack Signal Command processing routine for an Information Request command.
 *
692
 *  \param[in]  SignalCommandHeader  Pointer to the start of the received packet's Signal Command header
693
 */
694
static inline void Bluetooth_Signal_InformationReq(const BT_Signal_Header_t* const SignalCommandHeader)
695
{
696
	BT_Signal_InformationReq_t InformationRequest;
697
698
699

	Pipe_Read_Stream_LE(&InformationRequest, sizeof(InformationRequest));

700
701
	BT_ACL_DEBUG(1, "<< L2CAP Information Request", NULL);
	BT_ACL_DEBUG(2, "-- Info Type: 0x%04X", InformationRequest.InfoType);
702
703
704
705
	
	Pipe_ClearIN();
	Pipe_Freeze();

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

742
743
744
	/* Fill out the Information Response in the response packet */
	ResponsePacket.InformationResponse.InfoType = InformationRequest.InfoType;
	
745
	Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket) - sizeof(ResponsePacket.Data) + DataLen), NULL);
746

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