From ddbd85757401f9fcd905b820b0cd81d50a559a8f Mon Sep 17 00:00:00 2001
From: Pim van den Berg <pim.van.den.berg@mendix.com>
Date: Wed, 4 Nov 2020 15:13:46 +0100
Subject: [PATCH] feat(itho-wpu): add checksum validation for received messages

---
 itho-wpu.py | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/itho-wpu.py b/itho-wpu.py
index e06e7e5..fbc6e8c 100755
--- a/itho-wpu.py
+++ b/itho-wpu.py
@@ -68,7 +68,8 @@ class I2CSlave():
         if b:
             logger.debug(f"Received {b} bytes! Status {s}")
             result = [hex(c) for c in d]
-            logger.info(f"Response: {result}")
+            if self.is_checksum_valid(result):
+                logger.info(f"Response: {result}")
         else:
             logger.error(f"Received number of bytes was {b}")
 
@@ -81,7 +82,8 @@ class I2CSlave():
                 if b:
                     logger.debug(f"Received {b} bytes! Status {s}")
                     result = [hex(c) for c in d]
-                    q.put(result)
+                    if self.is_checksum_valid(result):
+                        q.put(result)
                 else:
                     logger.error(f"Received number of bytes was {b}")
             else:
@@ -91,6 +93,18 @@ class I2CSlave():
         else:
             self.close()
 
+    def is_checksum_valid(self, b):
+        s = 0x80
+        for i in b[:-1]:
+            s += int(i, 0)
+        checksum = 256 - (s % 256)
+        if checksum == 256:
+            checksum = 0
+        if checksum != int(b[-1], 0):
+            logger.debug(f"Checksum invalid (0x{checksum:02x})")
+            return False
+        return True
+
     def close(self):
         self.pi.bsc_i2c(0)
         self.pi.stop()