Demos/Host/Incomplete/BluetoothHost/Lib/RFCOMM.c

   1 /*
   2              LUFA Library
   3      Copyright (C) Dean Camera, 2010.
   4
   5   dean [at] fourwalledcubicle [dot] com
   6       www.fourwalledcubicle.com
   7 */
   8
   9 /*
  10   Copyright 2010  Dean Camera (dean [at] fourwalledcubicle [dot] com)
  11
  12   Permission to use, copy, modify, distribute, and sell this
  13   software and its documentation for any purpose is hereby granted
  14   without fee, provided that the above copyright notice appear in
  15   all copies and that both that the copyright notice and this
  16   permission notice and warranty disclaimer appear in supporting
  17   documentation, and that the name of the author not be used in
  18   advertising or publicity pertaining to distribution of the
  19   software without specific, written prior permission.
  20
  21   The author disclaim all warranties with regard to this
  22   software, including all implied warranties of merchantability
  23   and fitness.  In no event shall the author be liable for any
  24   special, indirect or consequential damages or any damages
  25   whatsoever resulting from loss of use, data or profits, whether
  26   in an action of contract, negligence or other tortious action,
  27   arising out of or in connection with the use or performance of
  28   this software.
  29 */
  30
  31 /** \file
  32  *
  33  *  RFCOMM layer module. This module manages the RFCOMM layer of the
  34  *  stack, providing virtual serial port channels on top of the lower
  35  *  L2CAP layer.
  36  */
  37
  38 #define  INCLUDE_FROM_RFCOMM_C
  39 #include "RFCOMM.h"
  40
  41 /** 8-Bit CRC table used by the FCS field of each RFCOMM encoded frame, sourced from the ETSI TS 101 369 V7.2.0
  42  *  specification document, upon which the RFCOMM specification is based.
  43  */
  44 const uint8_t CRC8_Table[256] PROGMEM =
  45         {
  46                 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
  47                 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
  48                 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
  49                 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
  50                 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
  51                 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
  52                 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
  53                 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
  54                 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
  55                 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
  56                 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
  57                 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
  58                 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
  59                 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
  60                 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
  61                 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
  62         };
  63
  64 /** RFCOMM channel state structure, to retain information about each open channel in the RFCOMM multiplexer. */
  65 RFCOMM_Channel_t RFCOMM_Channels[RFCOMM_MAX_OPEN_CHANNELS];
  66
  67
  68 /** Initializes the RFCOMM service, ready for new connections from a SDP client. */
  69 void RFCOMM_Initialize(void)
  70 {
  71         /* Reset the RFCOMM channel structures, to invalidate any confiured RFCOMM channels */
  72         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
  73           RFCOMM_Channels[i].State = RFCOMM_Channel_Closed;
  74 }
  75
  76 void RFCOMM_ServiceChannels(Bluetooth_Channel_t* const ACLChannel)
  77 {
  78         /* Abort if the RFCOMM ACL channel is not currently open */
  79         if ((ACLChannel == NULL) || (ACLChannel->State != BT_Channel_Open))
  80           return;
  81
  82         /* Loop through each of the RFCOMM channels, send any required RFCOMM control commands */
  83         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
  84         {
  85                 RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];
  86
  87                 if (RFCOMMChannel->State == RFCOMM_Channel_Configure)
  88                 {
  89                         /* Check if the local signals have not yet been sent on the current channel */
  90                         if (!(RFCOMMChannel->ConfigFlags & RFCOMM_CONFIG_LOCALSIGNALSSENT))
  91                         {
  92                                 /* Indicate that the local signals have been sent, transmit them to the remote device */
  93                                 RFCOMMChannel->ConfigFlags |= RFCOMM_CONFIG_LOCALSIGNALSSENT;
  94                                 RFCOMM_SendChannelSignals(RFCOMMChannel, ACLChannel);
  95                         }
  96
  97                         /* If signals have been configured in both directions, progress to the open state */
  98                         if ((RFCOMMChannel->ConfigFlags & (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS)) ==
  99                                                           (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS))
 100                         {
 101                                 RFCOMMChannel->State = RFCOMM_Channel_Open;
 102                                 RFCOMM_ChannelOpened(RFCOMMChannel);
 103                         }
 104                 }
 105         }
 106 }
 107
 108 void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const ACLChannel)
 109 {
 110         const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
 111         const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
 112         uint16_t               FrameDataLen = RFCOMM_GetVariableFieldValue(&FrameData);
 113
 114         /* Decode the RFCOMM frame type from the header */
 115         switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
 116         {
 117                 case RFCOMM_Frame_DM:
 118                         RFCOMM_ProcessDM(&FrameHeader->Address, ACLChannel);
 119                         break;
 120                 case RFCOMM_Frame_DISC:
 121                         RFCOMM_ProcessDISC(&FrameHeader->Address, ACLChannel);
 122                         break;
 123                 case RFCOMM_Frame_SABM:
 124                         RFCOMM_ProcessSABM(&FrameHeader->Address, ACLChannel);
 125                         break;
 126                 case RFCOMM_Frame_UA:
 127                         RFCOMM_ProcessUA(&FrameHeader->Address, ACLChannel);
 128                         break;
 129                 case RFCOMM_Frame_UIH:
 130                         RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, ACLChannel);
 131                         break;
 132                 default:
 133                         BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
 134                         break;
 135         }
 136 }
 137
 138 void RFCOMM_SendChannelSignals(const RFCOMM_Channel_t* const RFCOMMChannel, Bluetooth_Channel_t* const ACLChannel)
 139 {
 140         BT_RFCOMM_DEBUG(1, ">> MSC Command");
 141         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI);
 142
 143         struct
 144         {
 145                 RFCOMM_Command_t        CommandHeader;
 146                 uint8_t                 Length;
 147                 RFCOMM_MSC_Parameters_t Params;
 148         } MSCommand;
 149
 150         MSCommand.CommandHeader      = (RFCOMM_Command_t){.Command = RFCOMM_Control_ModemStatus, .EA = true, .CR = true};
 151         MSCommand.Length             = (sizeof(MSCommand.Params) << 1) | 0x01;
 152         MSCommand.Params.Channel     = (RFCOMM_Address_t){.DLCI = RFCOMMChannel->DLCI, .EA = true, .CR = true};
 153         MSCommand.Params.Signals     = RFCOMMChannel->Local.Signals;
 154         MSCommand.Params.BreakSignal = RFCOMMChannel->Local.BreakSignal;
 155
 156         /* Send the MSC command to the remote device */
 157         RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, true, RFCOMM_Frame_UIH, sizeof(MSCommand), &MSCommand, ACLChannel);
 158 }
 159
 160 void RFCOMM_SendData(const uint16_t DataLen, const uint8_t* Data, const RFCOMM_Channel_t* const RFCOMMChannel,
 161                      Bluetooth_Channel_t* const ACLChannel)
 162 {
 163         if (RFCOMMChannel->State != RFCOMM_Channel_Open)
 164           return;
 165
 166         BT_RFCOMM_DEBUG(1, ">> UIH Frame");
 167         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI);
 168
 169         /* Send the MSC command to the remote device */
 170         RFCOMM_SendFrame(RFCOMMChannel->DLCI, false, RFCOMM_Frame_UIH, DataLen, Data, ACLChannel);
 171 }
 172
 173 RFCOMM_Channel_t* RFCOMM_GetFreeChannelEntry(const uint8_t DLCI)
 174 {
 175         /* Find a free entry in the RFCOMM channel multiplexer state array */
 176         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 177         {
 178                 RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];
 179
 180                 /* If the channel's state is closed, the channel state entry is free */
 181                 if (RFCOMMChannel->State == RFCOMM_Channel_Closed)
 182                 {
 183                         RFCOMMChannel->DLCI               = DLCI;
 184                         RFCOMMChannel->State              = RFCOMM_Channel_Configure;
 185                         RFCOMMChannel->Priority           = 7 + (RFCOMMChannel->DLCI & 0xF8);
 186                         RFCOMMChannel->MTU                = 0xFFFF;
 187                         RFCOMMChannel->Remote.Signals     = 0 | (1 << 0);
 188                         RFCOMMChannel->Remote.BreakSignal = 0 | (1 << 0);
 189                         RFCOMMChannel->Local.Signals      = RFCOMM_SIGNAL_RTC | RFCOMM_SIGNAL_RTR | RFCOMM_SIGNAL_DV | (1 << 0);
 190                         RFCOMMChannel->Local.BreakSignal  = 0 | (1 << 0);
 191                         RFCOMMChannel->ConfigFlags        = 0;
 192
 193                         return RFCOMMChannel;
 194                 }
 195         }
 196
 197         return NULL;
 198 }
 199
 200 RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
 201 {
 202         /* Search through the RFCOMM channel list, looking for the specified channel */
 203         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 204         {
 205                 RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
 206
 207                 /* If the current non-closed channel's DLCI matches the search DLCI, return it to the caller */
 208                 if ((CurrRFCOMMChannel->State != RFCOMM_Channel_Closed) && (CurrRFCOMMChannel->DLCI == DLCI))
 209                   return CurrRFCOMMChannel;
 210         }
 211
 212         /* Channel not found in the channel state table, return failure */
 213         return NULL;
 214 }
 215
 216 uint16_t RFCOMM_GetVariableFieldValue(const uint8_t** BufferPos)
 217 {
 218         uint8_t FirstOctet;
 219         uint8_t SecondOctet = 0;
 220
 221         FirstOctet = **BufferPos;
 222         (*BufferPos)++;
 223
 224         /* If the field size is more than a single byte, fetch the next byte in the variable length field */
 225         if (!(FirstOctet & 0x01))
 226         {
 227                 SecondOctet = **BufferPos;
 228                 (*BufferPos)++;
 229
 230                 /* Discard any remaining bytes in the variable length field that won't fit in the return value */
 231                 while (!(**BufferPos & 0x01))
 232                   (*BufferPos)++;
 233         }
 234
 235         /* Bitshift the bytes that comprise the variable length field so that they form a single integer */
 236         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 237 }
 238
 239 void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 240                       const void* Data, Bluetooth_Channel_t* const ACLChannel)
 241 {
 242         struct
 243         {
 244                 RFCOMM_Header_t FrameHeader;
 245                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 246                 uint8_t         Data[DataLen];
 247                 uint8_t         FCS;
 248         } ResponsePacket;
 249
 250         /* Set the frame header values to the specified address and frame type */
 251         ResponsePacket.FrameHeader.Control = Control;
 252         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t){.DLCI = DLCI, .EA   = true, .CR = CommandResponse};
 253
 254         /* Set the lower 7 bits of the packet length */
 255         ResponsePacket.Size[0] = (DataLen << 1);
 256
 257         /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
 258         if (DataLen < 128)
 259           ResponsePacket.Size[0] |= 0x01;
 260         else
 261           ResponsePacket.Size[1]  = (DataLen >> 7);
 262
 263         /* Copy over the packet data from the source buffer to the response packet buffer */
 264         memcpy(ResponsePacket.Data, Data, DataLen);
 265
 266         /* Determine the length of the frame which is to be used to calculate the CRC value */
 267         uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);
 268
 269         /* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
 270         if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
 271           CRCLength += sizeof(ResponsePacket.Size);
 272
 273         /* Calculate the frame checksum from the appropriate fields */
 274         ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
 275
 276         /* Send the completed response packet to the sender */
 277         Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), ACLChannel);
 278 }
 279
 280 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 281 {
 282         uint8_t FCS = 0xFF;
 283
 284         /* Calculate new Frame CRC value via the given data bytes and the CRC table */
 285         for (uint8_t i = 0; i < Length; i++)
 286           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((const uint8_t*)FrameStart)[i]]);
 287
 288         return ~FCS;
 289 }
 290
 291 static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
 292 {
 293         BT_RFCOMM_DEBUG(1, "<< DM Received");
 294         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 295 }
 296
 297 static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
 298 {
 299         BT_RFCOMM_DEBUG(1, "<< DISC Received");
 300         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 301
 302         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 303
 304         /* If the requested channel is currently open, destroy it */
 305         if (RFCOMMChannel != NULL)
 306           RFCOMMChannel->State = RFCOMM_Channel_Closed;
 307
 308         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 309         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
 310 }
 311
 312 static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
 313 {
 314         BT_RFCOMM_DEBUG(1, "<< SABM Received");
 315         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 316
 317         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 318         {
 319                 BT_RFCOMM_DEBUG(1, ">> UA Sent");
 320
 321                 /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
 322                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
 323
 324                 return;
 325         }
 326
 327         /* Find the existing channel's entry in the channel table */
 328         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 329
 330         /* Existing entry not found, create a new entry for the channel */
 331         if (RFCOMMChannel == NULL)
 332           RFCOMMChannel = RFCOMM_GetFreeChannelEntry(FrameAddress->DLCI);
 333
 334         /* If space was found in the channel table for the new channel, ACK the request */
 335         if (RFCOMMChannel != NULL)
 336         {
 337                 BT_RFCOMM_DEBUG(1, ">> UA Sent");
 338
 339                 /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
 340                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
 341         }
 342         else
 343         {
 344                 BT_RFCOMM_DEBUG(1, ">> DM Sent");
 345
 346                 /* No free channel in the multiplexer - decline the SABM by sending a DM frame */
 347                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, ACLChannel);
 348         }
 349 }
 350
 351 static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel)
 352 {
 353         BT_RFCOMM_DEBUG(1, "<< UA Received");
 354         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 355 }
 356
 357 static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength,
 358                               const uint8_t* FrameData, Bluetooth_Channel_t* const ACLChannel)
 359 {
 360         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 361         {
 362                 RFCOMM_ProcessControlCommand(FrameData, ACLChannel);
 363                 return;
 364         }
 365
 366         BT_RFCOMM_DEBUG(1, "<< UIH Received");
 367         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 368         BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
 369
 370         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 371
 372         if (RFCOMMChannel != NULL)
 373           RFCOMM_DataReceived(RFCOMMChannel, FrameLength, FrameData);
 374 }