layer2: crc checking

1 files changed, 67 insertions, 17 deletions

diff --git a/Network/EBus/Layer2.hs b/Network/EBus/Layer2.hs
index b194ee6..04d652c 100644
--- a/Network/EBus/Layer2.hs
+++ b/Network/EBus/Layer2.hs
@@ -1,9 +1,17 @@
 module Network.EBus.Layer2(
   ebusParserLayer2,
-  EbusPacket) where
+  EbusPacket,
+  ebusPacketType,
+  ebusPacketSource,
+  ebusPacketDestination,
+  ebusPacketPrimaryCommand,
+  ebusPacketSecondaryCommand,
+  ebusPacketPayload,
+  ebusPacketPayloadSlave) where
 
 import Control.Applicative ((<|>))
 import Data.Attoparsec (anyWord8, count, skipMany, try, Parser, word8)
+import Data.Bits (xor)
 import Data.Word (Word8)
 
 -- | Ebus Layer2 Constants
@@ -22,7 +30,7 @@ data EbusType = EbusBroadcast
               | EbusMasterSlave
               deriving (Show)
 
--- | Ebus Packet representation
+-- | Ebus Packet
 data EbusPacket = EbusPacket { 
   ebusPacketType :: EbusType,
   ebusPacketSource :: Word8,
@@ -35,12 +43,12 @@ data EbusPacket = EbusPacket {
 
 -- | Ebus Binary Escape Sequences for payload data
 parserPayload :: Parser Word8
-parserPayload = do{ word8 $ ebusConstant "ESCAPE"
-                  ; word8 $ ebusConstant "ESCAPE_ESCAPE"
+parserPayload = do{ _ <- word8 $ ebusConstant "ESCAPE"
+                  ; _ <- word8 $ ebusConstant "ESCAPE_ESCAPE"
                   ; return $ ebusConstant "ESCAPE"}
                 <|>
-                do{ word8 $ ebusConstant "ESCAPE"
-                  ; word8 $ ebusConstant "ESCAPE_SYN"
+                do{ _ <- word8 $ ebusConstant "ESCAPE"
+                  ; _ <- word8 $ ebusConstant "ESCAPE_SYN"
                   ; return $ ebusConstant "SYN"}
                 <|>
                 anyWord8;
@@ -59,9 +67,11 @@ ebusParserLayer2 = do{
     if destination == 0xfe then
       -- Broadcast
       do{ crc <- anyWord8
-        ; {- syn -} word8 $ ebusConstant "SYN"
-        ; if True then -- CHECK CRC
-            return $ EbusPacket EbusMasterMaster source destination primaryCommand secondaryCommand payload Nothing
+        ; _ <- {- syn -} word8 $ ebusConstant "SYN"
+        ; if ebusCrc ([source, destination, 
+                       primaryCommand, secondaryCommand, payloadLength] 
+                      ++ payload) == crc then
+            return $ EbusPacket EbusBroadcast source destination primaryCommand secondaryCommand payload Nothing
           else
             fail "Broadcast: CRC Check failed"}
       <|>
@@ -70,9 +80,11 @@ ebusParserLayer2 = do{
       -- Master-Master
       try( 
         do{ crc <- anyWord8
-          ; {- ack -} word8 $ ebusConstant "ACK"
-          ; {- syn -} word8 $ ebusConstant "SYN"
-          ; if True then -- CHECK CRC
+          ; _ <- {- ack -} word8 $ ebusConstant "ACK"
+          ; _ <- {- syn -} word8 $ ebusConstant "SYN"
+          ; if ebusCrc ([source, destination, 
+                         primaryCommand, secondaryCommand, payloadLength] 
+                        ++ payload) == crc then
               return $ EbusPacket EbusMasterMaster source destination primaryCommand secondaryCommand payload Nothing
             else
               fail "Master-Master: CRC Check failed"})
@@ -80,13 +92,16 @@ ebusParserLayer2 = do{
       -- Master Slave
       try(
         do{ crc <- anyWord8
-          ; {- ack -} word8 $ ebusConstant "ACK"
+          ; _ <- {- ack -} word8 $ ebusConstant "ACK"
           ; payloadSlaveLength <- anyWord8
           ; payloadSlave <- count (fromIntegral payloadSlaveLength) parserPayload
           ; crcSlave <- anyWord8
-          ; {- ackSlave -} word8 $ ebusConstant "ACK"
-          ; {- synSlave -} word8 $ ebusConstant "SYN"
-          ; if True then -- Check CRC
+          ; _ <- {- ackSlave -} word8 $ ebusConstant "ACK"
+          ; _ <- {- synSlave -} word8 $ ebusConstant "SYN"
+          ; if ebusCrc ([source, destination, 
+                         primaryCommand, secondaryCommand, payloadLength] 
+                        ++ payload) == crc &&
+               ebusCrc ([payloadSlaveLength] ++ payloadSlave) == crcSlave then
               return $ EbusPacket EbusMasterSlave source destination primaryCommand secondaryCommand payload (Just payloadSlave)
             else
               fail "Master-Slave: CRC Check failed"})
@@ -95,4 +110,39 @@ ebusParserLayer2 = do{
       fail "Failed to parse Master-Master/Master-Slave Packet"
   ; return followup
     <|> fail "Failed to parse packet"
-  }
\ No newline at end of file
+  }
+
+-- | Ebus CRC Calculation using a Table of precalculated values
+ebusCrc :: [Word8] -> Word8
+ebusCrc input = foldl step 0 input
+  where step acc x = (crctab (fromIntegral acc)) `xor` x
+
+-- | Table for CRC Calculation
+crctab :: Int -> Word8
+crctab pos = [
+    0, 155, 173, 54, 193, 90, 108, 247, 25, 130,   --0 -9
+    180, 47, 216, 67, 117, 238, 50, 169, 159, 4,   --10-19
+    243, 104, 94, 197, 43, 176, 134, 29, 234, 113, --20
+    71, 220, 100, 255, 201, 82, 165, 62, 8, 147,   --30
+    125, 230, 208, 75, 188, 39, 17, 138, 86, 205,
+    251, 96, 151, 12, 58, 161, 79, 212, 226, 121,
+    142, 21, 35, 184, 200, 83, 101, 254, 9, 146,
+    164, 63, 209, 74, 124, 231, 16, 139, 189, 38,
+    250, 97, 87, 204, 59, 160, 150, 13, 227, 120,
+    78, 213, 34, 185, 143, 20, 172, 55, 1, 154,
+    109, 246, 192, 91, 181, 46, 24, 131, 116, 239, --100
+    217, 66, 158, 5, 51, 168, 95, 196, 242, 105,
+    135, 28, 42, 177, 70, 221, 235, 112, 11, 144,
+    166, 61, 202, 81, 103, 252, 18, 137, 191, 36,
+    211, 72, 126, 229, 57, 162, 148, 15, 248, 99,
+    85, 206, 32, 187, 141, 22, 225, 122, 76, 215,
+    111, 244, 194, 89, 174, 53, 3, 152, 118, 237,
+    219, 64, 183, 44, 26, 129, 93, 198, 240, 107,
+    156, 7, 49, 170, 68, 223, 233, 114, 133, 30,
+    40, 179, 195, 88, 110, 245, 2, 153, 175, 52,
+    218, 65, 119, 236, 27, 128, 182, 45, 241, 106, --200
+    92, 199, 48, 171, 157, 6, 232, 115, 69, 222,
+    41, 178, 132, 31, 167, 60, 10, 145, 102, 253,
+    203, 80, 190, 37, 19, 136, 127, 228, 210, 73,
+    149, 14, 56, 163, 84, 207, 249, 98, 140, 23,
+    33, 186, 77, 214, 224, 123] !! pos
\ No newline at end of file