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(FrameData, Channel);
 170                 return;
 171         }
 172
 173         // TODO: Handle regular channel data here
 174 }
 175
 176 static void RFCOMM_ProcessControlCommand(const uint8_t* Command, Bluetooth_Channel_t* const Channel)
 177 {
 178         const RFCOMM_Command_t* CommandHeader  = (const RFCOMM_Command_t*)Command;
 179         const uint8_t*          CommandData    = (const uint8_t*)Command + sizeof(RFCOMM_Command_t);
 180
 181         switch (CommandHeader->Command)
 182         {
 183                 case RFCOMM_Control_Test:
 184                         BT_RFCOMM_DEBUG(1, "<< TEST Command");
 185                         break;
 186                 case RFCOMM_Control_FlowControlEnable:
 187                         BT_RFCOMM_DEBUG(1, "<< FCE Command");
 188                         break;
 189                 case RFCOMM_Control_FlowControlDisable:
 190                         BT_RFCOMM_DEBUG(1, "<< FCD Command");
 191                         break;
 192                 case RFCOMM_Control_ModemStatus:
 193                         BT_RFCOMM_DEBUG(1, "<< MS Command");
 194                         break;
 195                 case RFCOMM_Control_RemotePortNegotiation:
 196                         BT_RFCOMM_DEBUG(1, "<< RPN Command");
 197                         break;
 198                 case RFCOMM_Control_RemoteLineStatus:
 199                         BT_RFCOMM_DEBUG(1, "<< RLS Command");
 200                         break;
 201                 case RFCOMM_Control_DLCParameterNegotiation:
 202                         BT_RFCOMM_DEBUG(1, "<< DPN Command");
 203
 204                         // TODO - Set channel state
 205 //                      RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(
 206 //                      RFCOMMChannel->Configured = true;
 207
 208                         // TODO - send ACK/NAK response
 209                         break;
 210                 default:
 211                         BT_RFCOMM_DEBUG(1, "<< Unknown Command");
 212
 213                         struct
 214                         {
 215                                 RFCOMM_Command_t Header;
 216                                 RFCOMM_Command_t Command;
 217                         } Response =
 218                                 {
 219                                         .Header = (RFCOMM_Command_t)
 220                                         {
 221                                                 .Command = RFCOMM_Control_NonSupportedCommand,
 222                                                 .CR      = true,
 223                                                 .EA      = true,
 224                                         },
 225
 226                                         .Command = *CommandHeader,
 227                                 };
 228
 229                         RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, sizeof(RFCOMM_Command_t), &Response, Channel);
 230                         break;
 231         }
 232 }
 233
 234 static void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 235                              const void* Data, Bluetooth_Channel_t* const Channel)
 236 {
 237         struct
 238         {
 239                 RFCOMM_Header_t FrameHeader;
 240                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 241                 uint8_t         Data[DataLen];
 242                 uint8_t         FCS;
 243         } ResponsePacket;
 244
 245         /* Set the frame header values to the specified address and frame type */
 246         ResponsePacket.FrameHeader.Control = Control;
 247         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t)
 248                 {
 249                         .EA   = true,
 250                         .CR   = CommandResponse,
 251                         .DLCI = DLCI,
 252                 };
 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), Channel);
 278 }
 279
 280 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 281 {
 282         uint8_t FCS = 0xFF;
 283
 284         for (uint8_t i = 0; i < Length; i++)
 285           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
 286
 287         return ~FCS;
 288 }
 289
 290 static uint16_t RFCOMM_GetFrameDataLength(const uint8_t* const BufferPos)
 291 {
 292         uint8_t FirstOctet  = BufferPos[0];
 293         uint8_t SecondOctet = 0;
 294
 295         if (!(FirstOctet & 0x01))
 296           SecondOctet = BufferPos[1];
 297
 298         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 299 }
 300
 301 RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
 302 {
 303         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 304         {
 305                 RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
 306
 307                 if (CurrRFCOMMChannel->DLCI == DLCI)
 308                   return CurrRFCOMMChannel;
 309         }
 310
 311         return NULL;
 312 }