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 /** Initializes the RFCOMM service, ready for new connections from a SDP client. */
  65 void RFCOMM_Initialize(void)
  66 {
  67         /* Not currently used */
  68 }
  69
  70 void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
  71 {
  72         const RFCOMM_Header_t* FrameHeader  = (const RFCOMM_Header_t*)Data;
  73         const uint8_t*         FrameData    = (const uint8_t*)Data + sizeof(RFCOMM_Header_t);
  74         uint16_t               FrameDataLen = RFCOMM_GetFrameDataLength(FrameData);
  75
  76         /* Adjust the frame data pointer to skip over the variable size field */
  77         FrameData += (FrameDataLen < 128) ? 1 : 2;
  78
  79         /* Decode the RFCOMM frame type from the header */
  80         switch (FrameHeader->Control & ~FRAME_POLL_FINAL)
  81         {
  82                 case RFCOMM_Frame_DM:
  83                         RFCOMM_ProcessDM(&FrameHeader->Address, Channel);
  84                         break;
  85                 case RFCOMM_Frame_DISC:
  86                         RFCOMM_ProcessDISC(&FrameHeader->Address, Channel);
  87                         break;
  88                 case RFCOMM_Frame_SABM:
  89                         RFCOMM_ProcessSABM(&FrameHeader->Address, Channel);
  90                         break;
  91                 case RFCOMM_Frame_UA:
  92                         RFCOMM_ProcessUA(&FrameHeader->Address, Channel);
  93                         break;
  94                 case RFCOMM_Frame_UIH:
  95                         RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, Channel);
  96                         break;
  97                 default:
  98                         BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received");
  99                         break;
 100         }
 101 }
 102
 103 static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 104 {
 105         BT_RFCOMM_DEBUG(1, "<< DM Received");
 106         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 107 }
 108
 109 static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 110 {
 111         BT_RFCOMM_DEBUG(1, "<< DISC Received");
 112         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 113
 114         // TODO: Close down connection
 115
 116         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 117         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 118 }
 119
 120 static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 121 {
 122         BT_RFCOMM_DEBUG(1, "<< SABM Received");
 123         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 124
 125         // TODO: Reset channel send/receive state
 126
 127         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 128         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 129 }
 130
 131 static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 132 {
 133         BT_RFCOMM_DEBUG(1, "<< UA Received");
 134         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 135 }
 136
 137 static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength,
 138                               const uint8_t* FrameData, Bluetooth_Channel_t* const Channel)
 139 {
 140         BT_RFCOMM_DEBUG(1, "<< UIH Received");
 141         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 142         BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
 143
 144         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 145         {
 146                 RFCOMM_ProcessControlCommand((const RFCOMM_Command_t*)FrameData, Channel);
 147                 return;
 148         }
 149
 150         // TODO: Handle regular channel data here
 151 }
 152
 153 static void RFCOMM_ProcessControlCommand(const RFCOMM_Command_t* CommandHeader, Bluetooth_Channel_t* const Channel)
 154 {
 155         switch (CommandHeader->Command)
 156         {
 157                 case RFCOMM_Control_Test:
 158                         BT_RFCOMM_DEBUG(1, "<< TEST Command");
 159                         break;
 160                 case RFCOMM_Control_FlowControlEnable:
 161                         BT_RFCOMM_DEBUG(1, "<< FCE Command");
 162                         break;
 163                 case RFCOMM_Control_FlowControlDisable:
 164                         BT_RFCOMM_DEBUG(1, "<< FCD Command");
 165                         break;
 166                 case RFCOMM_Control_ModemStatus:
 167                         BT_RFCOMM_DEBUG(1, "<< MS Command");
 168                         break;
 169                 case RFCOMM_Control_RemotePortNegotiation:
 170                         BT_RFCOMM_DEBUG(1, "<< RPN Command");
 171                         break;
 172                 case RFCOMM_Control_RemoteLineStatus:
 173                         BT_RFCOMM_DEBUG(1, "<< RLS Command");
 174                         break;
 175                 case RFCOMM_Control_DLCParameterNegotiation:
 176                         BT_RFCOMM_DEBUG(1, "<< DPN Command");
 177
 178                         struct
 179                         {
 180                                 RFCOMM_Command_t Header;
 181                                 RFCOMM_Command_t Command;
 182                         } Response =
 183                                 {
 184                                         .Header = (RFCOMM_Command_t)
 185                                         {
 186                                                 .Command = RFCOMM_Control_NonSupportedCommand,
 187                                                 .CR      = true,
 188                                                 .EA      = true,
 189                                         },
 190
 191                                         .Command = *CommandHeader,
 192                                 };
 193
 194                         RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, sizeof(RFCOMM_Command_t), &Response, Channel);
 195                         break;
 196                 default:
 197                         BT_RFCOMM_DEBUG(1, "<< Unknown Command");
 198         }
 199 }
 200
 201 static void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 202                              const void* Data, Bluetooth_Channel_t* const Channel)
 203 {
 204         struct
 205         {
 206                 RFCOMM_Header_t FrameHeader;
 207                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 208                 uint8_t         Data[DataLen];
 209                 uint8_t         FCS;
 210         } ResponsePacket;
 211
 212         /* Set the frame header values to the specified address and frame type */
 213         ResponsePacket.FrameHeader.Control = Control;
 214         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t)
 215                 {
 216                         .EA   = true,
 217                         .CR   = CommandResponse,
 218                         .DLCI = DLCI,
 219                 };
 220
 221         /* Set the lower 7 bits of the packet length */
 222         ResponsePacket.Size[0] = (DataLen << 1);
 223
 224         /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
 225         if (DataLen < 128)
 226           ResponsePacket.Size[0] |= 0x01;
 227         else
 228           ResponsePacket.Size[1]  = (DataLen >> 7);
 229
 230         /* Copy over the packet data from the source buffer to the response packet buffer */
 231         memcpy(ResponsePacket.Data, Data, DataLen);
 232
 233         /* Determine the length of the frame which is to be used to calculate the CRC value */
 234         uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);
 235
 236         if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
 237           CRCLength += sizeof(ResponsePacket.Size);
 238
 239         /* Calculate the frame checksum from the appropriate fields */
 240         ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
 241
 242         /* Send the completed response packet to the sender */
 243         Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), Channel);
 244 }
 245
 246 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 247 {
 248         uint8_t FCS = 0xFF;
 249
 250         for (uint8_t i = 0; i < Length; i++)
 251           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
 252
 253         return ~FCS;
 254 }
 255
 256 static uint16_t RFCOMM_GetFrameDataLength(const uint8_t* const BufferPos)
 257 {
 258         uint8_t FirstOctet  = BufferPos[0];
 259         uint8_t SecondOctet = 0;
 260
 261         if (!(FirstOctet & 0x01))
 262           SecondOctet = BufferPos[1];
 263
 264         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 265 }
 266