2019-02-08 10:00:00 -08:00
|
|
|
/*
|
|
|
|
* Copyright (c) 2019 Foundries.io
|
|
|
|
*
|
|
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <kernel.h>
|
2021-07-23 16:38:23 +02:00
|
|
|
#include <stdio.h>
|
2019-08-27 10:39:53 -07:00
|
|
|
#include <stdlib.h>
|
2021-07-21 13:46:05 +02:00
|
|
|
#include <ctype.h>
|
2019-02-08 10:00:00 -08:00
|
|
|
#include "lwm2m_util.h"
|
|
|
|
|
|
|
|
#define SHIFT_LEFT(v, o, m) (((v) << (o)) & (m))
|
|
|
|
#define SHIFT_RIGHT(v, o, m) (((v) >> (o)) & (m))
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
#define PRECISION64_LEN 17U
|
|
|
|
#define PRECISION64 100000000000000000ULL
|
|
|
|
|
|
|
|
#define PRECISION32 1000000000UL
|
|
|
|
|
|
|
|
/* convert from float to binary32 */
|
|
|
|
int lwm2m_float_to_b32(double *in, uint8_t *b32, size_t len)
|
2019-02-08 10:00:00 -08:00
|
|
|
{
|
2020-05-27 11:26:57 -05:00
|
|
|
int32_t e = -1, v, f = 0;
|
2021-07-23 16:38:23 +02:00
|
|
|
int32_t val1 = (int32_t)*in;
|
|
|
|
int32_t val2 = (*in - (int32_t)*in) * PRECISION32;
|
2019-02-08 10:00:00 -08:00
|
|
|
int i;
|
|
|
|
|
|
|
|
if (len != 4) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
2019-02-12 22:22:01 -08:00
|
|
|
/* handle zero value special case */
|
2021-07-23 16:38:23 +02:00
|
|
|
if (val1 == 0 && val2 == 0) {
|
2019-02-12 22:22:01 -08:00
|
|
|
memset(b32, 0, len);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-02-08 10:00:00 -08:00
|
|
|
/* sign handled later */
|
2021-07-23 16:38:23 +02:00
|
|
|
v = abs(val1);
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* add whole value to fraction */
|
|
|
|
while (v > 0) {
|
|
|
|
f >>= 1;
|
|
|
|
|
|
|
|
if (v & 1) {
|
|
|
|
f |= (1 << 23);
|
|
|
|
}
|
|
|
|
|
|
|
|
v >>= 1;
|
|
|
|
e++;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* sign handled later */
|
2021-07-23 16:38:23 +02:00
|
|
|
v = abs(val2);
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* add decimal to fraction */
|
|
|
|
i = e;
|
|
|
|
while (v > 0 && i < 23) {
|
|
|
|
v *= 2;
|
2021-07-23 16:38:23 +02:00
|
|
|
if (!f && e < 0 && v < PRECISION32) {
|
2019-02-08 10:00:00 -08:00
|
|
|
/* handle -e */
|
|
|
|
e--;
|
|
|
|
continue;
|
2021-07-23 16:38:23 +02:00
|
|
|
} else if (v >= PRECISION32) {
|
|
|
|
v -= PRECISION32;
|
2019-02-08 10:00:00 -08:00
|
|
|
f |= 1 << (22 - i);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (v == 0) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
i++;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* adjust exponent for bias */
|
|
|
|
e += 127;
|
|
|
|
|
|
|
|
memset(b32, 0, len);
|
|
|
|
|
|
|
|
/* sign: bit 31 */
|
2021-07-23 16:38:23 +02:00
|
|
|
if (val1 == 0) {
|
|
|
|
b32[0] = val2 < 0 ? 0x80 : 0;
|
2019-08-27 10:39:53 -07:00
|
|
|
} else {
|
2021-07-23 16:38:23 +02:00
|
|
|
b32[0] = val1 < 0 ? 0x80 : 0;
|
2019-08-27 10:39:53 -07:00
|
|
|
}
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* exponent: bits 30-23 */
|
|
|
|
b32[0] |= e >> 1;
|
|
|
|
b32[1] = (e & 1) << 7;
|
|
|
|
|
|
|
|
/* fraction: bits 22-0 */
|
|
|
|
/* NOTE: ignore the "hidden" bit 23 in fraction */
|
|
|
|
b32[1] |= (f >> 16) & 0x7F;
|
|
|
|
b32[2] = (f >> 8) & 0xFF;
|
|
|
|
b32[3] = f & 0xFF;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
/* convert from float to binary64 */
|
|
|
|
int lwm2m_float_to_b64(double *in, uint8_t *b64, size_t len)
|
2019-02-08 10:00:00 -08:00
|
|
|
{
|
2020-05-27 11:26:57 -05:00
|
|
|
int64_t v, f = 0;
|
|
|
|
int32_t e = -1;
|
2021-07-23 16:38:23 +02:00
|
|
|
int64_t val1 = (int64_t)*in;
|
|
|
|
int64_t val2 = (*in - (int64_t)*in) * PRECISION64;
|
2019-02-08 10:00:00 -08:00
|
|
|
int i;
|
|
|
|
|
|
|
|
if (len != 8) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
2019-02-12 22:22:01 -08:00
|
|
|
/* handle zero value special case */
|
2021-07-23 16:38:23 +02:00
|
|
|
if (val1 == 0 && val2 == 0) {
|
2019-02-12 22:22:01 -08:00
|
|
|
memset(b64, 0, len);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-02-08 10:00:00 -08:00
|
|
|
/* sign handled later */
|
2021-07-23 16:38:23 +02:00
|
|
|
v = llabs(val1);
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* add whole value to fraction */
|
|
|
|
while (v > 0) {
|
|
|
|
f >>= 1;
|
|
|
|
|
|
|
|
if (v & 1) {
|
2020-05-27 11:26:57 -05:00
|
|
|
f |= ((int64_t)1 << 52);
|
2019-02-08 10:00:00 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
v >>= 1;
|
|
|
|
e++;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* sign handled later */
|
2021-07-23 16:38:23 +02:00
|
|
|
v = llabs(val2);
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* add decimal to fraction */
|
|
|
|
i = e;
|
|
|
|
while (v > 0 && i < 52) {
|
|
|
|
v *= 2;
|
2021-07-23 16:38:23 +02:00
|
|
|
if (!f && e < 0 && v < PRECISION64) {
|
2019-02-08 10:00:00 -08:00
|
|
|
/* handle -e */
|
|
|
|
e--;
|
|
|
|
continue;
|
2021-07-23 16:38:23 +02:00
|
|
|
} else if (v >= PRECISION64) {
|
|
|
|
v -= PRECISION64;
|
2020-05-27 11:26:57 -05:00
|
|
|
f |= (int64_t)1 << (51 - i);
|
2019-02-08 10:00:00 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
if (v == 0) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
i++;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* adjust exponent for bias */
|
|
|
|
e += 1023;
|
|
|
|
|
|
|
|
memset(b64, 0, len);
|
|
|
|
|
|
|
|
/* sign: bit 63 */
|
2021-07-23 16:38:23 +02:00
|
|
|
if (val1 == 0) {
|
|
|
|
b64[0] = val2 < 0 ? 0x80 : 0;
|
2019-08-27 10:39:53 -07:00
|
|
|
} else {
|
2021-07-23 16:38:23 +02:00
|
|
|
b64[0] = val1 < 0 ? 0x80 : 0;
|
2019-08-27 10:39:53 -07:00
|
|
|
}
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* exponent: bits 62-52 */
|
|
|
|
b64[0] |= (e >> 4);
|
|
|
|
b64[1] = ((e & 0xF) << 4);
|
|
|
|
|
|
|
|
/* fraction: bits 51-0 */
|
|
|
|
/* NOTE: ignore the "hidden" bit 52 in fraction */
|
|
|
|
b64[1] |= ((f >> 48) & 0xF);
|
|
|
|
b64[2] = (f >> 40) & 0xFF;
|
|
|
|
b64[3] = (f >> 32) & 0xFF;
|
|
|
|
b64[4] = (f >> 24) & 0xFF;
|
|
|
|
b64[5] = (f >> 16) & 0xFF;
|
|
|
|
b64[6] = (f >> 8) & 0xFF;
|
|
|
|
b64[7] = f & 0xFF;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
/* convert from binary32 to float */
|
|
|
|
int lwm2m_b32_to_float(uint8_t *b32, size_t len, double *out)
|
2019-02-08 10:00:00 -08:00
|
|
|
{
|
2020-05-27 11:26:57 -05:00
|
|
|
int32_t f, k, i, e;
|
2019-02-08 10:00:00 -08:00
|
|
|
bool sign = false;
|
2021-07-23 16:38:23 +02:00
|
|
|
int32_t val1, val2;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
if (len != 4) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val1 = 0;
|
|
|
|
val2 = 0;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* calc sign: bit 31 */
|
|
|
|
sign = SHIFT_RIGHT(b32[0], 7, 0x1);
|
|
|
|
|
|
|
|
/* calc exponent: bits 30-23 */
|
|
|
|
e = SHIFT_LEFT(b32[0], 1, 0xFF);
|
|
|
|
e += SHIFT_RIGHT(b32[1], 7, 0x1);
|
|
|
|
/* remove bias */
|
|
|
|
e -= 127;
|
|
|
|
|
2021-09-07 13:08:49 +02:00
|
|
|
/* enable "hidden" fraction bit 24 which is always 1 */
|
|
|
|
f = ((int32_t)1 << 23);
|
2019-02-08 10:00:00 -08:00
|
|
|
/* calc fraction: bits 22-0 */
|
2020-05-27 11:26:57 -05:00
|
|
|
f += ((int32_t)(b32[1] & 0x7F) << 16);
|
|
|
|
f += ((int32_t)b32[2] << 8);
|
2019-02-08 10:00:00 -08:00
|
|
|
f += b32[3];
|
|
|
|
|
|
|
|
/* handle whole number */
|
|
|
|
if (e > -1) {
|
|
|
|
/* precision overflow */
|
|
|
|
if (e > 23) {
|
|
|
|
e = 23;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val1 = (f >> (23 - e)) * (sign ? -1 : 1);
|
2019-02-08 10:00:00 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* calculate the rest of the decimal */
|
2021-07-23 16:38:23 +02:00
|
|
|
k = PRECISION32;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* account for -e */
|
|
|
|
while (e < -1) {
|
|
|
|
k /= 2;
|
|
|
|
e++;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (i = 22 - e; i >= 0; i--) {
|
|
|
|
k /= 2;
|
|
|
|
if (f & (1 << i)) {
|
2021-07-23 16:38:23 +02:00
|
|
|
val2 += k;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
if (sign) {
|
|
|
|
*out = (double)val1 - (double)val2 / PRECISION32;
|
|
|
|
} else {
|
|
|
|
*out = (double)val1 + (double)val2 / PRECISION32;
|
|
|
|
}
|
|
|
|
|
2019-02-08 10:00:00 -08:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
/* convert from binary64 to float */
|
|
|
|
int lwm2m_b64_to_float(uint8_t *b64, size_t len, double *out)
|
2019-02-08 10:00:00 -08:00
|
|
|
{
|
2020-05-27 11:26:57 -05:00
|
|
|
int64_t f, k;
|
2019-02-08 10:00:00 -08:00
|
|
|
int i, e;
|
|
|
|
bool sign = false;
|
2021-07-23 16:38:23 +02:00
|
|
|
int64_t val1, val2;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
if (len != 8) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val1 = 0LL;
|
|
|
|
val2 = 0LL;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* calc sign: bit 63 */
|
|
|
|
sign = SHIFT_RIGHT(b64[0], 7, 0x1);
|
|
|
|
|
|
|
|
/* get exponent: bits 62-52 */
|
2020-05-27 11:26:57 -05:00
|
|
|
e = SHIFT_LEFT((uint16_t)b64[0], 4, 0x7F0);
|
2019-02-08 10:00:00 -08:00
|
|
|
e += SHIFT_RIGHT(b64[1], 4, 0xF);
|
|
|
|
/* remove bias */
|
|
|
|
e -= 1023;
|
|
|
|
|
|
|
|
/* enable "hidden" fraction bit 53 which is always 1 */
|
2020-05-27 11:26:57 -05:00
|
|
|
f = ((int64_t)1 << 52);
|
2019-02-08 10:00:00 -08:00
|
|
|
/* get fraction: bits 51-0 */
|
2020-05-27 11:26:57 -05:00
|
|
|
f += ((int64_t)(b64[1] & 0xF) << 48);
|
|
|
|
f += ((int64_t)b64[2] << 40);
|
|
|
|
f += ((int64_t)b64[3] << 32);
|
|
|
|
f += ((int64_t)b64[4] << 24);
|
|
|
|
f += ((int64_t)b64[5] << 16);
|
|
|
|
f += ((int64_t)b64[6] << 8);
|
2019-02-08 10:00:00 -08:00
|
|
|
f += b64[7];
|
|
|
|
|
|
|
|
/* handle whole number */
|
|
|
|
if (e > -1) {
|
|
|
|
/* precision overflow */
|
|
|
|
if (e > 52) {
|
|
|
|
e = 52;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val1 = (f >> (52 - e)) * (sign ? -1 : 1);
|
2019-02-08 10:00:00 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* calculate the rest of the decimal */
|
2021-07-23 16:38:23 +02:00
|
|
|
k = PRECISION64;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
/* account for -e */
|
|
|
|
while (e < -1) {
|
|
|
|
k /= 2;
|
|
|
|
e++;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (i = 51 - e; i >= 0; i--) {
|
|
|
|
k /= 2;
|
2020-05-27 11:26:57 -05:00
|
|
|
if (f & ((int64_t)1 << i)) {
|
2021-07-23 16:38:23 +02:00
|
|
|
val2 += k;
|
2019-02-08 10:00:00 -08:00
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
if (sign) {
|
|
|
|
*out = (double)val1 - (double)val2 / PRECISION64;
|
|
|
|
} else {
|
|
|
|
*out = (double)val1 + (double)val2 / PRECISION64;
|
|
|
|
}
|
|
|
|
|
2019-02-08 10:00:00 -08:00
|
|
|
return 0;
|
|
|
|
}
|
2021-07-21 13:46:05 +02:00
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
int lwm2m_atof(const char *input, double *out)
|
2021-07-21 13:46:05 +02:00
|
|
|
{
|
|
|
|
char *pos, *end, buf[24];
|
|
|
|
long val;
|
2021-07-23 16:38:23 +02:00
|
|
|
int64_t base = PRECISION64, sign = 1;
|
|
|
|
int64_t val1, val2;
|
2021-07-21 13:46:05 +02:00
|
|
|
|
|
|
|
if (!input || !out) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
|
|
|
strncpy(buf, input, sizeof(buf) - 1);
|
|
|
|
buf[sizeof(buf) - 1] = '\0';
|
|
|
|
|
|
|
|
if (strchr(buf, '-')) {
|
|
|
|
sign = -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
pos = strchr(buf, '.');
|
|
|
|
if (pos) {
|
|
|
|
*pos = '\0';
|
|
|
|
}
|
|
|
|
|
|
|
|
errno = 0;
|
|
|
|
val = strtol(buf, &end, 10);
|
|
|
|
if (errno || *end || val < INT_MIN) {
|
|
|
|
return -EINVAL;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val1 = (int64_t)val;
|
|
|
|
val2 = 0;
|
2021-07-21 13:46:05 +02:00
|
|
|
|
|
|
|
if (!pos) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
while (*(++pos) && base > 1 && isdigit((unsigned char)*pos)) {
|
2021-07-23 16:38:23 +02:00
|
|
|
val2 = val2 * 10 + (*pos - '0');
|
2021-07-21 13:46:05 +02:00
|
|
|
base /= 10;
|
|
|
|
}
|
|
|
|
|
2021-07-23 16:38:23 +02:00
|
|
|
val2 *= sign * base;
|
|
|
|
|
|
|
|
*out = (double)val1 + (double)val2 / PRECISION64;
|
|
|
|
|
2021-07-21 13:46:05 +02:00
|
|
|
return !*pos || base == 1 ? 0 : -EINVAL;
|
|
|
|
}
|
2021-07-23 16:38:23 +02:00
|
|
|
|
|
|
|
int lwm2m_ftoa(double *input, char *out, size_t outlen, int8_t dec_limit)
|
|
|
|
{
|
|
|
|
size_t len;
|
|
|
|
char buf[PRECISION64_LEN + 1];
|
|
|
|
int64_t val1 = (int64_t)*input;
|
|
|
|
int64_t val2 = (*input - (int64_t)*input) * PRECISION64;
|
|
|
|
|
|
|
|
len = snprintk(buf, sizeof(buf), "%0*lld", PRECISION64_LEN, llabs(val2));
|
|
|
|
if (len != PRECISION64_LEN) {
|
|
|
|
strcpy(buf, "0");
|
|
|
|
} else {
|
|
|
|
/* Round the value to the specified decimal point. */
|
|
|
|
if (dec_limit > 0 && dec_limit < sizeof(buf) &&
|
|
|
|
buf[dec_limit] != '\0') {
|
|
|
|
bool round_up = buf[dec_limit] >= '5';
|
|
|
|
|
|
|
|
buf[dec_limit] = '\0';
|
|
|
|
len = dec_limit;
|
|
|
|
|
|
|
|
while (round_up && dec_limit > 0) {
|
|
|
|
dec_limit--;
|
|
|
|
buf[dec_limit]++;
|
|
|
|
|
|
|
|
if (buf[dec_limit] > '9') {
|
|
|
|
buf[dec_limit] = '0';
|
|
|
|
} else {
|
|
|
|
round_up = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (round_up) {
|
|
|
|
if (*input < 0) {
|
|
|
|
val1--;
|
|
|
|
} else {
|
|
|
|
val1++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/* clear ending zeroes, but leave 1 if needed */
|
|
|
|
while (len > 1U && buf[len - 1] == '0') {
|
|
|
|
buf[--len] = '\0';
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return snprintk(out, outlen, "%s%lld.%s",
|
|
|
|
/* handle negative val2 when val1 is 0 */
|
|
|
|
(val1 == 0 && val2 < 0) ? "-" : "", val1, buf);
|
|
|
|
}
|