fix bcd encoder

1 files changed, 33 insertions, 1 deletions

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;
+}
+
+
+