tmk_core/protocol/usb_hid/USB_Host_Shield_2.0/SPP.cpp

   1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
   2
   3  This software may be distributed and modified under the terms of the GNU
   4  General Public License version 2 (GPL2) as published by the Free Software
   5  Foundation and appearing in the file GPL2.TXT included in the packaging of
   6  this file. Please note that GPL2 Section 2[b] requires that all works based
   7  on this software must also be made publicly available under the terms of
   8  the GPL2 ("Copyleft").
   9
  10  Contact information
  11  -------------------
  12
  13  Kristian Lauszus, TKJ Electronics
  14  Web      :  http://www.tkjelectronics.com
  15  e-mail   :  kristianl@tkjelectronics.com
  16  */
  17
  18 #include "SPP.h"
  19 // To enable serial debugging see "settings.h"
  20 //#define EXTRADEBUG // Uncomment to get even more debugging data
  21 //#define PRINTREPORT // Uncomment to print the report sent to the Arduino
  22
  23 /*
  24  * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0.
  25  */
  26 const uint8_t rfcomm_crc_table[256] PROGMEM = {/* reversed, 8-bit, poly=0x07 */
  27         0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
  28         0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
  29         0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
  30         0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
  31         0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
  32         0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
  33         0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
  34         0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
  35         0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
  36         0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
  37         0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
  38         0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
  39         0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
  40         0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
  41         0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
  42         0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
  43 };
  44
  45 SPP::SPP(BTD *p, const char* name, const char* pin) :
  46 BluetoothService(p) // Pointer to BTD class instance - mandatory
  47 {
  48         pBtd->btdName = name;
  49         pBtd->btdPin = pin;
  50
  51         /* Set device cid for the SDP and RFCOMM channelse */
  52         sdp_dcid[0] = 0x50; // 0x0050
  53         sdp_dcid[1] = 0x00;
  54         rfcomm_dcid[0] = 0x51; // 0x0051
  55         rfcomm_dcid[1] = 0x00;
  56
  57         Reset();
  58 }
  59
  60 void SPP::Reset() {
  61         connected = false;
  62         RFCOMMConnected = false;
  63         SDPConnected = false;
  64         waitForLastCommand = false;
  65         l2cap_sdp_state = L2CAP_SDP_WAIT;
  66         l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
  67         l2cap_event_flag = 0;
  68         sppIndex = 0;
  69         creditSent = false;
  70 }
  71
  72 void SPP::disconnect() {
  73         connected = false;
  74         // First the two L2CAP channels has to be disconnected and then the HCI connection
  75         if(RFCOMMConnected)
  76                 pBtd->l2cap_disconnection_request(hci_handle, ++identifier, rfcomm_scid, rfcomm_dcid);
  77         if(RFCOMMConnected && SDPConnected)
  78                 delay(1); // Add delay between commands
  79         if(SDPConnected)
  80                 pBtd->l2cap_disconnection_request(hci_handle, ++identifier, sdp_scid, sdp_dcid);
  81         l2cap_sdp_state = L2CAP_DISCONNECT_RESPONSE;
  82 }
  83
  84 void SPP::ACLData(uint8_t* l2capinbuf) {
  85         if(!connected) {
  86                 if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
  87                         if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM && !pBtd->sdpConnectionClaimed) {
  88                                 pBtd->sdpConnectionClaimed = true;
  89                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
  90                                 l2cap_sdp_state = L2CAP_SDP_WAIT; // Reset state
  91                         } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM && !pBtd->rfcommConnectionClaimed) {
  92                                 pBtd->rfcommConnectionClaimed = true;
  93                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
  94                                 l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; // Reset state
  95                         }
  96                 }
  97         }
  98
  99         if(checkHciHandle(l2capinbuf, hci_handle)) { // acl_handle_ok
 100                 if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U
 101                         if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 102 #ifdef DEBUG_USB_HOST
 103                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
 104                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 105                                 Notify(PSTR(" "), 0x80);
 106                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 107                                 Notify(PSTR(" Data: "), 0x80);
 108                                 D_PrintHex<uint8_t > (l2capinbuf[17], 0x80);
 109                                 Notify(PSTR(" "), 0x80);
 110                                 D_PrintHex<uint8_t > (l2capinbuf[16], 0x80);
 111                                 Notify(PSTR(" "), 0x80);
 112                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 113                                 Notify(PSTR(" "), 0x80);
 114                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 115 #endif
 116                         } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 117 #ifdef EXTRADEBUG
 118                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
 119                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
 120                                 Notify(PSTR(" "), 0x80);
 121                                 D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
 122                                 Notify(PSTR(" SCID: "), 0x80);
 123                                 D_PrintHex<uint8_t > (l2capinbuf[15], 0x80);
 124                                 Notify(PSTR(" "), 0x80);
 125                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 126                                 Notify(PSTR(" Identifier: "), 0x80);
 127                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 128 #endif
 129                                 if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so
 130                                         identifier = l2capinbuf[9];
 131                                         sdp_scid[0] = l2capinbuf[14];
 132                                         sdp_scid[1] = l2capinbuf[15];
 133                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST);
 134                                 } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { // ----- || -----
 135                                         identifier = l2capinbuf[9];
 136                                         rfcomm_scid[0] = l2capinbuf[14];
 137                                         rfcomm_scid[1] = l2capinbuf[15];
 138                                         l2cap_set_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST);
 139                                 }
 140                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
 141                                 if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
 142                                         if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
 143                                                 //Notify(PSTR("\r\nSDP Configuration Complete"), 0x80);
 144                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS);
 145                                         } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
 146                                                 //Notify(PSTR("\r\nRFCOMM Configuration Complete"), 0x80);
 147                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS);
 148                                         }
 149                                 }
 150                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
 151                                 if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
 152                                         //Notify(PSTR("\r\nSDP Configuration Request"), 0x80);
 153                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], sdp_scid);
 154                                 } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
 155                                         //Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80);
 156                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], rfcomm_scid);
 157                                 }
 158                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
 159                                 if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
 160                                         //Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80);
 161                                         identifier = l2capinbuf[9];
 162                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST);
 163                                 } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
 164                                         //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80);
 165                                         identifier = l2capinbuf[9];
 166                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST);
 167                                 }
 168                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
 169                                 if(l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) {
 170                                         //Notify(PSTR("\r\nDisconnect Response: SDP Channel"), 0x80);
 171                                         identifier = l2capinbuf[9];
 172                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RESPONSE);
 173                                 } else if(l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) {
 174                                         //Notify(PSTR("\r\nDisconnect Response: RFCOMM Channel"), 0x80);
 175                                         identifier = l2capinbuf[9];
 176                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RESPONSE);
 177                                 }
 178                         } else if(l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) {
 179 #ifdef DEBUG_USB_HOST
 180                                 Notify(PSTR("\r\nInformation request"), 0x80);
 181 #endif
 182                                 identifier = l2capinbuf[9];
 183                                 pBtd->l2cap_information_response(hci_handle, identifier, l2capinbuf[12], l2capinbuf[13]);
 184                         }
 185 #ifdef EXTRADEBUG
 186                         else {
 187                                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
 188                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
 189                         }
 190 #endif
 191                 } else if(l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP
 192                         if(l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) {
 193                                 if(((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == SERIALPORT_UUID)) { // Check if it's sending the full UUID, see: https://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm, we will just check the first four bytes
 194                                         if(firstMessage) {
 195                                                 serialPortResponse1(l2capinbuf[9], l2capinbuf[10]);
 196                                                 firstMessage = false;
 197                                         } else {
 198                                                 serialPortResponse2(l2capinbuf[9], l2capinbuf[10]); // Serialport continuation state
 199                                                 firstMessage = true;
 200                                         }
 201                                 } else if(((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == L2CAP_UUID)) {
 202                                         if(firstMessage) {
 203                                                 l2capResponse1(l2capinbuf[9], l2capinbuf[10]);
 204                                                 firstMessage = false;
 205                                         } else {
 206                                                 l2capResponse2(l2capinbuf[9], l2capinbuf[10]); // L2CAP continuation state
 207                                                 firstMessage = true;
 208                                         }
 209                                 } else
 210                                         serviceNotSupported(l2capinbuf[9], l2capinbuf[10]); // The service is not supported
 211 #ifdef EXTRADEBUG
 212                                 Notify(PSTR("\r\nUUID: "), 0x80);
 213                                 uint16_t uuid;
 214                                 if((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000) // Check if it's sending the UUID as a 128-bit UUID
 215                                         uuid = (l2capinbuf[18] << 8 | l2capinbuf[19]);
 216                                 else // Short UUID
 217                                         uuid = (l2capinbuf[16] << 8 | l2capinbuf[17]);
 218                                 D_PrintHex<uint16_t > (uuid, 0x80);
 219
 220                                 Notify(PSTR("\r\nLength: "), 0x80);
 221                                 uint16_t length = l2capinbuf[11] << 8 | l2capinbuf[12];
 222                                 D_PrintHex<uint16_t > (length, 0x80);
 223                                 Notify(PSTR("\r\nData: "), 0x80);
 224                                 for(uint8_t i = 0; i < length; i++) {
 225                                         D_PrintHex<uint8_t > (l2capinbuf[13 + i], 0x80);
 226                                         Notify(PSTR(" "), 0x80);
 227                                 }
 228 #endif
 229                         }
 230 #ifdef EXTRADEBUG
 231                         else {
 232                                 Notify(PSTR("\r\nUnknown PDU: "), 0x80);
 233                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
 234                         }
 235 #endif
 236                 } else if(l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM
 237                         rfcommChannel = l2capinbuf[8] & 0xF8;
 238                         rfcommDirection = l2capinbuf[8] & 0x04;
 239                         rfcommCommandResponse = l2capinbuf[8] & 0x02;
 240                         rfcommChannelType = l2capinbuf[9] & 0xEF;
 241                         rfcommPfBit = l2capinbuf[9] & 0x10;
 242
 243                         if(rfcommChannel >> 3 != 0x00)
 244                                 rfcommChannelConnection = rfcommChannel;
 245
 246 #ifdef EXTRADEBUG
 247                         Notify(PSTR("\r\nRFCOMM Channel: "), 0x80);
 248                         D_PrintHex<uint8_t > (rfcommChannel >> 3, 0x80);
 249                         Notify(PSTR(" Direction: "), 0x80);
 250                         D_PrintHex<uint8_t > (rfcommDirection >> 2, 0x80);
 251                         Notify(PSTR(" CommandResponse: "), 0x80);
 252                         D_PrintHex<uint8_t > (rfcommCommandResponse >> 1, 0x80);
 253                         Notify(PSTR(" ChannelType: "), 0x80);
 254                         D_PrintHex<uint8_t > (rfcommChannelType, 0x80);
 255                         Notify(PSTR(" PF_BIT: "), 0x80);
 256                         D_PrintHex<uint8_t > (rfcommPfBit, 0x80);
 257 #endif
 258                         if(rfcommChannelType == RFCOMM_DISC) {
 259 #ifdef DEBUG_USB_HOST
 260                                 Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80);
 261                                 D_PrintHex<uint8_t > (rfcommChannel >> 3, 0x80);
 262 #endif
 263                                 connected = false;
 264                                 sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command
 265                         }
 266                         if(connected) {
 267                                 /* Read the incoming message */
 268                                 if(rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) {
 269                                         uint8_t length = l2capinbuf[10] >> 1; // Get length
 270                                         uint8_t offset = l2capinbuf[4] - length - 4; // Check if there is credit
 271                                         if(checkFcs(&l2capinbuf[8], l2capinbuf[11 + length + offset])) {
 272                                                 uint8_t i = 0;
 273                                                 for(; i < length; i++) {
 274                                                         if(rfcommAvailable + i >= sizeof (rfcommDataBuffer)) {
 275 #ifdef DEBUG_USB_HOST
 276                                                                 Notify(PSTR("\r\nWarning: Buffer is full!"), 0x80);
 277 #endif
 278                                                                 break;
 279                                                         }
 280                                                         rfcommDataBuffer[rfcommAvailable + i] = l2capinbuf[11 + i + offset];
 281                                                 }
 282                                                 rfcommAvailable += i;
 283 #ifdef EXTRADEBUG
 284                                                 Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80);
 285                                                 Notify(rfcommAvailable, 0x80);
 286                                                 if(offset) {
 287                                                         Notify(PSTR(" - Credit: 0x"), 0x80);
 288                                                         D_PrintHex<uint8_t > (l2capinbuf[11], 0x80);
 289                                                 }
 290 #endif
 291                                         }
 292 #ifdef DEBUG_USB_HOST
 293                                         else
 294                                                 Notify(PSTR("\r\nError in FCS checksum!"), 0x80);
 295 #endif
 296 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth
 297                                         for(uint8_t i = 0; i < length; i++)
 298                                                 Notifyc(l2capinbuf[i + 11 + offset], 0x80);
 299 #endif
 300                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
 301 #ifdef DEBUG_USB_HOST
 302                                         Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
 303 #endif
 304                                         rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command
 305                                         rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
 306                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1
 307                                         rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
 308                                         rfcommbuf[4] = l2capinbuf[15]; // Priority
 309                                         rfcommbuf[5] = l2capinbuf[16]; // Timer
 310                                         rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB
 311                                         rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB
 312                                         rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm.
 313                                         rfcommbuf[9] = l2capinbuf[20]; // Number of Frames
 314                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); // UIH Remote Port Negotiation Response
 315                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
 316 #ifdef DEBUG_USB_HOST
 317                                         Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
 318 #endif
 319                                         rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response
 320                                         rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
 321                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3)
 322                                         rfcommbuf[3] = l2capinbuf[14];
 323                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04);
 324                                 }
 325                         } else {
 326                                 if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish
 327 #ifdef DEBUG_USB_HOST
 328                                         Notify(PSTR("\r\nReceived SABM Command"), 0x80);
 329 #endif
 330                                         sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command
 331                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command
 332 #ifdef DEBUG_USB_HOST
 333                                         Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80);
 334 #endif
 335                                         rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response
 336                                         rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
 337                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1
 338                                         rfcommbuf[3] = 0xE0; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
 339                                         rfcommbuf[4] = 0x00; // Priority
 340                                         rfcommbuf[5] = 0x00; // Timer
 341                                         rfcommbuf[6] = BULK_MAXPKTSIZE - 14; // Max Fram Size LSB - set to the size of received data (50)
 342                                         rfcommbuf[7] = 0x00; // Max Fram Size MSB
 343                                         rfcommbuf[8] = 0x00; // MaxRatransm.
 344                                         rfcommbuf[9] = 0x00; // Number of Frames
 345                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A);
 346                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
 347 #ifdef DEBUG_USB_HOST
 348                                         Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
 349 #endif
 350                                         rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response
 351                                         rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
 352                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3)
 353                                         rfcommbuf[3] = l2capinbuf[14];
 354                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04);
 355
 356                                         delay(1);
 357 #ifdef DEBUG_USB_HOST
 358                                         Notify(PSTR("\r\nSend UIH Modem Status Command"), 0x80);
 359 #endif
 360                                         rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command
 361                                         rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1
 362                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3)
 363                                         rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES)
 364
 365                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04);
 366                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response
 367                                         if(!creditSent) {
 368 #ifdef DEBUG_USB_HOST
 369                                                 Notify(PSTR("\r\nSend UIH Command with credit"), 0x80);
 370 #endif
 371                                                 sendRfcommCredit(rfcommChannelConnection, rfcommDirection, 0, RFCOMM_UIH, 0x10, sizeof (rfcommDataBuffer)); // Send credit
 372                                                 creditSent = true;
 373                                                 timer = millis();
 374                                                 waitForLastCommand = true;
 375                                         }
 376                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit
 377 #ifdef DEBUG_USB_HOST
 378                                         Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80);
 379 #endif
 380                                 } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
 381 #ifdef DEBUG_USB_HOST
 382                                         Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
 383 #endif
 384                                         rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command
 385                                         rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1
 386                                         rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1
 387                                         rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
 388                                         rfcommbuf[4] = l2capinbuf[15]; // Priority
 389                                         rfcommbuf[5] = l2capinbuf[16]; // Timer
 390                                         rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB
 391                                         rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB
 392                                         rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm.
 393                                         rfcommbuf[9] = l2capinbuf[20]; // Number of Frames
 394                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); // UIH Remote Port Negotiation Response
 395 #ifdef DEBUG_USB_HOST
 396                                         Notify(PSTR("\r\nRFCOMM Connection is now established\r\n"), 0x80);
 397 #endif
 398                                         onInit();
 399                                 }
 400 #ifdef EXTRADEBUG
 401                                 else if(rfcommChannelType != RFCOMM_DISC) {
 402                                         Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80);
 403                                         D_PrintHex<uint8_t > (rfcommChannelType, 0x80);
 404                                         Notify(PSTR(" Command: "), 0x80);
 405                                         D_PrintHex<uint8_t > (l2capinbuf[11], 0x80);
 406                                 }
 407 #endif
 408                         }
 409                 }
 410 #ifdef EXTRADEBUG
 411                 else {
 412                         Notify(PSTR("\r\nUnsupported L2CAP Data - Channel ID: "), 0x80);
 413                         D_PrintHex<uint8_t > (l2capinbuf[7], 0x80);
 414                         Notify(PSTR(" "), 0x80);
 415                         D_PrintHex<uint8_t > (l2capinbuf[6], 0x80);
 416                 }
 417 #endif
 418                 SDP_task();
 419                 RFCOMM_task();
 420         }
 421 }
 422
 423 void SPP::Run() {
 424         if(waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it
 425 #ifdef DEBUG_USB_HOST
 426                 Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80);
 427 #endif
 428                 onInit();
 429         }
 430         send(); // Send all bytes currently in the buffer
 431 }
 432
 433 void SPP::onInit() {
 434         creditSent = false;
 435         waitForLastCommand = false;
 436         connected = true; // The RFCOMM channel is now established
 437         sppIndex = 0;
 438         if(pFuncOnInit)
 439                 pFuncOnInit(); // Call the user function
 440 };
 441
 442 void SPP::SDP_task() {
 443         switch(l2cap_sdp_state) {
 444                 case L2CAP_SDP_WAIT:
 445                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST)) {
 446                                 l2cap_clear_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST); // Clear flag
 447 #ifdef DEBUG_USB_HOST
 448                                 Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80);
 449 #endif
 450                                 pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, PENDING);
 451                                 delay(1);
 452                                 pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, SUCCESSFUL);
 453                                 identifier++;
 454                                 delay(1);
 455                                 pBtd->l2cap_config_request(hci_handle, identifier, sdp_scid);
 456                                 l2cap_sdp_state = L2CAP_SDP_SUCCESS;
 457                         } else if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST)) {
 458                                 l2cap_clear_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST); // Clear flag
 459                                 SDPConnected = false;
 460 #ifdef DEBUG_USB_HOST
 461                                 Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80);
 462 #endif
 463                                 pBtd->l2cap_disconnection_response(hci_handle, identifier, sdp_dcid, sdp_scid);
 464                         }
 465                         break;
 466                 case L2CAP_SDP_SUCCESS:
 467                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS)) {
 468                                 l2cap_clear_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS); // Clear flag
 469 #ifdef DEBUG_USB_HOST
 470                                 Notify(PSTR("\r\nSDP Successfully Configured"), 0x80);
 471 #endif
 472                                 firstMessage = true; // Reset bool
 473                                 SDPConnected = true;
 474                                 l2cap_sdp_state = L2CAP_SDP_WAIT;
 475                         }
 476                         break;
 477
 478                 case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected
 479                         if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_RESPONSE)) {
 480 #ifdef DEBUG_USB_HOST
 481                                 Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80);
 482 #endif
 483                                 pBtd->hci_disconnect(hci_handle);
 484                                 hci_handle = -1; // Reset handle
 485                                 Reset();
 486                         }
 487                         break;
 488         }
 489 }
 490
 491 void SPP::RFCOMM_task() {
 492         switch(l2cap_rfcomm_state) {
 493                 case L2CAP_RFCOMM_WAIT:
 494                         if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST)) {
 495                                 l2cap_clear_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST); // Clear flag
 496 #ifdef DEBUG_USB_HOST
 497                                 Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80);
 498 #endif
 499                                 pBtd->l2cap_connection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid, PENDING);
 500                                 delay(1);
 501                                 pBtd->l2cap_connection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL);
 502                                 identifier++;
 503                                 delay(1);
 504                                 pBtd->l2cap_config_request(hci_handle, identifier, rfcomm_scid);
 505                                 l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS;
 506                         } else if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST)) {
 507                                 l2cap_clear_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST); // Clear flag
 508                                 RFCOMMConnected = false;
 509                                 connected = false;
 510 #ifdef DEBUG_USB_HOST
 511                                 Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80);
 512 #endif
 513                                 pBtd->l2cap_disconnection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid);
 514                         }
 515                         break;
 516                 case L2CAP_RFCOMM_SUCCESS:
 517                         if(l2cap_check_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS)) {
 518                                 l2cap_clear_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS); // Clear flag
 519 #ifdef DEBUG_USB_HOST
 520                                 Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80);
 521 #endif
 522                                 rfcommAvailable = 0; // Reset number of bytes available
 523                                 bytesRead = 0; // Reset number of bytes received
 524                                 RFCOMMConnected = true;
 525                                 l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
 526                         }
 527                         break;
 528         }
 529 }
 530 /************************************************************/
 531 /*                    SDP Commands                          */
 532
 533 /************************************************************/
 534 void SPP::SDP_Command(uint8_t* data, uint8_t nbytes) { // See page 223 in the Bluetooth specs
 535         pBtd->L2CAP_Command(hci_handle, data, nbytes, sdp_scid[0], sdp_scid[1]);
 536 }
 537
 538 void SPP::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow) { // See page 235 in the Bluetooth specs
 539         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
 540         l2capoutbuf[1] = transactionIDHigh;
 541         l2capoutbuf[2] = transactionIDLow;
 542         l2capoutbuf[3] = 0x00; // MSB Parameter Length
 543         l2capoutbuf[4] = 0x05; // LSB Parameter Length = 5
 544         l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
 545         l2capoutbuf[6] = 0x02; // LSB AttributeListsByteCount = 2
 546
 547         /* Attribute ID/Value Sequence: */
 548         l2capoutbuf[7] = 0x35; // Data element sequence - length in next byte
 549         l2capoutbuf[8] = 0x00; // Length = 0
 550         l2capoutbuf[9] = 0x00; // No continuation state
 551
 552         SDP_Command(l2capoutbuf, 10);
 553 }
 554
 555 void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
 556         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
 557         l2capoutbuf[1] = transactionIDHigh;
 558         l2capoutbuf[2] = transactionIDLow;
 559         l2capoutbuf[3] = 0x00; // MSB Parameter Length
 560         l2capoutbuf[4] = 0x2B; // LSB Parameter Length = 43
 561         l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
 562         l2capoutbuf[6] = 0x26; // LSB AttributeListsByteCount = 38
 563
 564         /* Attribute ID/Value Sequence: */
 565         l2capoutbuf[7] = 0x36; // Data element sequence - length in next two bytes
 566         l2capoutbuf[8] = 0x00; // MSB Length
 567         l2capoutbuf[9] = 0x3C; // LSB Length = 60
 568
 569         l2capoutbuf[10] = 0x36; // Data element sequence - length in next two bytes
 570         l2capoutbuf[11] = 0x00; // MSB Length
 571         l2capoutbuf[12] = 0x39; // LSB Length = 57
 572
 573         l2capoutbuf[13] = 0x09; // Unsigned Integer - length 2 bytes
 574         l2capoutbuf[14] = 0x00; // MSB ServiceRecordHandle
 575         l2capoutbuf[15] = 0x00; // LSB ServiceRecordHandle
 576         l2capoutbuf[16] = 0x0A; // Unsigned int - length 4 bytes
 577         l2capoutbuf[17] = 0x00; // ServiceRecordHandle value - TODO: Is this related to HCI_Handle?
 578         l2capoutbuf[18] = 0x01;
 579         l2capoutbuf[19] = 0x00;
 580         l2capoutbuf[20] = 0x06;
 581
 582         l2capoutbuf[21] = 0x09; // Unsigned Integer - length 2 bytes
 583         l2capoutbuf[22] = 0x00; // MSB ServiceClassIDList
 584         l2capoutbuf[23] = 0x01; // LSB ServiceClassIDList
 585         l2capoutbuf[24] = 0x35; // Data element sequence - length in next byte
 586         l2capoutbuf[25] = 0x03; // Length = 3
 587         l2capoutbuf[26] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
 588         l2capoutbuf[27] = 0x11; // MSB SerialPort
 589         l2capoutbuf[28] = 0x01; // LSB SerialPort
 590
 591         l2capoutbuf[29] = 0x09; // Unsigned Integer - length 2 bytes
 592         l2capoutbuf[30] = 0x00; // MSB ProtocolDescriptorList
 593         l2capoutbuf[31] = 0x04; // LSB ProtocolDescriptorList
 594         l2capoutbuf[32] = 0x35; // Data element sequence - length in next byte
 595         l2capoutbuf[33] = 0x0C; // Length = 12
 596
 597         l2capoutbuf[34] = 0x35; // Data element sequence - length in next byte
 598         l2capoutbuf[35] = 0x03; // Length = 3
 599         l2capoutbuf[36] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
 600         l2capoutbuf[37] = 0x01; // MSB L2CAP
 601         l2capoutbuf[38] = 0x00; // LSB L2CAP
 602
 603         l2capoutbuf[39] = 0x35; // Data element sequence - length in next byte
 604         l2capoutbuf[40] = 0x05; // Length = 5
 605         l2capoutbuf[41] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
 606         l2capoutbuf[42] = 0x00; // MSB RFCOMM
 607         l2capoutbuf[43] = 0x03; // LSB RFCOMM
 608         l2capoutbuf[44] = 0x08; // Unsigned Integer - length 1 byte
 609
 610         l2capoutbuf[45] = 0x02; // ContinuationState - Two more bytes
 611         l2capoutbuf[46] = 0x00; // MSB length
 612         l2capoutbuf[47] = 0x19; // LSB length = 25 more bytes to come
 613
 614         SDP_Command(l2capoutbuf, 48);
 615 }
 616
 617 void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
 618         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
 619         l2capoutbuf[1] = transactionIDHigh;
 620         l2capoutbuf[2] = transactionIDLow;
 621         l2capoutbuf[3] = 0x00; // MSB Parameter Length
 622         l2capoutbuf[4] = 0x1C; // LSB Parameter Length = 28
 623         l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
 624         l2capoutbuf[6] = 0x19; // LSB AttributeListsByteCount = 25
 625
 626         /* Attribute ID/Value Sequence: */
 627         l2capoutbuf[7] = 0x01; // Channel 1 - TODO: Try different values, so multiple servers can be used at once
 628
 629         l2capoutbuf[8] = 0x09; // Unsigned Integer - length 2 bytes
 630         l2capoutbuf[9] = 0x00; // MSB LanguageBaseAttributeIDList
 631         l2capoutbuf[10] = 0x06; // LSB LanguageBaseAttributeIDList
 632         l2capoutbuf[11] = 0x35; // Data element sequence - length in next byte
 633         l2capoutbuf[12] = 0x09; // Length = 9
 634
 635         // Identifier representing the natural language = en = English - see: "ISO 639:1988"
 636         l2capoutbuf[13] = 0x09; // Unsigned Integer - length 2 bytes
 637         l2capoutbuf[14] = 0x65; // 'e'
 638         l2capoutbuf[15] = 0x6E; // 'n'
 639
 640         // "The second element of each triplet contains an identifier that specifies a character encoding used for the language"
 641         // Encoding is set to 106 (UTF-8) - see: http://www.iana.org/assignments/character-sets/character-sets.xhtml
 642         l2capoutbuf[16] = 0x09; // Unsigned Integer - length 2 bytes
 643         l2capoutbuf[17] = 0x00; // MSB of character encoding
 644         l2capoutbuf[18] = 0x6A; // LSB of character encoding (106)
 645
 646         // Attribute ID that serves as the base attribute ID for the natural language in the service record
 647         // "To facilitate the retrieval of human-readable universal attributes in a principal language, the base attribute ID value for the primary language supported by a service record shall be 0x0100"
 648         l2capoutbuf[19] = 0x09; // Unsigned Integer - length 2 bytes
 649         l2capoutbuf[20] = 0x01;
 650         l2capoutbuf[21] = 0x00;
 651
 652         l2capoutbuf[22] = 0x09; // Unsigned Integer - length 2 bytes
 653         l2capoutbuf[23] = 0x01; // MSB ServiceDescription
 654         l2capoutbuf[24] = 0x00; // LSB ServiceDescription
 655
 656         l2capoutbuf[25] = 0x25; // Text string - length in next byte
 657         l2capoutbuf[26] = 0x05; // Name length
 658         l2capoutbuf[27] = 'T';
 659         l2capoutbuf[28] = 'K';
 660         l2capoutbuf[29] = 'J';
 661         l2capoutbuf[30] = 'S';
 662         l2capoutbuf[31] = 'P';
 663         l2capoutbuf[32] = 0x00; // No continuation state
 664
 665         SDP_Command(l2capoutbuf, 33);
 666 }
 667
 668 void SPP::l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
 669         serialPortResponse1(transactionIDHigh, transactionIDLow); // These has to send all the supported functions, since it only supports virtual serialport it just sends the message again
 670 }
 671
 672 void SPP::l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) {
 673         serialPortResponse2(transactionIDHigh, transactionIDLow); // Same data as serialPortResponse2
 674 }
 675 /************************************************************/
 676 /*                    RFCOMM Commands                       */
 677
 678 /************************************************************/
 679 void SPP::RFCOMM_Command(uint8_t* data, uint8_t nbytes) {
 680         pBtd->L2CAP_Command(hci_handle, data, nbytes, rfcomm_scid[0], rfcomm_scid[1]);
 681 }
 682
 683 void SPP::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length) {
 684         l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address
 685         l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control
 686         l2capoutbuf[2] = length << 1 | 0x01; // Length and format (always 0x01 bytes format)
 687         uint8_t i = 0;
 688         for(; i < length; i++)
 689                 l2capoutbuf[i + 3] = data[i];
 690         l2capoutbuf[i + 3] = calcFcs(l2capoutbuf);
 691 #ifdef EXTRADEBUG
 692         Notify(PSTR(" - RFCOMM Data: "), 0x80);
 693         for(i = 0; i < length + 4; i++) {
 694                 D_PrintHex<uint8_t > (l2capoutbuf[i], 0x80);
 695                 Notify(PSTR(" "), 0x80);
 696         }
 697 #endif
 698         RFCOMM_Command(l2capoutbuf, length + 4);
 699 }
 700
 701 void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit) {
 702         l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address
 703         l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control
 704         l2capoutbuf[2] = 0x01; // Length = 0
 705         l2capoutbuf[3] = credit; // Credit
 706         l2capoutbuf[4] = calcFcs(l2capoutbuf);
 707 #ifdef EXTRADEBUG
 708         Notify(PSTR(" - RFCOMM Credit Data: "), 0x80);
 709         for(uint8_t i = 0; i < 5; i++) {
 710                 D_PrintHex<uint8_t > (l2capoutbuf[i], 0x80);
 711                 Notify(PSTR(" "), 0x80);
 712         }
 713 #endif
 714         RFCOMM_Command(l2capoutbuf, 5);
 715 }
 716
 717 /* CRC on 2 bytes */
 718 uint8_t SPP::crc(uint8_t *data) {
 719         return (pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xFF ^ data[0]]) ^ data[1]]));
 720 }
 721
 722 /* Calculate FCS */
 723 uint8_t SPP::calcFcs(uint8_t *data) {
 724         uint8_t temp = crc(data);
 725         if((data[1] & 0xEF) == RFCOMM_UIH)
 726                 return (0xFF - temp); // FCS on 2 bytes
 727         else
 728                 return (0xFF - pgm_read_byte(&rfcomm_crc_table[temp ^ data[2]])); // FCS on 3 bytes
 729 }
 730
 731 /* Check FCS */
 732 bool SPP::checkFcs(uint8_t *data, uint8_t fcs) {
 733         uint8_t temp = crc(data);
 734         if((data[1] & 0xEF) != RFCOMM_UIH)
 735                 temp = pgm_read_byte(&rfcomm_crc_table[temp ^ data[2]]); // FCS on 3 bytes
 736         return (pgm_read_byte(&rfcomm_crc_table[temp ^ fcs]) == 0xCF);
 737 }
 738
 739 /* Serial commands */
 740 #if defined(ARDUINO) && ARDUINO >=100
 741
 742 size_t SPP::write(uint8_t data) {
 743         return write(&data, 1);
 744 }
 745 #else
 746
 747 void SPP::write(uint8_t data) {
 748         write(&data, 1);
 749 }
 750 #endif
 751
 752 #if defined(ARDUINO) && ARDUINO >=100
 753
 754 size_t SPP::write(const uint8_t *data, size_t size) {
 755 #else
 756
 757 void SPP::write(const uint8_t *data, size_t size) {
 758 #endif
 759         for(uint8_t i = 0; i < size; i++) {
 760                 if(sppIndex >= sizeof (sppOutputBuffer) / sizeof (sppOutputBuffer[0]))
 761                         send(); // Send the current data in the buffer
 762                 sppOutputBuffer[sppIndex++] = data[i]; // All the bytes are put into a buffer and then send using the send() function
 763         }
 764 #if defined(ARDUINO) && ARDUINO >=100
 765         return size;
 766 #endif
 767 }
 768
 769 void SPP::send() {
 770         if(!connected || !sppIndex)
 771                 return;
 772         uint8_t length; // This is the length of the string we are sending
 773         uint8_t offset = 0; // This is used to keep track of where we are in the string
 774
 775         l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress; // RFCOMM Address
 776         l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control
 777
 778         while(sppIndex) { // We will run this while loop until this variable is 0
 779                 if(sppIndex > (sizeof (l2capoutbuf) - 4)) // Check if the string is larger than the outgoing buffer
 780                         length = sizeof (l2capoutbuf) - 4;
 781                 else
 782                         length = sppIndex;
 783
 784                 l2capoutbuf[2] = length << 1 | 1; // Length
 785                 uint8_t i = 0;
 786                 for(; i < length; i++)
 787                         l2capoutbuf[i + 3] = sppOutputBuffer[i + offset];
 788                 l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); // Calculate checksum
 789
 790                 RFCOMM_Command(l2capoutbuf, length + 4);
 791
 792                 sppIndex -= length;
 793                 offset += length; // Increment the offset
 794         }
 795 }
 796
 797 int SPP::available(void) {
 798         return rfcommAvailable;
 799 };
 800
 801 void SPP::discard(void) {
 802         rfcommAvailable = 0;
 803 }
 804
 805 int SPP::peek(void) {
 806         if(rfcommAvailable == 0) // Don't read if there is nothing in the buffer
 807                 return -1;
 808         return rfcommDataBuffer[0];
 809 }
 810
 811 int SPP::read(void) {
 812         if(rfcommAvailable == 0) // Don't read if there is nothing in the buffer
 813                 return -1;
 814         uint8_t output = rfcommDataBuffer[0];
 815         for(uint8_t i = 1; i < rfcommAvailable; i++)
 816                 rfcommDataBuffer[i - 1] = rfcommDataBuffer[i]; // Shift the buffer one left
 817         rfcommAvailable--;
 818         bytesRead++;
 819         if(bytesRead > (sizeof (rfcommDataBuffer) - 5)) { // We will send the command just before it runs out of credit
 820                 bytesRead = 0;
 821                 sendRfcommCredit(rfcommChannelConnection, rfcommDirection, 0, RFCOMM_UIH, 0x10, sizeof (rfcommDataBuffer)); // Send more credit
 822 #ifdef EXTRADEBUG
 823                 Notify(PSTR("\r\nSent "), 0x80);
 824                 Notify((uint8_t)sizeof (rfcommDataBuffer), 0x80);
 825                 Notify(PSTR(" more credit"), 0x80);
 826 #endif
 827         }
 828         return output;
 829 }