diff options
Diffstat (limited to 'util.c')
-rw-r--r-- | util.c | 34 |
1 files changed, 33 insertions, 1 deletions
@@ -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; +} + + + |