From 8a397bdf8f53efe5eba7d99da4b2877d15dd3b93 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Sat, 3 Aug 2013 02:12:55 +0200 Subject: [PATCH] Check FCS of incoming bytes Source: http://lxr.free-electrons.com/source/net/bluetooth/rfcomm/core.c#L172 --- SPP.cpp | 52 ++++++++++++++++++++++++++++++++++++---------------- SPP.h | 9 +++++---- 2 files changed, 41 insertions(+), 20 deletions(-) diff --git a/SPP.cpp b/SPP.cpp index cfaf37f2..f307ab25 100644 --- a/SPP.cpp +++ b/SPP.cpp @@ -269,19 +269,31 @@ void SPP::ACLData(uint8_t* l2capinbuf) { /* Read the incoming message */ if (rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) { uint8_t length = l2capinbuf[10] >> 1; // Get length - uint8_t offset = l2capinbuf[4] - length - 4; // See if there is credit - if (rfcommAvailable + length <= sizeof (rfcommDataBuffer)) { // Don't add data to buffer if it would be full - for (uint8_t i = 0; i < length; i++) + uint8_t offset = l2capinbuf[4] - length - 4; // Check if there is credit + if (checkFcs(&l2capinbuf[8], l2capinbuf[11 + length + offset])) { + uint8_t i = 0; + for (; i < length; i++) { + if (rfcommAvailable + i >= sizeof (rfcommDataBuffer)) { +#ifdef DEBUG_USB_HOST + Notify(PSTR("\r\nWarning: Buffer is full!"), 0x80); +#endif + break; + } rfcommDataBuffer[rfcommAvailable + i] = l2capinbuf[11 + i + offset]; - rfcommAvailable += length; - } + } + rfcommAvailable += i; #ifdef EXTRADEBUG - Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); - Notify(rfcommAvailable, 0x80); - if (offset) { - Notify(PSTR(" - Credit: 0x"), 0x80); - D_PrintHex (l2capinbuf[11], 0x80); + Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); + Notify(rfcommAvailable, 0x80); + if (offset) { + Notify(PSTR(" - Credit: 0x"), 0x80); + D_PrintHex (l2capinbuf[11], 0x80); + } +#endif } +#ifdef DEBUG_USB_HOST + else + Notify(PSTR("\r\nError in FCS checksum!"), 0x80); #endif #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth for (uint8_t i = 0; i < length; i++) @@ -722,16 +734,24 @@ void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8 } /* CRC on 2 bytes */ -uint8_t SPP::__crc(uint8_t* data) { - return (pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xff ^ data[0]]) ^ data[1]])); +uint8_t SPP::crc(uint8_t *data) { + return (pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xFF ^ data[0]]) ^ data[1]])); } -/* Calculate FCS - we never actually check if the host sends correct FCS to the Arduino */ +/* Calculate FCS */ uint8_t SPP::calcFcs(uint8_t *data) { if ((data[1] & 0xEF) == RFCOMM_UIH) - return (0xff - __crc(data)); // FCS on 2 bytes + return (0xFF - crc(data)); // FCS on 2 bytes else - return (0xff - pgm_read_byte(&rfcomm_crc_table[__crc(data) ^ data[2]])); // FCS on 3 bytes + return (0xFF - pgm_read_byte(&rfcomm_crc_table[crc(data) ^ data[2]])); // FCS on 3 bytes +} + +/* Check FCS */ +bool SPP::checkFcs(uint8_t *data, uint8_t fcs) { + uint8_t temp = crc(data); + if ((data[1] & 0xEF) != RFCOMM_UIH) + temp = pgm_read_byte(&rfcomm_crc_table[temp ^ data[2]]); // FCS on 3 bytes + return (pgm_read_byte(&rfcomm_crc_table[temp ^ fcs]) == 0xCF); } /* Serial commands */ @@ -739,7 +759,7 @@ size_t SPP::write(uint8_t data) { return write(&data,1); } -size_t SPP::write(const uint8_t* data, size_t size) { +size_t SPP::write(const uint8_t *data, size_t size) { for(uint8_t i = 0; i < size; i++) { if(sppIndex >= sizeof(sppOutputBuffer)/sizeof(sppOutputBuffer[0])) send(); // Send the current data in the buffer diff --git a/SPP.h b/SPP.h index 7a4f90e0..558f16bc 100644 --- a/SPP.h +++ b/SPP.h @@ -215,7 +215,7 @@ private: void RFCOMM_task(); // RFCOMM state machine /* SDP Commands */ - void SDP_Command(uint8_t* data, uint8_t nbytes); + void SDP_Command(uint8_t *data, uint8_t nbytes); void serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow); void serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); void serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); @@ -223,10 +223,11 @@ private: void l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); /* RFCOMM Commands */ - void RFCOMM_Command(uint8_t* data, uint8_t nbytes); - void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length); + void RFCOMM_Command(uint8_t *data, uint8_t nbytes); + void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t *data, uint8_t length); void sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit); uint8_t calcFcs(uint8_t *data); - uint8_t __crc(uint8_t* data); + bool checkFcs(uint8_t *data, uint8_t fcs); + uint8_t crc(uint8_t *data); }; #endif \ No newline at end of file