1 /*
2 * Copyright (c) 2019 Foundries.io
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <stdio.h>
9 #include <stdlib.h>
10 #include <ctype.h>
11 #include <inttypes.h>
12 #include "lwm2m_object.h"
13 #include "lwm2m_util.h"
14
15 #define SHIFT_LEFT(v, o, m) (((v) << (o)) & (m))
16 #define SHIFT_RIGHT(v, o, m) (((v) >> (o)) & (m))
17
18 #define PRECISION64_LEN 17U
19 #define PRECISION64 100000000000000000ULL
20
21 #define PRECISION32 1000000000UL
22
23 /* convert from float to binary32 */
lwm2m_float_to_b32(double * in,uint8_t * b32,size_t len)24 int lwm2m_float_to_b32(double *in, uint8_t *b32, size_t len)
25 {
26 int32_t e = -1, v, f = 0;
27 int32_t val1 = (int32_t)*in;
28 int32_t val2 = (*in - (int32_t)*in) * PRECISION32;
29 int i;
30
31 if (len != 4) {
32 return -EINVAL;
33 }
34
35 /* handle zero value special case */
36 if (val1 == 0 && val2 == 0) {
37 memset(b32, 0, len);
38 return 0;
39 }
40
41 /* sign handled later */
42 v = abs(val1);
43
44 /* add whole value to fraction */
45 while (v > 0) {
46 f >>= 1;
47
48 if (v & 1) {
49 f |= (1 << 23);
50 }
51
52 v >>= 1;
53 e++;
54 }
55
56 /* sign handled later */
57 v = abs(val2);
58
59 /* add decimal to fraction */
60 i = e;
61 while (v > 0 && i < 23) {
62 v *= 2;
63 if (!f && e < 0 && v < PRECISION32) {
64 /* handle -e */
65 e--;
66 continue;
67 } else if (v >= PRECISION32) {
68 v -= PRECISION32;
69 f |= 1 << (22 - i);
70 }
71
72 if (v == 0) {
73 break;
74 }
75
76 i++;
77 }
78
79 /* adjust exponent for bias */
80 e += 127;
81
82 memset(b32, 0, len);
83
84 /* sign: bit 31 */
85 if (val1 == 0) {
86 b32[0] = val2 < 0 ? 0x80 : 0;
87 } else {
88 b32[0] = val1 < 0 ? 0x80 : 0;
89 }
90
91 /* exponent: bits 30-23 */
92 b32[0] |= e >> 1;
93 b32[1] = (e & 1) << 7;
94
95 /* fraction: bits 22-0 */
96 /* NOTE: ignore the "hidden" bit 23 in fraction */
97 b32[1] |= (f >> 16) & 0x7F;
98 b32[2] = (f >> 8) & 0xFF;
99 b32[3] = f & 0xFF;
100
101 return 0;
102 }
103
104 /* convert from float to binary64 */
lwm2m_float_to_b64(double * in,uint8_t * b64,size_t len)105 int lwm2m_float_to_b64(double *in, uint8_t *b64, size_t len)
106 {
107 int64_t v, f = 0;
108 int32_t e = -1;
109 int64_t val1 = (int64_t)*in;
110 int64_t val2 = (*in - (int64_t)*in) * PRECISION64;
111 int i;
112
113 if (len != 8) {
114 return -EINVAL;
115 }
116
117 /* handle zero value special case */
118 if (val1 == 0 && val2 == 0) {
119 memset(b64, 0, len);
120 return 0;
121 }
122
123 /* sign handled later */
124 v = llabs(val1);
125
126 /* add whole value to fraction */
127 while (v > 0) {
128 f >>= 1;
129
130 if (v & 1) {
131 f |= ((int64_t)1 << 52);
132 }
133
134 v >>= 1;
135 e++;
136 }
137
138 /* sign handled later */
139 v = llabs(val2);
140
141 /* add decimal to fraction */
142 i = e;
143 while (v > 0 && i < 52) {
144 v *= 2;
145 if (!f && e < 0 && v < PRECISION64) {
146 /* handle -e */
147 e--;
148 continue;
149 } else if (v >= PRECISION64) {
150 v -= PRECISION64;
151 f |= (int64_t)1 << (51 - i);
152 }
153
154 if (v == 0) {
155 break;
156 }
157
158 i++;
159 }
160
161 /* adjust exponent for bias */
162 e += 1023;
163
164 memset(b64, 0, len);
165
166 /* sign: bit 63 */
167 if (val1 == 0) {
168 b64[0] = val2 < 0 ? 0x80 : 0;
169 } else {
170 b64[0] = val1 < 0 ? 0x80 : 0;
171 }
172
173 /* exponent: bits 62-52 */
174 b64[0] |= (e >> 4);
175 b64[1] = ((e & 0xF) << 4);
176
177 /* fraction: bits 51-0 */
178 /* NOTE: ignore the "hidden" bit 52 in fraction */
179 b64[1] |= ((f >> 48) & 0xF);
180 b64[2] = (f >> 40) & 0xFF;
181 b64[3] = (f >> 32) & 0xFF;
182 b64[4] = (f >> 24) & 0xFF;
183 b64[5] = (f >> 16) & 0xFF;
184 b64[6] = (f >> 8) & 0xFF;
185 b64[7] = f & 0xFF;
186
187 return 0;
188 }
189
190 /* convert from binary32 to float */
lwm2m_b32_to_float(uint8_t * b32,size_t len,double * out)191 int lwm2m_b32_to_float(uint8_t *b32, size_t len, double *out)
192 {
193 int32_t f, k, i, e;
194 bool sign = false;
195 int32_t val1, val2;
196
197 if (len != 4) {
198 return -EINVAL;
199 }
200
201 val1 = 0;
202 val2 = 0;
203
204 /* calc sign: bit 31 */
205 sign = SHIFT_RIGHT(b32[0], 7, 0x1);
206
207 /* calc exponent: bits 30-23 */
208 e = SHIFT_LEFT(b32[0], 1, 0xFF);
209 e += SHIFT_RIGHT(b32[1], 7, 0x1);
210 /* remove bias */
211 e -= 127;
212
213 /* enable "hidden" fraction bit 24 which is always 1 */
214 f = ((int32_t)1 << 23);
215 /* calc fraction: bits 22-0 */
216 f += ((int32_t)(b32[1] & 0x7F) << 16);
217 f += ((int32_t)b32[2] << 8);
218 f += b32[3];
219
220 /* handle whole number */
221 if (e > -1) {
222 /* precision overflow */
223 if (e > 23) {
224 e = 23;
225 }
226
227 val1 = (f >> (23 - e)) * (sign ? -1 : 1);
228 }
229
230 /* calculate the rest of the decimal */
231 k = PRECISION32;
232
233 /* account for -e */
234 while (e < -1) {
235 k /= 2;
236 e++;
237 }
238
239 for (i = 22 - e; i >= 0; i--) {
240 k /= 2;
241 if (f & (1 << i)) {
242 val2 += k;
243
244 }
245 }
246
247 if (sign) {
248 *out = (double)val1 - (double)val2 / PRECISION32;
249 } else {
250 *out = (double)val1 + (double)val2 / PRECISION32;
251 }
252
253 return 0;
254 }
255
256 /* convert from binary64 to float */
lwm2m_b64_to_float(uint8_t * b64,size_t len,double * out)257 int lwm2m_b64_to_float(uint8_t *b64, size_t len, double *out)
258 {
259 int64_t f, k;
260 int i, e;
261 bool sign = false;
262 int64_t val1, val2;
263
264 if (len != 8) {
265 return -EINVAL;
266 }
267
268 val1 = 0LL;
269 val2 = 0LL;
270
271 /* calc sign: bit 63 */
272 sign = SHIFT_RIGHT(b64[0], 7, 0x1);
273
274 /* get exponent: bits 62-52 */
275 e = SHIFT_LEFT((uint16_t)b64[0], 4, 0x7F0);
276 e += SHIFT_RIGHT(b64[1], 4, 0xF);
277 /* remove bias */
278 e -= 1023;
279
280 /* enable "hidden" fraction bit 53 which is always 1 */
281 f = ((int64_t)1 << 52);
282 /* get fraction: bits 51-0 */
283 f += ((int64_t)(b64[1] & 0xF) << 48);
284 f += ((int64_t)b64[2] << 40);
285 f += ((int64_t)b64[3] << 32);
286 f += ((int64_t)b64[4] << 24);
287 f += ((int64_t)b64[5] << 16);
288 f += ((int64_t)b64[6] << 8);
289 f += b64[7];
290
291 /* handle whole number */
292 if (e > -1) {
293 /* precision overflow */
294 if (e > 52) {
295 e = 52;
296 }
297
298 val1 = (f >> (52 - e)) * (sign ? -1 : 1);
299 }
300
301 /* calculate the rest of the decimal */
302 k = PRECISION64;
303
304 /* account for -e */
305 while (e < -1) {
306 k /= 2;
307 e++;
308 }
309
310 for (i = 51 - e; i >= 0; i--) {
311 k /= 2;
312 if (f & ((int64_t)1 << i)) {
313 val2 += k;
314
315 }
316 }
317
318 if (sign) {
319 *out = (double)val1 - (double)val2 / PRECISION64;
320 } else {
321 *out = (double)val1 + (double)val2 / PRECISION64;
322 }
323
324 return 0;
325 }
326
lwm2m_atof(const char * input,double * out)327 int lwm2m_atof(const char *input, double *out)
328 {
329 char *pos, *end, buf[24];
330 long val;
331 int64_t base = PRECISION64, sign = 1;
332 int64_t val1, val2;
333
334 if (!input || !out) {
335 return -EINVAL;
336 }
337
338 strncpy(buf, input, sizeof(buf) - 1);
339 buf[sizeof(buf) - 1] = '\0';
340
341 if (strchr(buf, '-')) {
342 sign = -1;
343 }
344
345 pos = strchr(buf, '.');
346 if (pos) {
347 *pos = '\0';
348 }
349
350 errno = 0;
351 val = strtol(buf, &end, 10);
352 if (errno || *end) {
353 return -EINVAL;
354 }
355
356 val1 = (int64_t)val;
357 val2 = 0;
358
359 if (!pos) {
360 *out = (double)val1;
361 return 0;
362 }
363
364 while (*(++pos) && base > 1 && isdigit((unsigned char)*pos) != 0) {
365 val2 = val2 * 10 + (*pos - '0');
366 base /= 10;
367 }
368
369 val2 *= sign * base;
370
371 *out = (double)val1 + (double)val2 / PRECISION64;
372
373 return !*pos || base == 1 ? 0 : -EINVAL;
374 }
375
lwm2m_ftoa(double * input,char * out,size_t outlen,int8_t dec_limit)376 int lwm2m_ftoa(double *input, char *out, size_t outlen, int8_t dec_limit)
377 {
378 size_t len;
379 char buf[PRECISION64_LEN + 1];
380 int64_t val1 = (int64_t)*input;
381 int64_t val2 = (*input - (int64_t)*input) * PRECISION64;
382
383 len = snprintk(buf, sizeof(buf), "%0*lld", PRECISION64_LEN,
384 (long long)llabs(val2));
385 if (len != PRECISION64_LEN) {
386 strcpy(buf, "0");
387 } else {
388 /* Round the value to the specified decimal point. */
389 if (dec_limit > 0 && dec_limit < sizeof(buf) &&
390 buf[dec_limit] != '\0') {
391 bool round_up = buf[dec_limit] >= '5';
392
393 buf[dec_limit] = '\0';
394 len = dec_limit;
395
396 while (round_up && dec_limit > 0) {
397 dec_limit--;
398 buf[dec_limit]++;
399
400 if (buf[dec_limit] > '9') {
401 buf[dec_limit] = '0';
402 } else {
403 round_up = false;
404 }
405 }
406
407 if (round_up) {
408 if (*input < 0) {
409 val1--;
410 } else {
411 val1++;
412 }
413 }
414 }
415
416 /* clear ending zeroes, but leave 1 if needed */
417 while (len > 1U && buf[len - 1] == '0') {
418 buf[--len] = '\0';
419 }
420 }
421
422 return snprintk(out, outlen, "%s%lld.%s",
423 /* handle negative val2 when val1 is 0 */
424 (val1 == 0 && val2 < 0) ? "-" : "", (long long)val1, buf);
425 }
426
lwm2m_atou16(const uint8_t * buf,uint16_t buflen,uint16_t * len)427 uint16_t lwm2m_atou16(const uint8_t *buf, uint16_t buflen, uint16_t *len)
428 {
429 uint16_t val = 0U;
430 uint16_t pos = 0U;
431
432 /* we should get a value first - consume all numbers */
433 while (pos < buflen && isdigit(buf[pos]) != 0) {
434 val = val * 10U + (buf[pos] - '0');
435 pos++;
436 }
437
438 *len = pos;
439 return val;
440 }
441
lwm2m_string_to_path(const char * pathstr,struct lwm2m_obj_path * path,char delim)442 int lwm2m_string_to_path(const char *pathstr, struct lwm2m_obj_path *path,
443 char delim)
444 {
445 uint16_t value, len;
446 int i, tokstart = -1, toklen;
447 int end_index = strlen(pathstr) - 1;
448
449 (void)memset(path, 0, sizeof(*path));
450 for (i = 0; i <= end_index; i++) {
451 /* search for first numeric */
452 if (tokstart == -1) {
453 if (isdigit((unsigned char)pathstr[i]) == 0) {
454 continue;
455 }
456
457 tokstart = i;
458 }
459
460 /* find delimiter char or end of string */
461 if (pathstr[i] == delim || i == end_index) {
462 toklen = i - tokstart + 1;
463
464 /* don't process delimiter char */
465 if (pathstr[i] == delim) {
466 toklen--;
467 }
468
469 if (toklen <= 0) {
470 continue;
471 }
472
473 value = lwm2m_atou16(&pathstr[tokstart], toklen, &len);
474 /* increase the path level for each token found */
475 path->level++;
476 switch (path->level) {
477 case LWM2M_PATH_LEVEL_OBJECT:
478 path->obj_id = value;
479 break;
480
481 case LWM2M_PATH_LEVEL_OBJECT_INST:
482 path->obj_inst_id = value;
483 break;
484
485 case LWM2M_PATH_LEVEL_RESOURCE:
486 path->res_id = value;
487 break;
488
489 case LWM2M_PATH_LEVEL_RESOURCE_INST:
490 path->res_inst_id = value;
491 break;
492
493 default:
494 return -EINVAL;
495
496 }
497
498 tokstart = -1;
499 }
500 }
501
502 return 0;
503 }
504
lwm2m_obj_path_equal(const struct lwm2m_obj_path * a,const struct lwm2m_obj_path * b)505 bool lwm2m_obj_path_equal(const struct lwm2m_obj_path *a, const struct lwm2m_obj_path *b)
506 {
507 uint8_t level = a->level;
508
509 if (a->level != b->level) {
510 return false;
511 }
512
513 if (level >= LWM2M_PATH_LEVEL_OBJECT && (a->obj_id != b->obj_id)) {
514 return false;
515 }
516
517 if (level >= LWM2M_PATH_LEVEL_OBJECT_INST && (a->obj_inst_id != b->obj_inst_id)) {
518 return false;
519 }
520
521 if (level >= LWM2M_PATH_LEVEL_RESOURCE && (a->res_id != b->res_id)) {
522 return false;
523 }
524
525 if (level >= LWM2M_PATH_LEVEL_RESOURCE_INST && (a->res_inst_id != b->res_inst_id)) {
526 return false;
527 }
528
529 return true;
530 }
531
532 /* for debugging: to print IP addresses */
lwm2m_sprint_ip_addr(const struct sockaddr * addr)533 char *lwm2m_sprint_ip_addr(const struct sockaddr *addr)
534 {
535 static char buf[NET_IPV6_ADDR_LEN];
536
537 if (addr->sa_family == AF_INET6) {
538 return net_addr_ntop(AF_INET6, &net_sin6(addr)->sin6_addr, buf, sizeof(buf));
539 }
540
541 if (addr->sa_family == AF_INET) {
542 return net_addr_ntop(AF_INET, &net_sin(addr)->sin_addr, buf, sizeof(buf));
543 }
544
545 return "::";
546 }
547
to_hex_digit(uint8_t digit)548 static uint8_t to_hex_digit(uint8_t digit)
549 {
550 if (digit >= 10U) {
551 return digit - 10U + 'a';
552 }
553
554 return digit + '0';
555 }
556
sprint_token(const uint8_t * token,uint8_t tkl)557 char *sprint_token(const uint8_t *token, uint8_t tkl)
558 {
559 static char buf[32];
560 char *ptr = buf;
561
562 if (token && tkl != 0) {
563 int i;
564
565 tkl = MIN(tkl, sizeof(buf) / 2 - 1);
566
567 for (i = 0; i < tkl; i++) {
568 *ptr++ = to_hex_digit(token[i] >> 4);
569 *ptr++ = to_hex_digit(token[i] & 0x0F);
570 }
571
572 *ptr = '\0';
573 } else {
574 strcpy(buf, "[no-token]");
575 }
576
577 return buf;
578 }
579
