From 24c8bcfe7f76b20124e2862ea050f815c0f768e7 Mon Sep 17 00:00:00 2001 From: tlatorre Date: Tue, 14 Aug 2018 10:08:27 -0500 Subject: move everything to src directory --- src/pack2b.c | 319 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 319 insertions(+) create mode 100644 src/pack2b.c (limited to 'src/pack2b.c') diff --git a/src/pack2b.c b/src/pack2b.c new file mode 100644 index 0000000..25e154d --- /dev/null +++ b/src/pack2b.c @@ -0,0 +1,319 @@ +#include +#include +#include +#include +#include +#include +#include "pack2b.h" + +/* +** pack754() -- pack a floating point number into IEEE-754 format +*/ +uint64_t pack754(long double f, unsigned bits, unsigned expbits) +{ + long double fnorm; + int shift; + long long sign, exp, significand; + unsigned significandbits = bits - expbits - 1; // -1 for sign bit + + if (f == 0.0) return 0; // get this special case out of the way + + // check sign and begin normalization + if (f < 0) { sign = 1; fnorm = -f; } + else { sign = 0; fnorm = f; } + + // get the normalized form of f and track the exponent + shift = 0; + while(fnorm >= 2.0) { fnorm /= 2.0; shift++; } + while(fnorm < 1.0) { fnorm *= 2.0; shift--; } + fnorm = fnorm - 1.0; + + // calculate the binary form (non-float) of the significand data + significand = fnorm * ((1LL<>significandbits)&((1LL< 0) { result *= 2.0; shift--; } + while(shift < 0) { result /= 2.0; shift++; } + + // sign it + result *= (i>>(bits-1))&1? -1.0: 1.0; + + return result; +} + +/* +** packi16() -- store a 16-bit int into a char buffer (like htons()) +*/ +void packi16(uint8_t *buf, int16_t i) +{ + uint16_t i2 = i; + + *buf++ = i2>>8; *buf++ = i2; +} + +/* +** packi32() -- store a 32-bit int into a char buffer (like htonl()) +*/ +void packi32(uint8_t *buf, int32_t i) +{ + uint32_t i2 = i; + + *buf++ = i2>>24; *buf++ = i2>>16; + *buf++ = i2>>8; *buf++ = i2; +} + +/* +** packi64() -- store a 64-bit int into a char buffer (like htonl()) +*/ +void packi64(uint8_t *buf, int64_t i) +{ + uint64_t i2 = i; + + *buf++ = i2>>56; *buf++ = i2>>48; + *buf++ = i2>>40; *buf++ = i2>>32; + *buf++ = i2>>24; *buf++ = i2>>16; + *buf++ = i2>>8; *buf++ = i2; +} + +/* +** unpacki16() -- unpack a 16-bit int from a char buffer (like ntohs()) +*/ +int16_t unpacki16(uint8_t *buf) +{ + uint16_t i2 = ((uint16_t)buf[0]<<8) | buf[1]; + int16_t i; + + // change unsigned numbers to signed + if (i2 <= 0x7fffu) { i = i2; } + //else { i = -(int16_t)((uint16_t)0xffff - i2 + (uint16_t)1u); } + else { i = -1 - (uint16_t)(0xffffu - i2); } + + return i; +} + +/* +** unpacki32() -- unpack a 32-bit int from a char buffer (like ntohl()) +*/ +int32_t unpacki32(uint8_t *buf) +{ + uint32_t i2 = ((uint32_t)buf[0]<<24) | ((uint32_t)buf[1]<<16) | + ((uint32_t)buf[2]<<8) | buf[3]; + int32_t i; + + // change unsigned numbers to signed + if (i2 <= 0x7fffffffu) { i = i2; } + else { i = -1 - (int32_t)(0xffffffffu - i2); } + + return i; +} + +/* +** unpacki64() -- unpack a 64-bit int from a char buffer (like ntohl()) +*/ +int64_t unpacki64(uint8_t *buf) +{ + uint64_t i2 = ((uint64_t)buf[0]<<56) | ((uint64_t)buf[1]<<48) | + ((uint64_t)buf[2]<<40) | ((uint64_t)buf[3]<<32) | + ((uint64_t)buf[4]<<24) | ((uint64_t)buf[5]<<16) | + ((uint64_t)buf[6]<<8) | buf[7]; + int64_t i; + + // change unsigned numbers to signed + if (i2 <= 0x7fffffffffffffffu) { i = i2; } + else { i = -1 -(int64_t)(0xffffffffffffffffu - i2); } + + return i; +} + +/* +** pack() -- store data dictated by the format string in the buffer +** +** c - 8-bit signed int h - 16-bit signed int +** l - 32-bit signed int f - 32-bit float +** L - 64-bit signed int F - 64-bit float +** s - string (16-bit length is automatically prepended) +*/ +int32_t pack(uint8_t *buf, char *format, ...) +{ + va_list ap; + int16_t h; + int32_t l; + int64_t L; + int8_t c; + float32_t f; + float64_t F; + char *s; + int32_t size = 0, len; + + va_start(ap, format); + + for(; *format != '\0'; format++) { + switch(*format) { + case 'h': // 16-bit + size += 2; + h = (int16_t)va_arg(ap, int); // promoted + packi16(buf, h); + buf += 2; + break; + + case 'l': // 32-bit + size += 4; + l = va_arg(ap, int32_t); + packi32(buf, l); + buf += 4; + break; + + case 'L': // 64-bit + size += 8; + L = va_arg(ap, int64_t); + packi64(buf, L); + buf += 8; + break; + + case 'c': // 8-bit + size += 1; + c = (int8_t)va_arg(ap, int); // promoted + *buf++ = (c>>0)&0xff; + break; + + case 'f': // float + size += 4; + f = (float32_t)va_arg(ap, double); // promoted + l = pack754_32(f); // convert to IEEE 754 + packi32(buf, l); + buf += 4; + break; + + case 'F': // float-64 + size += 8; + F = (float64_t)va_arg(ap, float64_t); + L = pack754_64(F); // convert to IEEE 754 + packi64(buf, L); + buf += 8; + break; + + case 's': // string + s = va_arg(ap, char*); + len = strlen(s); + size += len + 2; + packi16(buf, len); + buf += 2; + memcpy(buf, s, len); + buf += len; + break; + } + } + + va_end(ap); + + return size; +} + +/* +** unpack() -- unpack data dictated by the format string into the buffer +*/ +void unpack(uint8_t *buf, char *format, ...) +{ + va_list ap; + int16_t *h; + int32_t *l; + int64_t *L; + int32_t pf; + int64_t pF; + int8_t *c; + float32_t *f; + float64_t *F; + char *s; + int32_t len, count, maxstrlen=0; + + va_start(ap, format); + + for(; *format != '\0'; format++) { + switch(*format) { + case 'h': // 16-bit + h = va_arg(ap, int16_t*); + *h = unpacki16(buf); + buf += 2; + break; + + case 'l': // 32-bit + l = va_arg(ap, int32_t*); + *l = unpacki32(buf); + buf += 4; + break; + + case 'L': // 64-bit + L = va_arg(ap, int64_t*); + *L = unpacki64(buf); + buf += 8; + break; + + case 'c': // 8-bit + c = va_arg(ap, int8_t*); + if (*buf <= 0x7f) { *c = *buf;} + else { *c = -1 - (uint8_t)(0xffu - *buf); } + buf++; + break; + + case 'f': // float + f = va_arg(ap, float32_t*); + pf = unpacki32(buf); + buf += 4; + *f = unpack754_32(pf); + break; + + case 'F': // float-64 + F = va_arg(ap, float64_t*); + pF = unpacki64(buf); + buf += 8; + *F = unpack754_64(pF); + break; + + case 's': // string + s = va_arg(ap, char*); + len = unpacki16(buf); + buf += 2; + if (maxstrlen > 0 && len > maxstrlen) count = maxstrlen - 1; + else count = len; + memcpy(s, buf, count); + s[count] = '\0'; + buf += len; + break; + + default: + if (isdigit(*format)) { // track max str len + maxstrlen = maxstrlen * 10 + (*format-'0'); + } + } + + if (!isdigit(*format)) maxstrlen = 0; + } + + va_end(ap); +} -- cgit