ci2_pack and tests
This commit is contained in:
+596
@@ -0,0 +1,596 @@
|
||||
/* - | Copyright / About | ----------------------------------------------------
|
||||
Copyright (c) 2026 Randy Jordan
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of
|
||||
this software and associated documentation files (the "Software"), to deal in
|
||||
the Software without restriction, including without limitation the rights to
|
||||
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
|
||||
of the Software, and to permit persons to whom the Software is furnished to do
|
||||
so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
||||
* --------------------------------------------------------------------------*/
|
||||
#include "../include/platform/mem/ci2_pack.h"
|
||||
#include <ctype.h>
|
||||
#include <stdarg.h>
|
||||
#include <string.h>
|
||||
|
||||
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) + 0.5f);
|
||||
|
||||
// get the biased exponent
|
||||
exp = shift + ((1 << (expbits - 1)) - 1); // shift + bias
|
||||
|
||||
// return the final answer
|
||||
return (sign << (bits - 1)) | (exp << (bits - expbits - 1)) | significand;
|
||||
}
|
||||
|
||||
long double
|
||||
unpack754(uint64_t i, unsigned bits, unsigned expbits)
|
||||
{
|
||||
long double result;
|
||||
long long shift;
|
||||
unsigned bias;
|
||||
unsigned significandbits = bits - expbits - 1; // -1 for sign bit
|
||||
|
||||
if (i == 0)
|
||||
return 0.0;
|
||||
|
||||
// pull the significand
|
||||
result = (i & ((1LL << significandbits) - 1)); // mask
|
||||
result /= (1LL << significandbits); // convert back to float
|
||||
result += 1.0f; // add the one back on
|
||||
|
||||
// deal with the exponent
|
||||
bias = (1 << (expbits - 1)) - 1;
|
||||
shift = ((i >> significandbits) & ((1LL << expbits) - 1)) - bias;
|
||||
while (shift > 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;
|
||||
}
|
||||
|
||||
static inline void
|
||||
packi8(uint8_t* buf, int8_t v)
|
||||
{
|
||||
*buf = (uint8_t)v;
|
||||
}
|
||||
|
||||
static inline void
|
||||
packu8(uint8_t* buf, uint8_t v)
|
||||
{
|
||||
*buf = v;
|
||||
}
|
||||
|
||||
static inline void
|
||||
packi16(uint8_t* buf, int16_t v)
|
||||
{
|
||||
uint16_t u = (uint16_t)v;
|
||||
buf[0] = (uint8_t)(u >> 8);
|
||||
buf[1] = (uint8_t)(u);
|
||||
}
|
||||
|
||||
static inline void
|
||||
packu16(uint8_t* buf, uint16_t v)
|
||||
{
|
||||
buf[0] = (uint8_t)(v >> 8);
|
||||
buf[1] = (uint8_t)(v);
|
||||
}
|
||||
|
||||
static inline void
|
||||
packi32(uint8_t* buf, int32_t v)
|
||||
{
|
||||
uint32_t u = (uint32_t)v;
|
||||
buf[0] = (uint8_t)(u >> 24);
|
||||
buf[1] = (uint8_t)(u >> 16);
|
||||
buf[2] = (uint8_t)(u >> 8);
|
||||
buf[3] = (uint8_t)(u);
|
||||
}
|
||||
|
||||
static inline void
|
||||
packu32(uint8_t* buf, uint32_t v)
|
||||
{
|
||||
buf[0] = (uint8_t)(v >> 24);
|
||||
buf[1] = (uint8_t)(v >> 16);
|
||||
buf[2] = (uint8_t)(v >> 8);
|
||||
buf[3] = (uint8_t)(v);
|
||||
}
|
||||
|
||||
static inline void
|
||||
packi64(uint8_t* buf, int64_t v)
|
||||
{
|
||||
uint64_t u = (uint64_t)v;
|
||||
buf[0] = (uint8_t)(u >> 56);
|
||||
buf[1] = (uint8_t)(u >> 48);
|
||||
buf[2] = (uint8_t)(u >> 40);
|
||||
buf[3] = (uint8_t)(u >> 32);
|
||||
buf[4] = (uint8_t)(u >> 24);
|
||||
buf[5] = (uint8_t)(u >> 16);
|
||||
buf[6] = (uint8_t)(u >> 8);
|
||||
buf[7] = (uint8_t)(u);
|
||||
}
|
||||
|
||||
static inline void
|
||||
packu64(uint8_t* buf, uint64_t v)
|
||||
{
|
||||
buf[0] = (uint8_t)(v >> 56);
|
||||
buf[1] = (uint8_t)(v >> 48);
|
||||
buf[2] = (uint8_t)(v >> 40);
|
||||
buf[3] = (uint8_t)(v >> 32);
|
||||
buf[4] = (uint8_t)(v >> 24);
|
||||
buf[5] = (uint8_t)(v >> 16);
|
||||
buf[6] = (uint8_t)(v >> 8);
|
||||
buf[7] = (uint8_t)(v);
|
||||
}
|
||||
|
||||
static inline int8_t
|
||||
unpacki8(const uint8_t* buf)
|
||||
{
|
||||
return (int8_t)buf[0];
|
||||
}
|
||||
|
||||
static inline uint8_t
|
||||
unpacku8(const uint8_t* buf)
|
||||
{
|
||||
return buf[0];
|
||||
}
|
||||
|
||||
static inline int16_t
|
||||
unpacki16(const uint8_t* buf)
|
||||
{
|
||||
uint16_t u = ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]);
|
||||
return (int16_t)u;
|
||||
}
|
||||
|
||||
static inline uint16_t
|
||||
unpacku16(const uint8_t* buf)
|
||||
{
|
||||
return ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]);
|
||||
}
|
||||
|
||||
static inline int32_t
|
||||
unpacki32(const uint8_t* buf)
|
||||
{
|
||||
uint32_t u = ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
|
||||
((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]);
|
||||
return (int32_t)u;
|
||||
}
|
||||
|
||||
static inline uint32_t
|
||||
unpacku32(const uint8_t* buf)
|
||||
{
|
||||
return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) |
|
||||
((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]);
|
||||
}
|
||||
|
||||
static inline int64_t
|
||||
unpacki64(const uint8_t* buf)
|
||||
{
|
||||
uint64_t u = ((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) | ((uint64_t)buf[7]);
|
||||
return (int64_t)u;
|
||||
}
|
||||
|
||||
static inline uint64_t
|
||||
unpacku64(const uint8_t* buf)
|
||||
{
|
||||
return ((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) | ((uint64_t)buf[7]);
|
||||
}
|
||||
|
||||
size_t
|
||||
ci2_pre_pack(const char* format, ...)
|
||||
{
|
||||
/* Signed Formats */
|
||||
int8_t g;
|
||||
int16_t h;
|
||||
int32_t i;
|
||||
int64_t j;
|
||||
|
||||
/* Unsigned Formats */
|
||||
uint8_t G;
|
||||
uint16_t H;
|
||||
uint32_t I;
|
||||
uint64_t J;
|
||||
|
||||
/* Float Formats */
|
||||
float f;
|
||||
double F;
|
||||
// uint32_t l; // float32
|
||||
// uint64_t L; // double-64
|
||||
|
||||
/* Strings */
|
||||
char* s; // strings
|
||||
size_t len = 0; // track string size
|
||||
size_t size = 0; // total pack size
|
||||
|
||||
/* Args */
|
||||
va_list ap;
|
||||
va_start(ap, format);
|
||||
|
||||
for (; *format != '\0'; format++) {
|
||||
switch (*format) {
|
||||
|
||||
case ' ': // Ignore spaces
|
||||
break;
|
||||
|
||||
case 'g': // 8-bit Signed
|
||||
size += 1;
|
||||
g = (int8_t)va_arg(ap, int); // promoted
|
||||
(void)g;
|
||||
break;
|
||||
|
||||
case 'G': // 8-bit Unsigned
|
||||
size += 1;
|
||||
G = (uint8_t)va_arg(ap, int); // promoted
|
||||
(void)G;
|
||||
break;
|
||||
|
||||
case 'h': // 16-bit Signed
|
||||
size += 2;
|
||||
h = (int16_t)va_arg(ap, int); // promoted
|
||||
(void)h;
|
||||
break;
|
||||
|
||||
case 'H': // 16-bit Unsigned
|
||||
size += 2;
|
||||
H = (uint16_t)va_arg(ap, int); // promoted
|
||||
(void)H;
|
||||
break;
|
||||
|
||||
case 'i': // 32-bit Signed
|
||||
size += 4;
|
||||
i = va_arg(ap, int32_t);
|
||||
(void)i;
|
||||
break;
|
||||
|
||||
case 'I': // 32-bit Unsigned
|
||||
size += 4;
|
||||
I = va_arg(ap, uint32_t);
|
||||
(void)I;
|
||||
break;
|
||||
|
||||
case 'j': // 64-bit Signed
|
||||
size += 8;
|
||||
j = va_arg(ap, int64_t);
|
||||
(void)j;
|
||||
break;
|
||||
|
||||
case 'J': // 64-bit Unigned
|
||||
size += 8;
|
||||
J = va_arg(ap, uint64_t);
|
||||
(void)J;
|
||||
break;
|
||||
|
||||
case 'f': // float 32-bit
|
||||
size += 4;
|
||||
f = (float)va_arg(ap, double); // promoted
|
||||
(void)f;
|
||||
break;
|
||||
|
||||
case 'F': // double 64-bit
|
||||
size += 8;
|
||||
F = (double)va_arg(ap, double);
|
||||
(void)F;
|
||||
break;
|
||||
|
||||
case 's': // string
|
||||
s = va_arg(ap, char*);
|
||||
len = strlen(s);
|
||||
size += len + 8;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
va_end(ap);
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
size_t
|
||||
ci2_pack(uint8_t* buf, const char* format, ...)
|
||||
{
|
||||
/* Signed Formats */
|
||||
int8_t g;
|
||||
int16_t h;
|
||||
int32_t i;
|
||||
int64_t j;
|
||||
|
||||
/* Unsigned Formats */
|
||||
uint8_t G;
|
||||
uint16_t H;
|
||||
uint32_t I;
|
||||
uint64_t J;
|
||||
|
||||
/* Float Formats */
|
||||
float f;
|
||||
double F;
|
||||
uint32_t l; // float32
|
||||
uint64_t L; // double-64
|
||||
|
||||
/* Strings */
|
||||
char* s; // strings
|
||||
size_t len = 0; // track string size
|
||||
size_t size = 0; // total pack size
|
||||
|
||||
/* Args */
|
||||
va_list ap;
|
||||
va_start(ap, format);
|
||||
|
||||
for (; *format != '\0'; format++) {
|
||||
switch (*format) {
|
||||
|
||||
case ' ': // Ignore spaces
|
||||
break;
|
||||
|
||||
case 'g': // 8-bit Signed
|
||||
size += 1;
|
||||
g = (int8_t)va_arg(ap, int); // promoted
|
||||
packi8(buf, g);
|
||||
buf += 1;
|
||||
break;
|
||||
|
||||
case 'G': // 8-bit Unsigned
|
||||
size += 1;
|
||||
G = (uint8_t)va_arg(ap, int); // promoted
|
||||
packu8(buf, G);
|
||||
buf += 1;
|
||||
break;
|
||||
|
||||
case 'h': // 16-bit Signed
|
||||
size += 2;
|
||||
h = (int16_t)va_arg(ap, int); // promoted
|
||||
packi16(buf, h);
|
||||
buf += 2;
|
||||
break;
|
||||
|
||||
case 'H': // 16-bit Unsigned
|
||||
size += 2;
|
||||
H = (uint16_t)va_arg(ap, int); // promoted
|
||||
packu16(buf, H);
|
||||
buf += 2;
|
||||
break;
|
||||
|
||||
case 'i': // 32-bit Signed
|
||||
size += 4;
|
||||
i = va_arg(ap, int32_t);
|
||||
packi32(buf, i);
|
||||
buf += 4;
|
||||
break;
|
||||
|
||||
case 'I': // 32-bit Unsigned
|
||||
size += 4;
|
||||
I = va_arg(ap, uint32_t);
|
||||
packu32(buf, I);
|
||||
buf += 4;
|
||||
break;
|
||||
|
||||
case 'j': // 64-bit Signed
|
||||
size += 8;
|
||||
j = va_arg(ap, int64_t);
|
||||
packi64(buf, j);
|
||||
buf += 8;
|
||||
break;
|
||||
|
||||
case 'J': // 64-bit Unigned
|
||||
size += 8;
|
||||
J = va_arg(ap, uint64_t);
|
||||
packu64(buf, J);
|
||||
buf += 8;
|
||||
break;
|
||||
|
||||
case 'f': // float 32-bit
|
||||
size += 4;
|
||||
f = (float)va_arg(ap, double); // promoted
|
||||
l = pack754_32(f); // convert to IEEE 754
|
||||
packi32(buf, l);
|
||||
buf += 4;
|
||||
break;
|
||||
|
||||
case 'F': // double 64-bit
|
||||
size += 8;
|
||||
F = (double)va_arg(ap, double);
|
||||
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 + 8;
|
||||
packu64(buf, (uint64_t)len);
|
||||
buf += 8;
|
||||
memcpy(buf, s, len);
|
||||
buf += len;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
va_end(ap);
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
size_t
|
||||
ci2_unpack(uint8_t* buf, const char* format, ...)
|
||||
{
|
||||
int8_t* g;
|
||||
int16_t* h;
|
||||
int32_t* i;
|
||||
int64_t* j;
|
||||
|
||||
uint8_t* G;
|
||||
uint16_t* H;
|
||||
uint32_t* I;
|
||||
uint64_t* J;
|
||||
|
||||
float* f;
|
||||
double* F;
|
||||
|
||||
char* s;
|
||||
uint64_t len = 0;
|
||||
size_t count = 0;
|
||||
size_t size = 0;
|
||||
size_t maxstrlen = 0;
|
||||
|
||||
va_list ap;
|
||||
va_start(ap, format);
|
||||
|
||||
for (; *format != '\0'; format++) {
|
||||
switch (*format) {
|
||||
case 'g':
|
||||
g = va_arg(ap, int8_t*);
|
||||
*g = unpacki8(buf);
|
||||
buf += 1;
|
||||
size += 1;
|
||||
break;
|
||||
|
||||
case 'G':
|
||||
G = va_arg(ap, uint8_t*);
|
||||
*G = unpacku8(buf);
|
||||
buf += 1;
|
||||
size += 1;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
h = va_arg(ap, int16_t*);
|
||||
*h = unpacki16(buf);
|
||||
buf += 2;
|
||||
size += 2;
|
||||
break;
|
||||
|
||||
case 'H':
|
||||
H = va_arg(ap, uint16_t*);
|
||||
*H = unpacku16(buf);
|
||||
buf += 2;
|
||||
size += 2;
|
||||
break;
|
||||
|
||||
case 'i':
|
||||
i = va_arg(ap, int32_t*);
|
||||
*i = unpacki32(buf);
|
||||
buf += 4;
|
||||
size += 4;
|
||||
break;
|
||||
|
||||
case 'I':
|
||||
I = va_arg(ap, uint32_t*);
|
||||
*I = unpacku32(buf);
|
||||
buf += 4;
|
||||
size += 4;
|
||||
break;
|
||||
|
||||
case 'j':
|
||||
j = va_arg(ap, int64_t*);
|
||||
*j = unpacki64(buf);
|
||||
buf += 8;
|
||||
size += 8;
|
||||
break;
|
||||
|
||||
case 'J':
|
||||
J = va_arg(ap, uint64_t*);
|
||||
*J = unpacku64(buf);
|
||||
buf += 8;
|
||||
size += 8;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
f = va_arg(ap, float*);
|
||||
*f = unpack754_32(unpacku32(buf));
|
||||
buf += 4;
|
||||
size += 4;
|
||||
break;
|
||||
|
||||
case 'F':
|
||||
F = va_arg(ap, double*);
|
||||
*F = unpack754_64(unpacku64(buf));
|
||||
buf += 8;
|
||||
size += 8;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
s = va_arg(ap, char*);
|
||||
len = unpacku64(buf);
|
||||
buf += 8;
|
||||
size += 8 + (size_t)len;
|
||||
|
||||
if (maxstrlen > 0 && len > maxstrlen)
|
||||
count = maxstrlen - 1;
|
||||
else
|
||||
count = (size_t)len;
|
||||
|
||||
memcpy(s, buf, count);
|
||||
s[count] = '\0';
|
||||
buf += len;
|
||||
break;
|
||||
|
||||
default:
|
||||
if (isdigit((unsigned char)*format)) {
|
||||
maxstrlen = maxstrlen * 10 + (*format - '0');
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (!isdigit((unsigned char)*format))
|
||||
maxstrlen = 0;
|
||||
}
|
||||
|
||||
va_end(ap);
|
||||
return size;
|
||||
}
|
||||
Reference in New Issue
Block a user