From 20a4d181e1485e6160c7443b6f81e86aa64e4aeb Mon Sep 17 00:00:00 2001 From: Robert Lubos Date: Wed, 21 Jul 2021 13:46:05 +0200 Subject: [PATCH] net: lwm2m: Fix plain text floating point handling Floating point parser for plain text format was parsing floats wrongly, ignoring leading zeros after decimal points. Fix this, by reusing atof32() function, already avaialbe in a different part of the engine, which did the parsing correctly. Signed-off-by: Robert Lubos --- subsys/net/lib/lwm2m/lwm2m_engine.c | 47 +------------- subsys/net/lib/lwm2m/lwm2m_rw_plain_text.c | 71 ++++++++++++++-------- subsys/net/lib/lwm2m/lwm2m_util.c | 45 ++++++++++++++ subsys/net/lib/lwm2m/lwm2m_util.h | 3 + 4 files changed, 95 insertions(+), 71 deletions(-) diff --git a/subsys/net/lib/lwm2m/lwm2m_engine.c b/subsys/net/lib/lwm2m/lwm2m_engine.c index 3de2aca8a77..80e9e29373c 100644 --- a/subsys/net/lib/lwm2m/lwm2m_engine.c +++ b/subsys/net/lib/lwm2m/lwm2m_engine.c @@ -43,6 +43,7 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME); #include "lwm2m_rw_link_format.h" #include "lwm2m_rw_plain_text.h" #include "lwm2m_rw_oma_tlv.h" +#include "lwm2m_util.h" #ifdef CONFIG_LWM2M_RW_JSON_SUPPORT #include "lwm2m_rw_json.h" #endif @@ -829,50 +830,6 @@ static uint16_t atou16(uint8_t *buf, uint16_t buflen, uint16_t *len) return val; } -static int atof32(const char *input, float32_value_t *out) -{ - char *pos, *end, buf[24]; - long int val; - int32_t base = 1000000, sign = 1; - - 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; - } - - out->val1 = (int32_t) val; - out->val2 = 0; - - if (!pos) { - return 0; - } - - while (*(++pos) && base > 1 && isdigit((unsigned char)*pos)) { - out->val2 = out->val2 * 10 + (*pos - '0'); - base /= 10; - } - - out->val2 *= sign * base; - return !*pos || base == 1 ? 0 : -EINVAL; -} - static int coap_options_to_path(struct coap_option *opt, int options_count, struct lwm2m_obj_path *path) { @@ -2831,7 +2788,7 @@ static int lwm2m_write_attr_handler(struct lwm2m_engine_obj *obj, val.val1 = v; } else { /* gt/lt/st: type float */ - ret = atof32(opt_buf, &val); + ret = lwm2m_atof32(opt_buf, &val); } if (ret < 0) { diff --git a/subsys/net/lib/lwm2m/lwm2m_rw_plain_text.c b/subsys/net/lib/lwm2m/lwm2m_rw_plain_text.c index af19e16c3af..b27ce429755 100644 --- a/subsys/net/lib/lwm2m/lwm2m_rw_plain_text.c +++ b/subsys/net/lib/lwm2m/lwm2m_rw_plain_text.c @@ -71,6 +71,7 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME); #include "lwm2m_object.h" #include "lwm2m_rw_plain_text.h" #include "lwm2m_engine.h" +#include "lwm2m_util.h" /* some temporary buffer space for format conversions */ static char pt_buffer[42]; /* can handle float64 format */ @@ -176,22 +177,15 @@ static size_t put_objlnk(struct lwm2m_output_context *out, value->obj_inst); } -static size_t plain_text_read_number(struct lwm2m_input_context *in, - int64_t *value1, - int64_t *value2, - bool accept_sign, bool accept_dot) +static size_t plain_text_read_int(struct lwm2m_input_context *in, + int64_t *value, bool accept_sign) { - int64_t *counter = value1; int i = 0; bool neg = false; - bool dot_found = false; uint8_t tmp; /* initialize values to 0 */ - *value1 = 0; - if (value2) { - *value2 = 0; - } + *value = 0; while (in->offset < in->in_cpkt->offset) { if (buf_read_u8(&tmp, CPKT_BUF_READ(in->in_cpkt), @@ -201,12 +195,8 @@ static size_t plain_text_read_number(struct lwm2m_input_context *in, if (tmp == '-' && accept_sign && i == 0) { neg = true; - } else if (tmp == '.' && i > 0 && accept_dot && !dot_found && - value2) { - dot_found = true; - counter = value2; } else if (isdigit(tmp)) { - *counter = *counter * 10 + (tmp - '0'); + *value = *value * 10 + (tmp - '0'); } else { /* anything else stop reading */ in->offset--; @@ -217,7 +207,7 @@ static size_t plain_text_read_number(struct lwm2m_input_context *in, } if (neg) { - *value1 = -*value1; + *value = -*value; } return i; @@ -228,7 +218,7 @@ static size_t get_s32(struct lwm2m_input_context *in, int32_t *value) int64_t tmp = 0; size_t len = 0; - len = plain_text_read_number(in, &tmp, NULL, true, false); + len = plain_text_read_int(in, &tmp, true); if (len > 0) { *value = (int32_t)tmp; } @@ -238,7 +228,7 @@ static size_t get_s32(struct lwm2m_input_context *in, int32_t *value) static size_t get_s64(struct lwm2m_input_context *in, int64_t *value) { - return plain_text_read_number(in, value, NULL, true, false); + return plain_text_read_int(in, value, true); } static size_t get_string(struct lwm2m_input_context *in, @@ -266,13 +256,42 @@ static size_t get_string(struct lwm2m_input_context *in, static size_t get_float32fix(struct lwm2m_input_context *in, float32_value_t *value) { - int64_t tmp1, tmp2; - size_t len = 0; + size_t i = 0, len = 0; + bool has_dot = false; + uint8_t tmp, buf[24]; - len = plain_text_read_number(in, &tmp1, &tmp2, true, true); - if (len > 0) { - value->val1 = (int32_t)tmp1; - value->val2 = (int32_t)tmp2; + + while (in->offset < in->in_cpkt->offset) { + if (buf_read_u8(&tmp, CPKT_BUF_READ(in->in_cpkt), + &in->offset) < 0) { + break; + } + + if ((tmp == '-' && i == 0) || (tmp == '.' && !has_dot) || + isdigit(tmp)) { + len++; + + /* Copy only if it fits into provided buffer - we won't + * get better precision anyway. + */ + if (i < sizeof(buf) - 1) { + buf[i++] = tmp; + } + + if (tmp == '.') { + has_dot = true; + } + } else { + /* anything else stop reading */ + in->offset--; + break; + } + } + + buf[i] = '\0'; + + if (lwm2m_atof32(buf, value) != 0) { + LOG_ERR("Failed to parse float value"); } return len; @@ -342,14 +361,14 @@ static size_t get_objlnk(struct lwm2m_input_context *in, int64_t tmp; size_t len; - len = plain_text_read_number(in, &tmp, NULL, false, false); + len = plain_text_read_int(in, &tmp, false); value->obj_id = (uint16_t)tmp; /* Skip ':' delimeter. */ in->offset++; len++; - len += plain_text_read_number(in, &tmp, NULL, false, false); + len += plain_text_read_int(in, &tmp, false); value->obj_inst = (uint16_t)tmp; return len; diff --git a/subsys/net/lib/lwm2m/lwm2m_util.c b/subsys/net/lib/lwm2m/lwm2m_util.c index a5be1f5448b..71ffcc4abb8 100644 --- a/subsys/net/lib/lwm2m/lwm2m_util.c +++ b/subsys/net/lib/lwm2m/lwm2m_util.c @@ -6,6 +6,7 @@ #include #include +#include #include "lwm2m_util.h" #define SHIFT_LEFT(v, o, m) (((v) << (o)) & (m)) @@ -296,3 +297,47 @@ int lwm2m_b64_to_f32(uint8_t *b64, size_t len, float32_value_t *f32) return 0; } + +int lwm2m_atof32(const char *input, float32_value_t *out) +{ + char *pos, *end, buf[24]; + long val; + int32_t base = LWM2M_FLOAT32_DEC_MAX, sign = 1; + + 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; + } + + out->val1 = (int32_t) val; + out->val2 = 0; + + if (!pos) { + return 0; + } + + while (*(++pos) && base > 1 && isdigit((unsigned char)*pos)) { + out->val2 = out->val2 * 10 + (*pos - '0'); + base /= 10; + } + + out->val2 *= sign * base; + return !*pos || base == 1 ? 0 : -EINVAL; +} diff --git a/subsys/net/lib/lwm2m/lwm2m_util.h b/subsys/net/lib/lwm2m/lwm2m_util.h index 068597b7f40..216d83de5f2 100644 --- a/subsys/net/lib/lwm2m/lwm2m_util.h +++ b/subsys/net/lib/lwm2m/lwm2m_util.h @@ -17,4 +17,7 @@ int lwm2m_f32_to_b64(float32_value_t *f32, uint8_t *b64, size_t len); int lwm2m_b32_to_f32(uint8_t *b32, size_t len, float32_value_t *f32); int lwm2m_b64_to_f32(uint8_t *b64, size_t len, float32_value_t *f32); +/* convert string to float struct */ +int lwm2m_atof32(const char *input, float32_value_t *out); + #endif /* LWM2M_UTIL_H_ */