specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Projects Releases Packages Wiki Activity

Check FCS of incoming bytes

Browse source 
Source: http://lxr.free-electrons.com/source/net/bluetooth/rfcomm/core.c#L172
This commit is contained in:
Kristian Sloth Lauszus 2013-08-03 02:12:55 +02:00
parent 8e6ab3f3ae
commit 8a397bdf8f
2 changed files with 41 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
SPP.cpp
View file

@ -269,12 +269,19 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
/* Read the incoming message */ /* Read the incoming message */
if (rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) { if (rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) {
uint8_t length = l2capinbuf[10] >> 1; // Get length uint8_t length = l2capinbuf[10] >> 1; // Get length
uint8_t offset = l2capinbuf[4] - length - 4; // See if there is credit uint8_t offset = l2capinbuf[4] - length - 4; // Check if there is credit
if (rfcommAvailable + length <= sizeof (rfcommDataBuffer)) { // Don't add data to buffer if it would be full if (checkFcs(&l2capinbuf[8], l2capinbuf[11 + length + offset])) {
for (uint8_t i = 0; i < length; i++) uint8_t i = 0;
rfcommDataBuffer[rfcommAvailable + i] = l2capinbuf[11 + i + offset]; for (; i < length; i++) {
rfcommAvailable += length; 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 += i;
#ifdef EXTRADEBUG #ifdef EXTRADEBUG
Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80); Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80);
Notify(rfcommAvailable, 0x80); Notify(rfcommAvailable, 0x80);
@ -282,6 +289,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
Notify(PSTR(" - Credit: 0x"), 0x80); Notify(PSTR(" - Credit: 0x"), 0x80);
D_PrintHex<uint8_t > (l2capinbuf[11], 0x80); D_PrintHex<uint8_t > (l2capinbuf[11], 0x80);
} }
#endif
}
#ifdef DEBUG_USB_HOST
else
Notify(PSTR("\r\nError in FCS checksum!"), 0x80);
#endif #endif
#ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth
for (uint8_t i = 0; i < length; i++) 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 */ /* CRC on 2 bytes */
uint8_t SPP::__crc(uint8_t* data) { 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]])); 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) { uint8_t SPP::calcFcs(uint8_t *data) {
if ((data[1] & 0xEF) == RFCOMM_UIH) if ((data[1] & 0xEF) == RFCOMM_UIH)
return (0xff - __crc(data)); // FCS on 2 bytes return (0xFF - crc(data)); // FCS on 2 bytes
else 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 */ /* Serial commands */

3
SPP.h
View file

@ -227,6 +227,7 @@ private:
void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t *data, uint8_t length); 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); 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 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 #endif