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 <robert.lubos@nordicsemi.no>

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) {

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,
+static size_t plain_text_read_int(struct lwm2m_input_context *in,
-				     int64_t *value1,
+				  int64_t *value, bool accept_sign)
-				     int64_t *value2,
-				     bool accept_sign, bool accept_dot)
 {
-	int64_t *counter = value1;
 	int i = 0;
 	bool neg = false;
-	bool dot_found = false;
 	uint8_t tmp;
 
 	/* initialize values to 0 */
-	*value1 = 0;
+	*value = 0;
-	if (value2) {
-		*value2 = 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 i = 0, len = 0;
-	size_t len = 0;
+	bool has_dot = false;
+	uint8_t tmp, buf[24];
 
-	len = plain_text_read_number(in, &tmp1, &tmp2, true, true);
+
-	if (len > 0) {
+	while (in->offset < in->in_cpkt->offset) {
-		value->val1 = (int32_t)tmp1;
+		if (buf_read_u8(&tmp, CPKT_BUF_READ(in->in_cpkt),
-		value->val2 = (int32_t)tmp2;
+				&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;

subsys/net/lib/lwm2m/lwm2m_util.c

@@ -6,6 +6,7 @@
 
 #include <kernel.h>
 #include <stdlib.h>
+#include <ctype.h>
 #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;
+}

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