diff options
Diffstat (limited to 'app/util.c')
-rw-r--r-- | app/util.c | 66 |
1 files changed, 66 insertions, 0 deletions
diff --git a/app/util.c b/app/util.c new file mode 100644 index 0000000..ff35288 --- /dev/null +++ b/app/util.c @@ -0,0 +1,66 @@ +#include "project.h" + +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, b, i; + + for (i = e, c = 1, b = 0; i >= s; --i, b++) { + + if (d[i]) ret += c; + + + switch (b & 3) { + case 0: + case 1: + case 2: + c <<= 1; + break; + + default: + c >>= 3; + c *= 10; + } + } + + return ret; +} + + + +unsigned le_bcd (uint8_t *d, unsigned s, unsigned e) +{ + unsigned ret = 0, c, b, i; + + for (i = s, c = 1, b = 0; i <= e; ++i, b++) { + + if (d[i]) ret += c; + + + switch (b & 3) { + case 0: + case 1: + case 2: + c <<= 1; + break; + + default: + c >>= 3; + c *= 10; + } + } + + return ret; +} + + |