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 BluetoothChannel)
  77 {
  78         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
  79         {
  80                 RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];
  81
  82                 if (RFCOMMChannel->State == RFCOMM_Channel_Configure)
  83                 {
  84                         /* Check if the local signals have not yet been sent on the current channel */
  85                         if (!(RFCOMMChannel->ConfigFlags & RFCOMM_CONFIG_LOCALSIGNALSSENT))
  86                         {
  87                                 /* Indicate that the local signals have been sent, transmit them to the remote device */
  88                                 RFCOMMChannel->ConfigFlags |= RFCOMM_CONFIG_LOCALSIGNALSSENT;
  89                                 RFCOMM_SendChannelSignals(RFCOMMChannel, BluetoothChannel);
  90                         }
  91
  92                         /* If signals have been configured in both directions, progress to the open state */
  93                         if ((RFCOMMChannel->ConfigFlags & (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS)) ==
  94                                                           (RFCOMM_CONFIG_REMOTESIGNALS | RFCOMM_CONFIG_LOCALSIGNALS))
  95                         {
  96                                 RFCOMMChannel->State = RFCOMM_Channel_Open;
  97                         }
  98                 }
  99         }
 100 }
 101
 102 void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
 103 {
 104         const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
 105         const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
 106         uint16_t               FrameDataLen = RFCOMM_GetVariableFieldValue(&FrameData);
 107
 108         /* Decode the RFCOMM frame type from the header */
 109         switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
 110         {
 111                 case RFCOMM_Frame_DM:
 112                         RFCOMM_ProcessDM(&FrameHeader->Address, Channel);
 113                         break;
 114                 case RFCOMM_Frame_DISC:
 115                         RFCOMM_ProcessDISC(&FrameHeader->Address, Channel);
 116                         break;
 117                 case RFCOMM_Frame_SABM:
 118                         RFCOMM_ProcessSABM(&FrameHeader->Address, Channel);
 119                         break;
 120                 case RFCOMM_Frame_UA:
 121                         RFCOMM_ProcessUA(&FrameHeader->Address, Channel);
 122                         break;
 123                 case RFCOMM_Frame_UIH:
 124                         RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, Channel);
 125                         break;
 126                 default:
 127                         BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
 128                         break;
 129         }
 130 }
 131
 132 RFCOMM_Channel_t* RFCOMM_OpenChannel(Bluetooth_Channel_t* const BluetoothChannel)
 133 {
 134         return NULL;
 135 }
 136
 137 void RFCOMM_SendChannelSignals(const RFCOMM_Channel_t* const RFCOMMChannel, Bluetooth_Channel_t* const BluetoothChannel)
 138 {
 139         BT_RFCOMM_DEBUG(1, ">> MSC Command");
 140         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI);
 141
 142         struct
 143         {
 144                 RFCOMM_Command_t        CommandHeader;
 145                 uint8_t                 Length;
 146                 RFCOMM_MSC_Parameters_t Params;
 147         } MSCommand;
 148
 149         MSCommand.CommandHeader      = (RFCOMM_Command_t){.Command = RFCOMM_Control_ModemStatus, .EA = true, .CR = true};
 150         MSCommand.Length             = (sizeof(MSCommand.Params) << 1) | 0x01;
 151         MSCommand.Params.Channel     = (RFCOMM_Address_t){.DLCI = RFCOMMChannel->DLCI, .EA = true, .CR = true};
 152         MSCommand.Params.Signals     = RFCOMMChannel->Local.Signals;
 153         MSCommand.Params.BreakSignal = RFCOMMChannel->Local.BreakSignal;
 154
 155         /* Send the MSC command to the remote device */
 156         RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, true, RFCOMM_Frame_UIH, sizeof(MSCommand), &MSCommand, BluetoothChannel);
 157 }
 158
 159 RFCOMM_Channel_t* RFCOMM_GetFreeChannelEntry(const uint8_t DLCI)
 160 {
 161         /* Find a free entry in the RFCOMM channel multiplexer state array */
 162         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 163         {
 164                 RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i];
 165
 166                 /* If the channel's state is closed, the channel state entry is free */
 167                 if (RFCOMMChannel->State == RFCOMM_Channel_Closed)
 168                 {
 169                         RFCOMMChannel->DLCI               = DLCI;
 170                         RFCOMMChannel->State              = RFCOMM_Channel_Configure;
 171                         RFCOMMChannel->Priority           = 7 + (RFCOMMChannel->DLCI & 0xF8);
 172                         RFCOMMChannel->MTU                = 0xFFFF;
 173                         RFCOMMChannel->Remote.Signals     = 0 | (1 << 0);
 174                         RFCOMMChannel->Remote.BreakSignal = 0 | (1 << 0);
 175                         RFCOMMChannel->Local.Signals      = RFCOMM_SIGNAL_RTC | RFCOMM_SIGNAL_RTR | RFCOMM_SIGNAL_DV | (1 << 0);
 176                         RFCOMMChannel->Local.BreakSignal  = 0 | (1 << 0);
 177                         RFCOMMChannel->ConfigFlags        = 0;
 178
 179                         return RFCOMMChannel;
 180                 }
 181         }
 182
 183         return NULL;
 184 }
 185
 186 RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
 187 {
 188         /* Search through the RFCOMM channel list, looking for the specified channel */
 189         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 190         {
 191                 RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
 192
 193                 /* If the current non-closed channel's DLCI matches the search DLCI, return it to the caller */
 194                 if ((CurrRFCOMMChannel->State != RFCOMM_Channel_Closed) && (CurrRFCOMMChannel->DLCI == DLCI))
 195                   return CurrRFCOMMChannel;
 196         }
 197
 198         /* Channel not found in the channel state table, return failure */
 199         return NULL;
 200 }
 201
 202 uint16_t RFCOMM_GetVariableFieldValue(const uint8_t** BufferPos)
 203 {
 204         uint8_t FirstOctet;
 205         uint8_t SecondOctet = 0;
 206
 207         FirstOctet = **BufferPos;
 208         (*BufferPos)++;
 209
 210         /* If the field size is more than a single byte, fetch the next byte in the variable length field */
 211         if (!(FirstOctet & 0x01))
 212         {
 213                 SecondOctet = **BufferPos;
 214                 (*BufferPos)++;
 215
 216                 /* Discard any remaining bytes in the variable length field that won't fit in the return value */
 217                 while (!(**BufferPos & 0x01))
 218                   (*BufferPos)++;
 219         }
 220
 221         /* Bitshift the bytes that comprise the variable length field so that they form a single integer */
 222         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 223 }
 224
 225 void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 226                       const void* Data, Bluetooth_Channel_t* const Channel)
 227 {
 228         struct
 229         {
 230                 RFCOMM_Header_t FrameHeader;
 231                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 232                 uint8_t         Data[DataLen];
 233                 uint8_t         FCS;
 234         } ResponsePacket;
 235
 236         /* Set the frame header values to the specified address and frame type */
 237         ResponsePacket.FrameHeader.Control = Control;
 238         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t){.DLCI = DLCI, .EA   = true, .CR = CommandResponse};
 239
 240         /* Set the lower 7 bits of the packet length */
 241         ResponsePacket.Size[0] = (DataLen << 1);
 242
 243         /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
 244         if (DataLen < 128)
 245           ResponsePacket.Size[0] |= 0x01;
 246         else
 247           ResponsePacket.Size[1]  = (DataLen >> 7);
 248
 249         /* Copy over the packet data from the source buffer to the response packet buffer */
 250         memcpy(ResponsePacket.Data, Data, DataLen);
 251
 252         /* Determine the length of the frame which is to be used to calculate the CRC value */
 253         uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);
 254
 255         /* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
 256         if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
 257           CRCLength += sizeof(ResponsePacket.Size);
 258
 259         /* Calculate the frame checksum from the appropriate fields */
 260         ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
 261
 262         /* Send the completed response packet to the sender */
 263         Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), Channel);
 264 }
 265
 266 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 267 {
 268         uint8_t FCS = 0xFF;
 269
 270         /* Calculate new Frame CRC value via the given data bytes and the CRC table */
 271         for (uint8_t i = 0; i < Length; i++)
 272           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
 273
 274         return ~FCS;
 275 }
 276
 277 static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 278 {
 279         BT_RFCOMM_DEBUG(1, "<< DM Received");
 280         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 281 }
 282
 283 static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 284 {
 285         BT_RFCOMM_DEBUG(1, "<< DISC Received");
 286         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 287
 288         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 289
 290         /* If the requested channel is currently open, destroy it */
 291         if (RFCOMMChannel != NULL)
 292           RFCOMMChannel->State = RFCOMM_Channel_Closed;
 293
 294         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 295         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 296 }
 297
 298 static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 299 {
 300         BT_RFCOMM_DEBUG(1, "<< SABM Received");
 301         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 302
 303         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 304         {
 305                 BT_RFCOMM_DEBUG(1, ">> UA Sent");
 306
 307                 /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
 308                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 309
 310                 return;
 311         }
 312
 313         /* Find the existing channel's entry in the channel table */
 314         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 315
 316         /* Existing entry not found, create a new entry for the channel */
 317         if (RFCOMMChannel == NULL)
 318           RFCOMMChannel = RFCOMM_GetFreeChannelEntry(FrameAddress->DLCI);
 319
 320         /* If space was found in the channel table for the new channel, ACK the request */
 321         if (RFCOMMChannel != NULL)
 322         {
 323                 BT_RFCOMM_DEBUG(1, ">> UA Sent");
 324
 325                 /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */
 326                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 327         }
 328         else
 329         {
 330                 BT_RFCOMM_DEBUG(1, ">> DM Sent");
 331
 332                 /* No free channel in the multiplexer - decline the SABM by sending a DM frame */
 333                 RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, Channel);
 334         }
 335 }
 336
 337 static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 338 {
 339         BT_RFCOMM_DEBUG(1, "<< UA Received");
 340         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 341 }
 342
 343 static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength,
 344                               const uint8_t* FrameData, Bluetooth_Channel_t* const Channel)
 345 {
 346         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 347         {
 348                 RFCOMM_ProcessControlCommand(FrameData, Channel);
 349                 return;
 350         }
 351
 352         BT_RFCOMM_DEBUG(1, "<< UIH Received");
 353         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 354         BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
 355
 356         puts("RFCOMM Data: ");
 357
 358         for (uint8_t i = 0; i < FrameLength; i++)
 359           printf("0x%02X (%c) ", FrameData[i], FrameData[i]);
 360
 361         printf("\r\n");
 362 }