RFCOMM.c 9.48 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
73
	/* Reset the RFCOMM channel structures, to invalidate any confiured RFCOMM channels */
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	  RFCOMM_Channels[i].DLCI = 0x00;
74
75
76
77
}

void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
{
78
79
80
81
	const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
	const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
	uint16_t               FrameDataLen = RFCOMM_GetFrameDataLength(FrameData);

82
	/* Adjust the frame data pointer to skip over the variable size field */
83
	FrameData += (FrameDataLen < 128) ? 1 : 2;
84
85
86
87
88
	
	/* Decode the RFCOMM frame type from the header */
	switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
	{
		case RFCOMM_Frame_DM:
89
			RFCOMM_ProcessDM(&FrameHeader->Address, Channel);
90
91
			break;
		case RFCOMM_Frame_DISC:
92
			RFCOMM_ProcessDISC(&FrameHeader->Address, Channel);
93
			break;
94
		case RFCOMM_Frame_SABM:
95
			RFCOMM_ProcessSABM(&FrameHeader->Address, Channel);
96
97
			break;
		case RFCOMM_Frame_UA:
98
			RFCOMM_ProcessUA(&FrameHeader->Address, Channel);
99
			break;
100
		case RFCOMM_Frame_UIH:
101
			RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, Channel);
102
103
			break;
		default:
104
			BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
105
106
107
108
			break;
	}
}

109
RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
110
{
111
112
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	{
113
		RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
114
	
115
116
		if (CurrRFCOMMChannel->DLCI == DLCI)
		  return CurrRFCOMMChannel;
117
	}
118
	
119
	return NULL;
120
121
}

122
uint16_t RFCOMM_GetFrameDataLength(const uint8_t* const BufferPos)
123
{
124
125
126
127
128
129
130
	uint8_t FirstOctet  = BufferPos[0];
	uint8_t SecondOctet = 0;
	
	if (!(FirstOctet & 0x01))
	  SecondOctet = BufferPos[1];
	
	return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
131
132
}

133
134
void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
                      const void* Data, Bluetooth_Channel_t* const Channel)
135
136
137
138
{
	struct
	{
		RFCOMM_Header_t FrameHeader;
139
		uint8_t         Size[(DataLen < 128) ? 1 : 2];
140
141
142
143
144
		uint8_t         Data[DataLen];
		uint8_t         FCS;
	} ResponsePacket;
	
	/* Set the frame header values to the specified address and frame type */
145
146
147
148
149
150
151
	ResponsePacket.FrameHeader.Control = Control;
	ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t)
		{
			.EA   = true,
			.CR   = CommandResponse,
			.DLCI = DLCI,
		};
152
153
154
155
156
157
158
159
160
161
162
163
164
	
	/* 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);
	
165
166
167
	/* Determine the length of the frame which is to be used to calculate the CRC value */
	uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);

168
	/* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
169
170
171
	if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
	  CRCLength += sizeof(ResponsePacket.Size);
	
172
	/* Calculate the frame checksum from the appropriate fields */
173
	ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
174

175
176
177
178
	/* Send the completed response packet to the sender */
	Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), Channel);
}

179
static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
180
{
181
	uint8_t FCS = 0xFF;
182
	
183
	for (uint8_t i = 0; i < Length; i++)
184
	  FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
185

186
	return ~FCS;
187
188
}

189
static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
190
{
191
192
193
194
195
196
197
198
199
200
	BT_RFCOMM_DEBUG(1, "<< DM Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
}

static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
{
	BT_RFCOMM_DEBUG(1, "<< DISC Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);

	RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
201
	
202
203
204
205
206
207
	/* If the requested channel is currently open, destroy it */
	if (RFCOMMChannel != NULL)
	  RFCOMMChannel->DLCI = 0x00;

	BT_RFCOMM_DEBUG(1, ">> UA Sent");
	RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
208
209
}

210
static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
211
{
212
213
214
215
	BT_RFCOMM_DEBUG(1, "<< SABM Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
	
	/* Find a free entry in the RFCOMM channel multiplexer state array */
216
217
	for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
	{
218
		RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
219
	
220
221
222
223
224
225
226
227
228
229
		/* If the channel's DLCI is zero, the channel state entry is free */
		if (!(CurrRFCOMMChannel->DLCI))
		{
			CurrRFCOMMChannel->DLCI       = FrameAddress->DLCI;
			CurrRFCOMMChannel->Configured = false;
		
			BT_RFCOMM_DEBUG(1, ">> UA Sent");
			RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
			return;
		}
230
	}
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249

	BT_RFCOMM_DEBUG(1, ">> DM Sent");

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

static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
{
	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, 
                              const uint8_t* FrameData, Bluetooth_Channel_t* const Channel)
{
	BT_RFCOMM_DEBUG(1, "<< UIH Received");
	BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
	BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
250
	
251
252
253
254
255
256
257
	if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
	{
		RFCOMM_ProcessControlCommand(FrameData, Channel);
		return;
	}

	// TODO: Handle regular channel data here
258
}