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_ */