RFCOMM.c 16 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
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
/*
             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.
*/

/** \file
 *
 *  RFCOMM layer module. This module manages the RFCOMM layer of the
 *  stack, providing virtual serial port channels on top of the lower
 *  L2CAP layer.
 */

#define  INCLUDE_FROM_RFCOMM_C
#include "RFCOMM.h"

/** 8-Bit CRC table used by the FCS field of each RFCOMM encoded frame, sourced from the ETSI TS 101 369 V7.2.0
 *  specification document, upon which the RFCOMM specification is based.
 */
const uint8_t CRC8_Table[256] PROGMEM =
	{
		0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
		0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
		0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
		0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
		0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
		0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
		0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
		0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
		0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
		0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
		0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
		0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
		0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
		0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
		0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
		0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
	};

64
65
66
67
/** RFCOMM channel state structure, to retain information about each open channel in the RFCOMM multiplexer. */
RFCOMM_Channel_t RFCOMM_Channels[RFCOMM_MAX_OPEN_CHANNELS];


68
69
70
/** Initializes the RFCOMM service, ready for new connections from a SDP client. */
void RFCOMM_Initialize(void)
{
71
72
	/* Reset the RFCOMM channel structures, to invalidate any confiured RFCOMM channels */
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
73
	  RFCOMM_Channels[i].State = RFCOMM_Channel_Closed;
74
75
}

76
77
78
79
80
81
82
/** Services all the logical RFCOMM channels on the given ACL channel, sending any RFCOMM control requests to
 *  the remote device as needed to establish new logical RFCOMM channels. This function should be called repeatedly
 *  in the main program loop when an ACL channel with an RFCOMM PSM has been established between the local and remote
 *  device.
 *
 *  \param[in] ACLChannel  ACL channel which has been previously opened to handle RFCOMM traffic between devices
 */
83
void RFCOMM_ServiceChannels(Bluetooth_Channel_t* const ACLChannel)
84
{
85
86
87
88
89
	/* Abort if the RFCOMM ACL channel is not currently open */
	if ((ACLChannel == NULL) || (ACLChannel->State != BT_Channel_Open))
	  return;

	/* Loop through each of the RFCOMM channels, send any required RFCOMM control commands */
90
91
92
93
94
95
96
97
98
99
100
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	{
		RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];

		if (RFCOMMChannel->State == RFCOMM_Channel_Configure)
		{
			/* Check if the local signals have not yet been sent on the current channel */
			if (!(RFCOMMChannel->ConfigFlags & RFCOMM_CONFIG_LOCALSIGNALSSENT))
			{
				/* Indicate that the local signals have been sent, transmit them to the remote device */
				RFCOMMChannel->ConfigFlags |= RFCOMM_CONFIG_LOCALSIGNALSSENT;
101
				RFCOMM_SendChannelSignals(RFCOMMChannel, ACLChannel);
102
103
104
105
106
107
108
			}

			/* If signals have been configured in both directions, progress to the open state */
			if ((RFCOMMChannel->ConfigFlags & (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS)) ==
			                                  (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS))
			{
				RFCOMMChannel->State = RFCOMM_Channel_Open;
109
				RFCOMM_ChannelOpened(RFCOMMChannel);
110
111
112
113
114
			}
		}
	}
}

115
116
117
118
119
120
/** Processes an incoming RFCOMM packet on an ACL channel which has been previously opened between the local and
 *  a remote device to handle RFCOMM traffic.
 *
 *  \param[in] Data        Incomming packet data containing the RFCOMM packet
 *  \param[in] ACLChannel  ACL channel the request was issued to by the remote device
 */
121
void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const ACLChannel)
122
{
123
124
	const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
	const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
125
	uint16_t               FrameDataLen = RFCOMM_GetVariableFieldValue(&FrameData);
126
127
128
129
130
	
	/* Decode the RFCOMM frame type from the header */
	switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
	{
		case RFCOMM_Frame_DM:
131
			RFCOMM_ProcessDM(&FrameHeader->Address, ACLChannel);
132
133
			break;
		case RFCOMM_Frame_DISC:
134
			RFCOMM_ProcessDISC(&FrameHeader->Address, ACLChannel);
135
			break;
136
		case RFCOMM_Frame_SABM:
137
			RFCOMM_ProcessSABM(&FrameHeader->Address, ACLChannel);
138
139
			break;
		case RFCOMM_Frame_UA:
140
			RFCOMM_ProcessUA(&FrameHeader->Address, ACLChannel);
141
			break;
142
		case RFCOMM_Frame_UIH:
143
			RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, ACLChannel);
144
145
			break;
		default:
146
			BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
147
148
149
150
			break;
	}
}

151
152
153
154
155
156
/** Sends an RFCOMM notification to the remote device that the local terminal control signals (located in the
 *  "Local" structure of the RFCOMM channel) have changed, pushing the new signals to the remote device.
 *
 *  \param[in] RFCOMMChannel  RFCOMM logical channel whose local terminal signals have changed
 *  \param[in] ACLChannel     ACL channel which has been opened to carry RFCOMM traffic between devices
 */
157
void RFCOMM_SendChannelSignals(const RFCOMM_Channel_t* const RFCOMMChannel, Bluetooth_Channel_t* const ACLChannel)
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
{
	BT_RFCOMM_DEBUG(1, ">> MSC Command");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI);

	struct
	{
		RFCOMM_Command_t        CommandHeader;
		uint8_t                 Length;
		RFCOMM_MSC_Parameters_t Params;
	} MSCommand;
	
	MSCommand.CommandHeader      = (RFCOMM_Command_t){.Command = RFCOMM_Control_ModemStatus, .EA = true, .CR = true};
	MSCommand.Length             = (sizeof(MSCommand.Params) << 1) | 0x01;
	MSCommand.Params.Channel     = (RFCOMM_Address_t){.DLCI = RFCOMMChannel->DLCI, .EA = true, .CR = true};
	MSCommand.Params.Signals     = RFCOMMChannel->Local.Signals;
	MSCommand.Params.BreakSignal = RFCOMMChannel->Local.BreakSignal;

	/* Send the MSC command to the remote device */
176
	RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, true, RFCOMM_Frame_UIH, sizeof(MSCommand), &MSCommand, ACLChannel);	
177
178
}

179
180
181
182
183
184
185
186
/** Sends new data through an open logical RFCOMM channel. This should be used to transmit data through a
 *  RFCOMM channel once it has been opened.
 *
 *  \param[in] DataLen        Length of the RFCOMM data to send, in bytes
 *  \param[in] Data           Pointer to a buffer where the data to send is located
 *  \param[in] RFCOMMChannel  RFCOMM logical channel which is to be transmitted to
 *  \param[in] ACLChannel     ACL channel which has been opened to carry RFCOMM traffic between devices
 */
187
void RFCOMM_SendData(const uint16_t DataLen, const uint8_t* Data, const RFCOMM_Channel_t* const RFCOMMChannel,
188
                     Bluetooth_Channel_t* const ACLChannel)
189
190
191
192
193
194
195
196
{
	if (RFCOMMChannel->State != RFCOMM_Channel_Open)
	  return;
	  
	BT_RFCOMM_DEBUG(1, ">> UIH Frame");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI);

	/* Send the MSC command to the remote device */
197
	RFCOMM_SendFrame(RFCOMMChannel->DLCI, false, RFCOMM_Frame_UIH, DataLen, Data, ACLChannel);		
198
199
}

200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
RFCOMM_Channel_t* RFCOMM_GetFreeChannelEntry(const uint8_t DLCI)
{
	/* Find a free entry in the RFCOMM channel multiplexer state array */
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	{
		RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];

		/* If the channel's state is closed, the channel state entry is free */
		if (RFCOMMChannel->State == RFCOMM_Channel_Closed)
		{
			RFCOMMChannel->DLCI               = DLCI;
			RFCOMMChannel->State              = RFCOMM_Channel_Configure;
			RFCOMMChannel->Priority           = 7 + (RFCOMMChannel->DLCI & 0xF8);
			RFCOMMChannel->MTU                = 0xFFFF;
			RFCOMMChannel->Remote.Signals     = 0 | (1 << 0);
			RFCOMMChannel->Remote.BreakSignal = 0 | (1 << 0);
			RFCOMMChannel->Local.Signals      = RFCOMM_SIGNAL_RTC | RFCOMM_SIGNAL_RTR | RFCOMM_SIGNAL_DV | (1 << 0);
			RFCOMMChannel->Local.BreakSignal  = 0 | (1 << 0);
			RFCOMMChannel->ConfigFlags        = 0;
			
			return RFCOMMChannel;
		}
	}
	
	return NULL;
}

227
RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
228
{
229
	/* Search through the RFCOMM channel list, looking for the specified channel */
230
231
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	{
232
		RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
233
	
234
		/* If the current non-closed channel's DLCI matches the search DLCI, return it to the caller */
235
		if ((CurrRFCOMMChannel->State != RFCOMM_Channel_Closed) && (CurrRFCOMMChannel->DLCI == DLCI))
236
		  return CurrRFCOMMChannel;
237
	}
238
239

	/* Channel not found in the channel state table, return failure */
240
	return NULL;
241
242
}

243
uint16_t RFCOMM_GetVariableFieldValue(const uint8_t** BufferPos)
244
{
245
	uint8_t FirstOctet;
246
247
	uint8_t SecondOctet = 0;
	
248
249
	FirstOctet = **BufferPos;
	(*BufferPos)++;
250
	
251
252
253
254
255
256
257
258
259
260
261
262
	/* If the field size is more than a single byte, fetch the next byte in the variable length field */
	if (!(FirstOctet & 0x01))
	{
		SecondOctet = **BufferPos;
		(*BufferPos)++;

		/* Discard any remaining bytes in the variable length field that won't fit in the return value */
		while (!(**BufferPos & 0x01))
		  (*BufferPos)++;
	}

	/* Bitshift the bytes that comprise the variable length field so that they form a single integer */
263
	return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
264
265
}

266
void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
267
                      const void* Data, Bluetooth_Channel_t* const ACLChannel)
268
269
270
271
{
	struct
	{
		RFCOMM_Header_t FrameHeader;
272
		uint8_t         Size[(DataLen < 128) ? 1 : 2];
273
274
275
276
277
		uint8_t         Data[DataLen];
		uint8_t         FCS;
	} ResponsePacket;
	
	/* Set the frame header values to the specified address and frame type */
278
	ResponsePacket.FrameHeader.Control = Control;
279
	ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t){.DLCI = DLCI, .EA   = true, .CR = CommandResponse};
280
281
282
283
284
285
286
287
288
289
290
291
292
	
	/* Set the lower 7 bits of the packet length */
	ResponsePacket.Size[0] = (DataLen << 1);
	
	/* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
	if (DataLen < 128)
	  ResponsePacket.Size[0] |= 0x01;
	else
	  ResponsePacket.Size[1]  = (DataLen >> 7);
	
	/* Copy over the packet data from the source buffer to the response packet buffer */
	memcpy(ResponsePacket.Data, Data, DataLen);
	
293
294
295
	/* Determine the length of the frame which is to be used to calculate the CRC value */
	uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);

296
	/* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
297
298
299
	if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
	  CRCLength += sizeof(ResponsePacket.Size);
	
300
	/* Calculate the frame checksum from the appropriate fields */
301
	ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
302

303
	/* Send the completed response packet to the sender */
304
	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), ACLChannel);
305
306
}

307
static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
308
{
309
	uint8_t FCS = 0xFF;
310
	
311
	/* Calculate new Frame CRC value via the given data bytes and the CRC table */
312
	for (uint8_t i = 0; i < Length; i++)
313
	  FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((const uint8_t*)FrameStart)[i]]);
314

315
	return ~FCS;
316
317
}

318
static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
319
{
320
321
322
323
	BT_RFCOMM_DEBUG(1, "<< DM Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
}

324
static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
325
326
327
328
329
{
	BT_RFCOMM_DEBUG(1, "<< DISC Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);

	RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
330
	
331
332
	/* If the requested channel is currently open, destroy it */
	if (RFCOMMChannel != NULL)
333
	  RFCOMMChannel->State = RFCOMM_Channel_Closed;
334
335

	BT_RFCOMM_DEBUG(1, ">> UA Sent");
336
	RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
337
338
}

339
static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
340
{
341
342
	BT_RFCOMM_DEBUG(1, "<< SABM Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
343

344
	if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
345
	{
346
347
348
		BT_RFCOMM_DEBUG(1, ">> UA Sent");
		
		/* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
349
		RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
350
351

		return;
352
	}
353
354
355

	/* Find the existing channel's entry in the channel table */
	RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
356
	
357
358
359
360
361
362
	/* Existing entry not found, create a new entry for the channel */
	if (RFCOMMChannel == NULL)
	  RFCOMMChannel = RFCOMM_GetFreeChannelEntry(FrameAddress->DLCI);

	/* If space was found in the channel table for the new channel, ACK the request */
	if (RFCOMMChannel != NULL)
363
364
365
366
	{
		BT_RFCOMM_DEBUG(1, ">> UA Sent");
		
		/* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
367
		RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
368
369
370
371
	}
	else
	{
		BT_RFCOMM_DEBUG(1, ">> DM Sent");
372

373
		/* No free channel in the multiplexer - decline the SABM by sending a DM frame */
374
		RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
375
	}
376
377
}

378
static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
379
380
381
382
383
384
{
	BT_RFCOMM_DEBUG(1, "<< UA Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
}

static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength, 
385
                              const uint8_t* FrameData, Bluetooth_Channel_t* const ACLChannel)
386
387
388
{
	if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
	{
389
		RFCOMM_ProcessControlCommand(FrameData, ACLChannel);
390
391
392
		return;
	}

393
394
395
396
	BT_RFCOMM_DEBUG(1, "<< UIH Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
	BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
	
397
	RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
398
	
399
400
	if (RFCOMMChannel != NULL)
	  RFCOMM_DataReceived(RFCOMMChannel, FrameLength, FrameData);
401
}