diff options
author | Daniel Friesel <derf@finalrewind.org> | 2022-05-26 09:36:26 +0200 |
---|---|---|
committer | Daniel Friesel <derf@finalrewind.org> | 2022-05-26 09:36:26 +0200 |
commit | 965aa3eec1b6da26848d47f8b4fb591f0bc53709 (patch) | |
tree | 23bcf4179f1e72081877e67868b28ac413d8f7a0 /src/driver/sen5x.cc | |
parent | 0e79b4fd86cb79538ff916e625b37c98c34828b1 (diff) |
sen5x: verify crc when reading data
Diffstat (limited to 'src/driver/sen5x.cc')
-rw-r--r-- | src/driver/sen5x.cc | 32 |
1 files changed, 32 insertions, 0 deletions
diff --git a/src/driver/sen5x.cc b/src/driver/sen5x.cc index 9edbcd0..28a4a0b 100644 --- a/src/driver/sen5x.cc +++ b/src/driver/sen5x.cc @@ -44,6 +44,11 @@ bool SEN5x::read() if (i2c.xmit(address, 0, txbuf, 24, rxbuf)) { return false; } + + if (!crcValid(rxbuf, 24)) { + return false; + } + pm1 = (rxbuf[0] << 8) + rxbuf[1]; pm2_5 = (rxbuf[3] << 8) + rxbuf[4]; pm4 = (rxbuf[6] << 8) + rxbuf[7]; @@ -67,6 +72,10 @@ bool SEN5x::readStatus() return false; } + if (!crcValid(rxbuf, 6)) { + return false; + } + fan_speed_warning = rxbuf[1] & 0x20; fan_cleaning_active = rxbuf[1] & 0x08; gas_sensor_error = rxbuf[4] & 0x80; @@ -77,4 +86,27 @@ bool SEN5x::readStatus() return true; } +unsigned char SEN5x::crcWord(unsigned char byte1, unsigned char byte2) +{ + unsigned char crc = 0xff ^ byte1; + for (unsigned char bit = 8; bit > 0; bit--) { + crc = (crc << 1) ^ (crc & 0x80 ? 0x31 : 0); + } + crc ^= byte2; + for (unsigned char bit = 8; bit > 0; bit--) { + crc = (crc << 1) ^ (crc & 0x80 ? 0x31 : 0); + } + return crc; +} + +bool SEN5x::crcValid(unsigned char* data, unsigned char length) +{ + for (unsigned char i = 0; i < length; i += 3) { + if (crcWord(data[i], data[i+1]) != data[i+2]) { + return false; + } + } + return true; +} + SEN5x sen5x; |