net: lwm2m: fix float32/64 handling
During the initial work on LwM2M, the float32/64 code was basically stubbed out. Float32 sent only whole values and float64 was completely broken. Let's clean up the OMA TLV formatting code by moving the float processing code into a separate file: lwm2m_util.c. Then using public definitions for binary32 and binary64, let's fix the processing code to correctly fill the float32_value_t and float64_value_t types. Signed-off-by: Michael Scott <mike@foundries.io>
This commit is contained in:
Michael Scott
Anas Nashif
parent
90d1e6f6c6
commit
d53a0855a1
|
|
@ -149,11 +149,14 @@ lwm2m_engine_user_cb_t lwm2m_firmware_get_update_cb(void);
|
|||
* Example: 123.456 == val1: 123, val2:456000
|
||||
* Example: 123.000456 = val1: 123, val2:456
|
||||
*/
|
||||
|
||||
#define LWM2M_FLOAT32_DEC_MAX 1000000
|
||||
typedef struct float32_value {
|
||||
s32_t val1;
|
||||
s32_t val2;
|
||||
} float32_value_t;
|
||||
|
||||
#define LWM2M_FLOAT64_DEC_MAX 1000000000LL
|
||||
typedef struct float64_value {
|
||||
s64_t val1;
|
||||
s64_t val2;
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ zephyr_library_sources(
|
|||
lwm2m_obj_device.c
|
||||
lwm2m_rw_plain_text.c
|
||||
lwm2m_rw_oma_tlv.c
|
||||
lwm2m_util.c
|
||||
)
|
||||
|
||||
# LWM2M RD Client Support
|
||||
|
|
|
|||
|
|
@ -72,6 +72,7 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME);
|
|||
#ifdef CONFIG_LWM2M_RD_CLIENT_SUPPORT
|
||||
#include "lwm2m_rd_client.h"
|
||||
#endif
|
||||
#include "lwm2m_util.h"
|
||||
|
||||
enum {
|
||||
OMA_TLV_TYPE_OBJECT_INSTANCE = 0,
|
||||
|
|
@ -496,7 +497,6 @@ static size_t put_string(struct lwm2m_output_context *out,
|
|||
return len;
|
||||
}
|
||||
|
||||
/* use binary32 format */
|
||||
static size_t put_float32fix(struct lwm2m_output_context *out,
|
||||
struct lwm2m_obj_path *path,
|
||||
float32_value_t *value)
|
||||
|
|
@ -504,57 +504,23 @@ static size_t put_float32fix(struct lwm2m_output_context *out,
|
|||
struct tlv_out_formatter_data *fd;
|
||||
size_t len;
|
||||
struct oma_tlv tlv;
|
||||
int e = 0;
|
||||
s32_t val = 0;
|
||||
s32_t v;
|
||||
u8_t b[4];
|
||||
|
||||
/*
|
||||
* TODO: Currently, there is no standard API for handling the decimal
|
||||
* portion of the float32_value structure. In the future, we should use
|
||||
* the value->val2 (decimal portion) to set the decimal mask and in the
|
||||
* following binary float calculations.
|
||||
*
|
||||
* HACK BELOW: hard code the decimal mask to 0 (whole number)
|
||||
*/
|
||||
int bits = 0;
|
||||
int ret;
|
||||
u8_t b32[4];
|
||||
|
||||
fd = engine_get_out_user_data(out);
|
||||
if (!fd) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
v = value->val1;
|
||||
if (v < 0) {
|
||||
v = -v;
|
||||
ret = lwm2m_f32_to_b32(value, b32, sizeof(b32));
|
||||
if (ret < 0) {
|
||||
LOG_ERR("float32 conversion error: %d", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
while (v > 1) {
|
||||
val = (val >> 1);
|
||||
|
||||
if (v & 1) {
|
||||
val = val | (1L << 22);
|
||||
}
|
||||
|
||||
v = (v >> 1);
|
||||
e++;
|
||||
}
|
||||
|
||||
/* convert to the thing we should have */
|
||||
e = e - bits + 127;
|
||||
if (value->val1 == 0) {
|
||||
e = 0;
|
||||
}
|
||||
|
||||
/* is this the right byte order? */
|
||||
b[0] = (value->val1 < 0 ? 0x80 : 0) | (e >> 1);
|
||||
b[1] = ((e & 1) << 7) | ((val >> 16) & 0x7f);
|
||||
b[2] = (val >> 8) & 0xff;
|
||||
b[3] = val & 0xff;
|
||||
|
||||
tlv_setup(&tlv, tlv_calc_type(fd->writer_flags),
|
||||
tlv_calc_id(fd->writer_flags, path), 4);
|
||||
len = oma_tlv_put(&tlv, out, b, false);
|
||||
tlv_calc_id(fd->writer_flags, path), sizeof(b32));
|
||||
len = oma_tlv_put(&tlv, out, b32, false);
|
||||
return len;
|
||||
}
|
||||
|
||||
|
|
@ -564,21 +530,24 @@ static size_t put_float64fix(struct lwm2m_output_context *out,
|
|||
{
|
||||
struct tlv_out_formatter_data *fd;
|
||||
size_t len;
|
||||
s64_t binary64 = 0, net_binary64 = 0;
|
||||
struct oma_tlv tlv;
|
||||
|
||||
/* TODO */
|
||||
u8_t b64[8];
|
||||
int ret;
|
||||
|
||||
fd = engine_get_out_user_data(out);
|
||||
if (!fd) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
net_binary64 = sys_cpu_to_be64(binary64);
|
||||
ret = lwm2m_f64_to_b64(value, b64, sizeof(b64));
|
||||
if (ret < 0) {
|
||||
LOG_ERR("float64 conversion error: %d", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
tlv_setup(&tlv, tlv_calc_type(fd->writer_flags),
|
||||
tlv_calc_id(fd->writer_flags, path),
|
||||
sizeof(net_binary64));
|
||||
len = oma_tlv_put(&tlv, out, (u8_t *)&net_binary64, false);
|
||||
tlv_calc_id(fd->writer_flags, path), sizeof(b64));
|
||||
len = oma_tlv_put(&tlv, out, b64, false);
|
||||
return len;
|
||||
}
|
||||
|
||||
|
|
@ -679,51 +648,37 @@ static size_t get_float32fix(struct lwm2m_input_context *in,
|
|||
{
|
||||
struct oma_tlv tlv;
|
||||
size_t size = oma_tlv_get(&tlv, in, false);
|
||||
int e;
|
||||
s32_t val;
|
||||
int sign = false;
|
||||
int bits = 0;
|
||||
u8_t values[4];
|
||||
u8_t b32[4];
|
||||
int ret;
|
||||
|
||||
if (size > 0) {
|
||||
/* TLV needs to be 4 bytes */
|
||||
if (buf_read(values, 4, CPKT_BUF_READ(in->in_cpkt),
|
||||
if (tlv.length != 4) {
|
||||
LOG_ERR("Invalid float32 length: %d", tlv.length);
|
||||
|
||||
/* dummy read */
|
||||
while (tlv.length--) {
|
||||
if (buf_read_u8(b32,
|
||||
CPKT_BUF_READ(in->in_cpkt),
|
||||
&in->offset) < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* read b32 in network byte order */
|
||||
if (buf_read(b32, tlv.length, CPKT_BUF_READ(in->in_cpkt),
|
||||
&in->offset) < 0) {
|
||||
/* TODO: Generate error? */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* sign */
|
||||
sign = (values[0] & 0x80) != 0;
|
||||
|
||||
e = ((values[0] << 1) & 0xff) | (values[1] >> 7);
|
||||
val = (((long)values[1] & 0x7f) << 16) |
|
||||
(values[2] << 8) |
|
||||
values[3];
|
||||
e = e - 127 + bits;
|
||||
|
||||
/* e is the number of times we need to roll the number */
|
||||
LOG_DBG("Actual e=%d", e);
|
||||
e = e - 23;
|
||||
LOG_DBG("E after sub %d", e);
|
||||
val = val | 1L << 23;
|
||||
if (e > 0) {
|
||||
val = val << e;
|
||||
} else {
|
||||
val = val >> -e;
|
||||
ret = lwm2m_b32_to_f32(b32, sizeof(b32), value);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("binary32 conversion error: %d", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
value->val1 = sign ? -val : val;
|
||||
|
||||
/*
|
||||
* TODO: Currently, there is no standard API for handling the
|
||||
* decimal portion of the float32_value structure. In the
|
||||
* future, once that is settled, we should calculate
|
||||
* value->val2 in the above float calculations.
|
||||
*
|
||||
* HACK BELOW: hard code the decimal value 0
|
||||
*/
|
||||
value->val2 = 0;
|
||||
}
|
||||
|
||||
return size;
|
||||
|
|
@ -734,14 +689,37 @@ static size_t get_float64fix(struct lwm2m_input_context *in,
|
|||
{
|
||||
struct oma_tlv tlv;
|
||||
size_t size = oma_tlv_get(&tlv, in, false);
|
||||
u8_t b64[8];
|
||||
int ret;
|
||||
|
||||
if (size > 0) {
|
||||
if (tlv.length != 8) {
|
||||
LOG_ERR("invalid length: %u (not 8)", tlv.length);
|
||||
LOG_ERR("invalid float64 length: %d", tlv.length);
|
||||
|
||||
/* dummy read */
|
||||
while (tlv.length--) {
|
||||
if (buf_read_u8(b64,
|
||||
CPKT_BUF_READ(in->in_cpkt),
|
||||
&in->offset) < 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* TODO */
|
||||
/* read b64 in network byte order */
|
||||
if (buf_read(b64, tlv.length, CPKT_BUF_READ(in->in_cpkt),
|
||||
&in->offset) < 0) {
|
||||
/* TODO: Generate error? */
|
||||
return 0;
|
||||
}
|
||||
|
||||
ret = lwm2m_b64_to_f64(b64, sizeof(b64), value);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("binary64 conversion error: %d", ret);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return size;
|
||||
|
|
|
|||
289
subsys/net/lib/lwm2m/lwm2m_util.c
Normal file
289
subsys/net/lib/lwm2m/lwm2m_util.c
Normal file
|
|
@ -0,0 +1,289 @@
|
|||
/*
|
||||
* Copyright (c) 2019 Foundries.io
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <kernel.h>
|
||||
#include "lwm2m_util.h"
|
||||
|
||||
#define SHIFT_LEFT(v, o, m) (((v) << (o)) & (m))
|
||||
#define SHIFT_RIGHT(v, o, m) (((v) >> (o)) & (m))
|
||||
|
||||
/* convert from float32 to binary32 */
|
||||
int lwm2m_f32_to_b32(float32_value_t *f32, u8_t *b32, size_t len)
|
||||
{
|
||||
s32_t e = -1, v, f = 0;
|
||||
int i;
|
||||
|
||||
if (len != 4) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
v = f32->val1;
|
||||
/* sign handled later */
|
||||
if (v < 0) {
|
||||
v = -v;
|
||||
}
|
||||
|
||||
/* add whole value to fraction */
|
||||
while (v > 0) {
|
||||
f >>= 1;
|
||||
|
||||
if (v & 1) {
|
||||
f |= (1 << 23);
|
||||
}
|
||||
|
||||
v >>= 1;
|
||||
e++;
|
||||
}
|
||||
|
||||
v = f32->val2;
|
||||
/* sign handled later */
|
||||
if (v < 0) {
|
||||
v = -v;
|
||||
}
|
||||
|
||||
/* add decimal to fraction */
|
||||
i = e;
|
||||
while (v > 0 && i < 23) {
|
||||
v *= 2;
|
||||
if (e < 0 && v < LWM2M_FLOAT32_DEC_MAX) {
|
||||
/* handle -e */
|
||||
e--;
|
||||
continue;
|
||||
} else if (v >= LWM2M_FLOAT32_DEC_MAX) {
|
||||
v -= LWM2M_FLOAT32_DEC_MAX;
|
||||
f |= 1 << (22 - i);
|
||||
}
|
||||
|
||||
if (v == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
/* adjust exponent for bias */
|
||||
e += 127;
|
||||
|
||||
memset(b32, 0, len);
|
||||
|
||||
/* sign: bit 31 */
|
||||
b32[0] = f32->val1 < 0 ? 0x80 : 0;
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* convert from float64 to binary64 */
|
||||
int lwm2m_f64_to_b64(float64_value_t *f64, u8_t *b64, size_t len)
|
||||
{
|
||||
s64_t v, f = 0;
|
||||
s32_t e = -1;
|
||||
int i;
|
||||
|
||||
if (len != 8) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
v = f64->val1;
|
||||
/* sign handled later */
|
||||
if (v < 0) {
|
||||
v = -v;
|
||||
}
|
||||
|
||||
/* add whole value to fraction */
|
||||
while (v > 0) {
|
||||
f >>= 1;
|
||||
|
||||
if (v & 1) {
|
||||
f |= ((s64_t)1 << 52);
|
||||
}
|
||||
|
||||
v >>= 1;
|
||||
e++;
|
||||
}
|
||||
|
||||
v = f64->val2;
|
||||
/* sign handled later */
|
||||
if (v < 0) {
|
||||
v = -v;
|
||||
}
|
||||
|
||||
/* add decimal to fraction */
|
||||
i = e;
|
||||
while (v > 0 && i < 52) {
|
||||
v *= 2;
|
||||
if (e < 0 && v < LWM2M_FLOAT64_DEC_MAX) {
|
||||
/* handle -e */
|
||||
e--;
|
||||
continue;
|
||||
} else if (v >= LWM2M_FLOAT64_DEC_MAX) {
|
||||
v -= LWM2M_FLOAT64_DEC_MAX;
|
||||
f |= (s64_t)1 << (51 - i);
|
||||
}
|
||||
|
||||
if (v == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
/* adjust exponent for bias */
|
||||
e += 1023;
|
||||
|
||||
memset(b64, 0, len);
|
||||
|
||||
/* sign: bit 63 */
|
||||
b64[0] = f64->val1 < 0 ? 0x80 : 0;
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* convert from binary32 to float32 */
|
||||
int lwm2m_b32_to_f32(u8_t *b32, size_t len, float32_value_t *f32)
|
||||
{
|
||||
s32_t f, k, i, e;
|
||||
bool sign = false;
|
||||
|
||||
if (len != 4) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
f32->val1 = 0;
|
||||
f32->val2 = 0;
|
||||
|
||||
/* 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;
|
||||
|
||||
/* enable "hidden" fraction bit 23 which is always 1 */
|
||||
f = ((s32_t)1 << 22);
|
||||
/* calc fraction: bits 22-0 */
|
||||
f += ((s32_t)(b32[1] & 0x7F) << 16);
|
||||
f += ((s32_t)b32[2] << 8);
|
||||
f += b32[3];
|
||||
|
||||
/* handle whole number */
|
||||
if (e > -1) {
|
||||
/* precision overflow */
|
||||
if (e > 23) {
|
||||
e = 23;
|
||||
}
|
||||
|
||||
f32->val1 = (f >> (23 - e)) * (sign ? -1 : 1);
|
||||
}
|
||||
|
||||
/* calculate the rest of the decimal */
|
||||
k = LWM2M_FLOAT32_DEC_MAX;
|
||||
|
||||
/* account for -e */
|
||||
while (e < -1) {
|
||||
k /= 2;
|
||||
e++;
|
||||
}
|
||||
|
||||
for (i = 22 - e; i >= 0; i--) {
|
||||
k /= 2;
|
||||
if (f & (1 << i)) {
|
||||
f32->val2 += k;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* convert from binary64 to float64 */
|
||||
int lwm2m_b64_to_f64(u8_t *b64, size_t len, float64_value_t *f64)
|
||||
{
|
||||
s64_t f, k;
|
||||
int i, e;
|
||||
bool sign = false;
|
||||
|
||||
if (len != 8) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
f64->val1 = 0LL;
|
||||
f64->val2 = 0LL;
|
||||
|
||||
/* calc sign: bit 63 */
|
||||
sign = SHIFT_RIGHT(b64[0], 7, 0x1);
|
||||
|
||||
/* get exponent: bits 62-52 */
|
||||
e = SHIFT_LEFT((u16_t)b64[0], 4, 0x7F0);
|
||||
e += SHIFT_RIGHT(b64[1], 4, 0xF);
|
||||
/* remove bias */
|
||||
e -= 1023;
|
||||
|
||||
/* enable "hidden" fraction bit 53 which is always 1 */
|
||||
f = ((s64_t)1 << 52);
|
||||
/* get fraction: bits 51-0 */
|
||||
f += ((s64_t)(b64[1] & 0xF) << 48);
|
||||
f += ((s64_t)b64[2] << 40);
|
||||
f += ((s64_t)b64[3] << 32);
|
||||
f += ((s64_t)b64[4] << 24);
|
||||
f += ((s64_t)b64[5] << 16);
|
||||
f += ((s64_t)b64[6] << 8);
|
||||
f += b64[7];
|
||||
|
||||
/* handle whole number */
|
||||
if (e > -1) {
|
||||
/* precision overflow */
|
||||
if (e > 52) {
|
||||
e = 52;
|
||||
}
|
||||
|
||||
f64->val1 = (f >> (52 - e)) * (sign ? -1 : 1);
|
||||
}
|
||||
|
||||
/* calculate the rest of the decimal */
|
||||
k = LWM2M_FLOAT64_DEC_MAX;
|
||||
|
||||
/* account for -e */
|
||||
while (e < -1) {
|
||||
k /= 2;
|
||||
e++;
|
||||
}
|
||||
|
||||
for (i = 51 - e; i >= 0; i--) {
|
||||
k /= 2;
|
||||
if (f & ((s64_t)1 << i)) {
|
||||
f64->val2 += k;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
20
subsys/net/lib/lwm2m/lwm2m_util.h
Normal file
20
subsys/net/lib/lwm2m/lwm2m_util.h
Normal file
|
|
@ -0,0 +1,20 @@
|
|||
/*
|
||||
* Copyright (c) 2018-2019 Foundries.io
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef LWM2M_UTIL_H_
|
||||
#define LWM2M_UTIL_H_
|
||||
|
||||
#include <net/lwm2m.h>
|
||||
|
||||
/* convert float struct to binary format */
|
||||
int lwm2m_f32_to_b32(float32_value_t *f32, u8_t *b32, size_t len);
|
||||
int lwm2m_f64_to_b64(float64_value_t *f64, u8_t *b64, size_t len);
|
||||
|
||||
/* convert binary format to float struct */
|
||||
int lwm2m_b32_to_f32(u8_t *b32, size_t len, float32_value_t *f32);
|
||||
int lwm2m_b64_to_f64(u8_t *b64, size_t len, float64_value_t *f64);
|
||||
|
||||
#endif /* LWM2M_UTIL_H_ */
|
||||
Loading…
Reference in a new issue