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