Unverified Commit 965aa3ee authored by Birte Kristina Friesel's avatar Birte Kristina Friesel
Browse files

sen5x: verify crc when reading data

parent 0e79b4fd

include/driver/sen5x.h

+3 −0
Original line number Diff line number Diff line
@@ -13,6 +13,9 @@ class SEN5x { 
		unsigned char txbuf[2];

		unsigned char rxbuf[24];



		unsigned char crcWord(unsigned char byte1, unsigned char byte2);

		bool crcValid(unsigned char* data, unsigned char length);



	public:

		SEN5x() {}

src/driver/sen5x.cc

+32 −0
Original line number Diff line number Diff line
@@ -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;