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].DLCI = 0x00;
  74 }
  75
  76 void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
  77 {
  78         const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
  79         const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
  80         uint16_t               FrameDataLen = RFCOMM_GetFrameDataLength(FrameData);
  81
  82         /* Adjust the frame data pointer to skip over the variable size field */
  83         FrameData += (FrameDataLen < 128) ? 1 : 2;
  84
  85         /* Decode the RFCOMM frame type from the header */
  86         switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
  87         {
  88                 case RFCOMM_Frame_DM:
  89                         RFCOMM_ProcessDM(&FrameHeader->Address, Channel);
  90                         break;
  91                 case RFCOMM_Frame_DISC:
  92                         RFCOMM_ProcessDISC(&FrameHeader->Address, Channel);
  93                         break;
  94                 case RFCOMM_Frame_SABM:
  95                         RFCOMM_ProcessSABM(&FrameHeader->Address, Channel);
  96                         break;
  97                 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                         break;
 103                 default:
 104                         BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
 105                         break;
 106         }
 107 }
 108
 109 static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 110 {
 111         BT_RFCOMM_DEBUG(1, "<< DM Received");
 112         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 113 }
 114
 115 static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 116 {
 117         BT_RFCOMM_DEBUG(1, "<< DISC Received");
 118         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 119
 120         // TODO: Close down connection
 121
 122         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 123         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 124 }
 125
 126 static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 127 {
 128         BT_RFCOMM_DEBUG(1, "<< SABM Received");
 129         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 130
 131         /* Find a free entry in the RFCOMM channel multiplexer state array */
 132         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 133         {
 134                 RFCOMM_Channel_t* CurrRFCOMMChannel = RFCOMM_Channels[i];
 135
 136                 /* If the channel's DLCI is zero, the channel state entry is free */
 137                 if (!(CurrRFCOMMChannel->DLCI))
 138                 {
 139                         CurrRFCOMMChannel->DLCI       = FrameAddress->DLCI;
 140                         CurrRFCOMMChannel->Configured = false;
 141
 142                         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 143                         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 144                         return;
 145                 }
 146         }
 147
 148         BT_RFCOMM_DEBUG(1, ">> DM Sent");
 149
 150         /* No free channel in the multiplexer - decline the SABM by sending a DM frame */
 151         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, Channel);
 152 }
 153
 154 static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 155 {
 156         BT_RFCOMM_DEBUG(1, "<< UA Received");
 157         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 158 }
 159
 160 static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength,
 161                               const uint8_t* FrameData, Bluetooth_Channel_t* const Channel)
 162 {
 163         BT_RFCOMM_DEBUG(1, "<< UIH Received");
 164         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 165         BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
 166
 167         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 168         {
 169                 RFCOMM_ProcessControlCommand((const RFCOMM_Command_t*)FrameData, Channel);
 170                 return;
 171         }
 172
 173         // TODO: Handle regular channel data here
 174 }
 175
 176 static void RFCOMM_ProcessControlCommand(const RFCOMM_Command_t* CommandHeader, Bluetooth_Channel_t* const Channel)
 177 {
 178         const uint8_t* CommandData = (const uint8_t*)Data + sizeof(RFCOMM_Command_t);
 179
 180         switch (CommandHeader->Command)
 181         {
 182                 case RFCOMM_Control_Test:
 183                         BT_RFCOMM_DEBUG(1, "<< TEST Command");
 184                         break;
 185                 case RFCOMM_Control_FlowControlEnable:
 186                         BT_RFCOMM_DEBUG(1, "<< FCE Command");
 187                         break;
 188                 case RFCOMM_Control_FlowControlDisable:
 189                         BT_RFCOMM_DEBUG(1, "<< FCD Command");
 190                         break;
 191                 case RFCOMM_Control_ModemStatus:
 192                         BT_RFCOMM_DEBUG(1, "<< MS Command");
 193                         break;
 194                 case RFCOMM_Control_RemotePortNegotiation:
 195                         BT_RFCOMM_DEBUG(1, "<< RPN Command");
 196                         break;
 197                 case RFCOMM_Control_RemoteLineStatus:
 198                         BT_RFCOMM_DEBUG(1, "<< RLS Command");
 199                         break;
 200                 case RFCOMM_Control_DLCParameterNegotiation:
 201                         BT_RFCOMM_DEBUG(1, "<< DPN Command");
 202
 203                         // TODO - Set channel state
 204 //                      RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(
 205                         RFCOMMChannel->Configured = true;
 206
 207                         // TODO - send ACK/NAK response
 208                         break;
 209                 default:
 210                         BT_RFCOMM_DEBUG(1, "<< Unknown Command");
 211
 212                         struct
 213                         {
 214                                 RFCOMM_Command_t Header;
 215                                 RFCOMM_Command_t Command;
 216                         } Response =
 217                                 {
 218                                         .Header = (RFCOMM_Command_t)
 219                                         {
 220                                                 .Command = RFCOMM_Control_NonSupportedCommand,
 221                                                 .CR      = true,
 222                                                 .EA      = true,
 223                                         },
 224
 225                                         .Command = *CommandHeader,
 226                                 };
 227
 228                         RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, sizeof(RFCOMM_Command_t), &Response, Channel);
 229                         break;
 230         }
 231 }
 232
 233 static void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 234                              const void* Data, Bluetooth_Channel_t* const Channel)
 235 {
 236         struct
 237         {
 238                 RFCOMM_Header_t FrameHeader;
 239                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 240                 uint8_t         Data[DataLen];
 241                 uint8_t         FCS;
 242         } ResponsePacket;
 243
 244         /* Set the frame header values to the specified address and frame type */
 245         ResponsePacket.FrameHeader.Control = Control;
 246         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t)
 247                 {
 248                         .EA   = true,
 249                         .CR   = CommandResponse,
 250                         .DLCI = DLCI,
 251                 };
 252
 253         /* Set the lower 7 bits of the packet length */
 254         ResponsePacket.Size[0] = (DataLen << 1);
 255
 256         /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
 257         if (DataLen < 128)
 258           ResponsePacket.Size[0] |= 0x01;
 259         else
 260           ResponsePacket.Size[1]  = (DataLen >> 7);
 261
 262         /* Copy over the packet data from the source buffer to the response packet buffer */
 263         memcpy(ResponsePacket.Data, Data, DataLen);
 264
 265         /* Determine the length of the frame which is to be used to calculate the CRC value */
 266         uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);
 267
 268         /* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
 269         if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
 270           CRCLength += sizeof(ResponsePacket.Size);
 271
 272         /* Calculate the frame checksum from the appropriate fields */
 273         ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
 274
 275         /* Send the completed response packet to the sender */
 276         Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), Channel);
 277 }
 278
 279 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 280 {
 281         uint8_t FCS = 0xFF;
 282
 283         for (uint8_t i = 0; i < Length; i++)
 284           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
 285
 286         return ~FCS;
 287 }
 288
 289 static uint16_t RFCOMM_GetFrameDataLength(const uint8_t* const BufferPos)
 290 {
 291         uint8_t FirstOctet  = BufferPos[0];
 292         uint8_t SecondOctet = 0;
 293
 294         if (!(FirstOctet & 0x01))
 295           SecondOctet = BufferPos[1];
 296
 297         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 298 }
 299
 300 RFCOMM_Channel_t RFCOMM_GetChannelData(const uint8_t DLCI)
 301 {
 302         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 303         {
 304                 RFCOMM_Channel_t* CurrRFCOMMChannel = RFCOMM_Channels[i];
 305
 306                 if (CurrRFCOMMChannel->DLCI == DLCI)
 307                   return CurrRFCOMMChannel;
 308         }
 309
 310         return NULL;
 311 }