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 RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI)
 110 {
 111         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 112         {
 113                 RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
 114
 115                 if (CurrRFCOMMChannel->DLCI == DLCI)
 116                   return CurrRFCOMMChannel;
 117         }
 118
 119         return NULL;
 120 }
 121
 122 uint16_t RFCOMM_GetFrameDataLength(const uint8_t* const BufferPos)
 123 {
 124         uint8_t FirstOctet  = BufferPos[0];
 125         uint8_t SecondOctet = 0;
 126
 127         if (!(FirstOctet & 0x01))
 128           SecondOctet = BufferPos[1];
 129
 130         return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1);
 131 }
 132
 133 void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen,
 134                       const void* Data, Bluetooth_Channel_t* const Channel)
 135 {
 136         struct
 137         {
 138                 RFCOMM_Header_t FrameHeader;
 139                 uint8_t         Size[(DataLen < 128) ? 1 : 2];
 140                 uint8_t         Data[DataLen];
 141                 uint8_t         FCS;
 142         } ResponsePacket;
 143
 144         /* Set the frame header values to the specified address and frame type */
 145         ResponsePacket.FrameHeader.Control = Control;
 146         ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t)
 147                 {
 148                         .EA   = true,
 149                         .CR   = CommandResponse,
 150                         .DLCI = DLCI,
 151                 };
 152
 153         /* Set the lower 7 bits of the packet length */
 154         ResponsePacket.Size[0] = (DataLen << 1);
 155
 156         /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */
 157         if (DataLen < 128)
 158           ResponsePacket.Size[0] |= 0x01;
 159         else
 160           ResponsePacket.Size[1]  = (DataLen >> 7);
 161
 162         /* Copy over the packet data from the source buffer to the response packet buffer */
 163         memcpy(ResponsePacket.Data, Data, DataLen);
 164
 165         /* Determine the length of the frame which is to be used to calculate the CRC value */
 166         uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader);
 167
 168         /* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */
 169         if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH)
 170           CRCLength += sizeof(ResponsePacket.Size);
 171
 172         /* Calculate the frame checksum from the appropriate fields */
 173         ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength);
 174
 175         /* Send the completed response packet to the sender */
 176         Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), Channel);
 177 }
 178
 179 static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length)
 180 {
 181         uint8_t FCS = 0xFF;
 182
 183         for (uint8_t i = 0; i < Length; i++)
 184           FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((uint8_t*)FrameStart)[i]]);
 185
 186         return ~FCS;
 187 }
 188
 189 static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 190 {
 191         BT_RFCOMM_DEBUG(1, "<< DM Received");
 192         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 193 }
 194
 195 static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 196 {
 197         BT_RFCOMM_DEBUG(1, "<< DISC Received");
 198         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 199
 200         RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI);
 201
 202         /* If the requested channel is currently open, destroy it */
 203         if (RFCOMMChannel != NULL)
 204           RFCOMMChannel->DLCI = 0x00;
 205
 206         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 207         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 208 }
 209
 210 static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 211 {
 212         BT_RFCOMM_DEBUG(1, "<< SABM Received");
 213         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 214
 215         /* Find a free entry in the RFCOMM channel multiplexer state array */
 216         for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++)
 217         {
 218                 RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i];
 219
 220                 /* If the channel's DLCI is zero, the channel state entry is free */
 221                 if (!(CurrRFCOMMChannel->DLCI))
 222                 {
 223                         CurrRFCOMMChannel->DLCI       = FrameAddress->DLCI;
 224                         CurrRFCOMMChannel->Configured = false;
 225
 226                         BT_RFCOMM_DEBUG(1, ">> UA Sent");
 227                         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, Channel);
 228                         return;
 229                 }
 230         }
 231
 232         BT_RFCOMM_DEBUG(1, ">> DM Sent");
 233
 234         /* No free channel in the multiplexer - decline the SABM by sending a DM frame */
 235         RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, Channel);
 236 }
 237
 238 static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const Channel)
 239 {
 240         BT_RFCOMM_DEBUG(1, "<< UA Received");
 241         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 242 }
 243
 244 static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength,
 245                               const uint8_t* FrameData, Bluetooth_Channel_t* const Channel)
 246 {
 247         BT_RFCOMM_DEBUG(1, "<< UIH Received");
 248         BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI);
 249         BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength);
 250
 251         if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI)
 252         {
 253                 RFCOMM_ProcessControlCommand(FrameData, Channel);
 254                 return;
 255         }
 256
 257         // TODO: Handle regular channel data here
 258 }