From 9ada072d5337a582e987ed57dc4816189208dea7 Mon Sep 17 00:00:00 2001
From: root <root@no.no.james.local>
Date: Tue, 26 Feb 2019 15:38:49 +0000
Subject: fix bcd encoder

---
 util.c | 34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

(limited to 'util.c')

diff --git a/util.c b/util.c
index 8be15a4..d8632c5 100644
--- a/util.c
+++ b/util.c
@@ -27,7 +27,6 @@ void  bcd_set (uint8_t *d, unsigned s, unsigned e, unsigned v)
       c <<= 1;
     else {
       c >>= 3;
-      c *= 10;
       v = v / 10;
       w = v % 10;
     }
@@ -35,4 +34,37 @@ void  bcd_set (uint8_t *d, unsigned s, unsigned e, unsigned v)
 }
 
 
+int check_parity (uint8_t *d, unsigned s, unsigned e, uint8_t p)
+{
+  unsigned i;
+
+  for (i = s; i <= e; ++i)
+    p ^= d[i];
+
+  return !p;
+}
+
+
+unsigned bcd (uint8_t *d, unsigned s, unsigned e)
+{
+  unsigned ret = 0, c, i;
+
+  for (i = e, c = 1 ; i >= s; --i) {
+
+    if (d[i]) ret += c;
+
+
+    if (c & 0x77777777)
+      c <<= 1;
+    else {
+      c >>= 3;
+      c *= 10;
+    }
+  }
+
+  return ret;
+}
+
+
+
 
-- 
cgit v1.2.3