diff options
Diffstat (limited to 'bcdfp.c')
-rw-r--r-- | bcdfp.c | 56 |
1 files changed, 56 insertions, 0 deletions
@@ -0,0 +1,56 @@ +#include <string.h> +#include "bcdfp.h" + +extern void die(const char *msg); + +unsigned char bcd2int(unsigned char bcd) { + return (bcd >> 4) * 10 + (bcd & 0x0f); +} + +unsigned char int2bcd(unsigned char i) { + return ((i / 10) << 4) | (i % 10); +} + +unsigned short fp2int(const unsigned char *fp) { + unsigned short result = 0; + + /* examine the exponent/sign byte */ + if(fp[0] == 0) return 0; /* special case */ + if(fp[0] & 0x80) die("negative numbers not supported"); + + switch(fp[0]) { + case 0x40: + result = bcd2int(fp[1]); break; + case 0x41: + result = bcd2int(fp[1]) * 100 + bcd2int(fp[2]); break; + case 0x42: + result = bcd2int(fp[1]) * 10000 + bcd2int(fp[2]) * 100 + bcd2int(fp[3]); break; + default: + die("number out of range"); break; + } + + return result; +} + +void int2fp(unsigned short num, unsigned char *fp) { + memset(fp, 0, 6); + + if(num == 0) return; + + if(num >= 10000) { + fp[0] = 0x42; + fp[3] = int2bcd(num % 100); + num /= 100; + fp[2] = int2bcd(num % 100); + num /= 100; + fp[1] = int2bcd(num); + } else if(num >= 100) { + fp[0] = 0x41; + fp[2] = int2bcd(num % 100); + num /= 100; + fp[1] = int2bcd(num); + } else { + fp[0] = 0x40; + fp[1] = int2bcd(num); + } +} |