From d7bb54ec6bf9f2295b13d55549f4a31d2b8dde35 Mon Sep 17 00:00:00 2001
From: Kari Hautio <khautio@gmail.com>
Date: Mon, 17 Feb 2014 17:14:04 +0200
Subject: [PATCH] Moving CRC16 routine to common code (to be used by EEPROM
 code)

---
 TX.h        | 28 +++++++---------------------
 common.h    | 20 ++++++++++++++++++++
 serialPPM.h | 17 ++++-------------
 3 files changed, 31 insertions(+), 34 deletions(-)

diff --git a/TX.h b/TX.h
index 4fb23846..7b605bd1 100644
--- a/TX.h
+++ b/TX.h
@@ -427,7 +427,6 @@ uint8_t compositeRSSI(uint8_t rssi, uint8_t linkq)
 
 uint8_t frameIndex=0;
 uint32_t srxLast=0;
-uint16_t srxCRC=0;
 uint8_t srxFlags=0;
 uint8_t srxChannels=0;
 
@@ -495,41 +494,28 @@ void processSBUS(uint8_t c)
   }
 }
 
-void sumdCRC16(uint8_t c)
-{
-  uint8_t i;
-  srxCRC ^= (uint16_t)c<<8;
-  for (i = 0; i < 8; i++) {
-    if (srxCRC & 0x8000) {
-      srxCRC = (srxCRC << 1) ^ 0x1021;
-    } else {
-      srxCRC = (srxCRC << 1);
-    }
-  }
-}
-
 void processSUMD(uint8_t c)
 {
   if ((frameIndex == 0) && (c == SUMD_HEAD)) {
-    srxCRC=0;
-    sumdCRC16(c);
+    CRC16_reset();
+    CRC16_add(c);
     frameIndex=1;
   } else {
     if (frameIndex == 1) {
       srxFlags = c;
-      sumdCRC16(c);
+      CRC16_add(c);
     } else if (frameIndex == 2) {
       srxChannels = c;
-      sumdCRC16(c);
+      CRC16_add(c);
     } else if (frameIndex < (3 + (srxChannels << 1))) {
       if (frameIndex < 35) {
         ppmWork.bytes[frameIndex-3] = c;
       }
-      sumdCRC16(c);
+      CRC16_add(c);
     } else if (frameIndex == (3 + (srxChannels << 1))) {
-      srxCRC ^= (uint16_t)c << 8;
+      CRC16_value ^= (uint16_t)c << 8;
     } else {
-      if ((srxCRC == c) && (srxFlags == 0x01)) {
+      if ((CRC16_value == c) && (srxFlags == 0x01)) {
         uint8_t ch;
         if (srxChannels > 16) {
           srxChannels = 16;
diff --git a/common.h b/common.h
index 6536e5df..e0e47ccb 100644
--- a/common.h
+++ b/common.h
@@ -148,6 +148,26 @@ uint8_t countSetBits(uint16_t x)
   return (x * 0x0101) >> 8;
 }
 
+static uint16_t CRC16_value;
+
+inline void CRC16_reset()
+{
+  CRC16_value = 0;
+}
+
+void CRC16_add(uint8_t c) // CCITT polynome
+{
+  uint8_t i;
+  CRC16_value ^= (uint16_t)c << 8;
+  for (i = 0; i < 8; i++) {
+    if (CRC16_value & 0x8000) {
+      CRC16_value = (CRC16_value << 1) ^ 0x1021;
+    } else {
+      CRC16_value = (CRC16_value << 1);
+    }
+  }
+}
+
 // Halt and blink failure code
 void fatalBlink(uint8_t blinks)
 {
diff --git a/serialPPM.h b/serialPPM.h
index b432006e..34f8090d 100644
--- a/serialPPM.h
+++ b/serialPPM.h
@@ -78,20 +78,11 @@ void sendSBUSFrame(uint8_t failsafe, uint8_t lostpack)
 }
 
 #define SUMD_HEAD 0xa8
-uint16_t sumdCRC;
 
 void sumdWriteCRC(uint8_t c)
 {
-  uint8_t i;
   Serial.write(c);
-  sumdCRC ^= (uint16_t)c<<8;
-  for (i = 0; i < 8; i++) {
-    if (sumdCRC & 0x8000) {
-      sumdCRC = (sumdCRC << 1) ^ 0x1021;
-    } else {
-      sumdCRC = (sumdCRC << 1);
-    }
-  }
+  CRC16_add(c);
 }
 
 void sendSUMDFrame(uint8_t failsafe)
@@ -99,7 +90,7 @@ void sendSUMDFrame(uint8_t failsafe)
   uint32_t now = micros();
   if ((now - sOutLast) > 10000) {
     sOutLast = now;
-    sumdCRC = 0;
+    CRC16_reset();
     sumdWriteCRC(SUMD_HEAD);
     sumdWriteCRC(failsafe ? 0x81 : 0x01);
     sumdWriteCRC(16);
@@ -108,7 +99,7 @@ void sendSUMDFrame(uint8_t failsafe)
       sumdWriteCRC(val >> 8);
       sumdWriteCRC(val & 0xff);
     }
-    Serial.write(sumdCRC >> 8);
-    Serial.write(sumdCRC & 0xff);
+    Serial.write(CRC16_value >> 8);
+    Serial.write(CRC16_value & 0xff);
   }
 }