597 lines
12 KiB
C
597 lines
12 KiB
C
/* - | 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;
|
|
}
|