1 /*
2 * Copyright (c) 2020 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stddef.h>
8 #include <stdbool.h>
9 #include <stdint.h>
10 #include <string.h>
11 #include "zcbor_common.h"
12 #include "zcbor_print.h"
13
14 _Static_assert((sizeof(size_t) == sizeof(void *)),
15 "This code needs size_t to be the same length as pointers.");
16
17 _Static_assert((sizeof(zcbor_state_t) >= sizeof(struct zcbor_state_constant)),
18 "This code needs zcbor_state_t to be at least as large as zcbor_backups_t.");
19
zcbor_new_backup(zcbor_state_t * state,size_t new_elem_count)20 bool zcbor_new_backup(zcbor_state_t *state, size_t new_elem_count)
21 {
22 ZCBOR_CHECK_ERROR();
23
24 if ((state->constant_state->current_backup)
25 >= state->constant_state->num_backups) {
26 ZCBOR_ERR(ZCBOR_ERR_NO_BACKUP_MEM);
27 }
28
29 state->payload_moved = false;
30
31 (state->constant_state->current_backup)++;
32
33 /* use the backup at current_backup - 1, since otherwise, the 0th
34 * backup would be unused. */
35 size_t i = (state->constant_state->current_backup) - 1;
36
37 memcpy(&state->constant_state->backup_list[i], state,
38 sizeof(zcbor_state_t));
39
40 state->elem_count = new_elem_count;
41
42 zcbor_log("New backup (level %zu)\n", i);
43
44 return true;
45 }
46
47
zcbor_process_backup(zcbor_state_t * state,uint32_t flags,size_t max_elem_count)48 bool zcbor_process_backup(zcbor_state_t *state, uint32_t flags,
49 size_t max_elem_count)
50 {
51 ZCBOR_CHECK_ERROR();
52 zcbor_state_t local_copy = *state;
53
54 if (state->constant_state->current_backup == 0) {
55 zcbor_log("No backups available.\r\n");
56 ZCBOR_ERR(ZCBOR_ERR_NO_BACKUP_ACTIVE);
57 }
58
59 /* use the backup at current_backup - 1, since otherwise, the
60 * 0th backup would be unused. */
61 size_t i = state->constant_state->current_backup - 1;
62
63 zcbor_log("Process backup (level %zu, flags 0x%x)\n", i, flags);
64
65 if (flags & ZCBOR_FLAG_RESTORE) {
66 if (!(flags & ZCBOR_FLAG_KEEP_PAYLOAD)) {
67 if (state->constant_state->backup_list[i].payload_moved) {
68 zcbor_log("Payload pointer out of date.\r\n");
69 ZCBOR_ERR(ZCBOR_ERR_PAYLOAD_OUTDATED);
70 }
71 }
72 memcpy(state, &state->constant_state->backup_list[i],
73 sizeof(zcbor_state_t));
74 }
75
76 if (flags & ZCBOR_FLAG_CONSUME) {
77 state->constant_state->current_backup--;
78 }
79
80 if (local_copy.elem_count > max_elem_count) {
81 zcbor_log("elem_count: %zu (expected max %zu)\r\n",
82 local_copy.elem_count, max_elem_count);
83 ZCBOR_ERR(ZCBOR_ERR_HIGH_ELEM_COUNT);
84 }
85
86 if (flags & ZCBOR_FLAG_KEEP_PAYLOAD) {
87 state->payload = local_copy.payload;
88 }
89
90 if (flags & ZCBOR_FLAG_KEEP_DECODE_STATE) {
91 /* Copy decode state */
92 state->decode_state = local_copy.decode_state;
93 }
94
95 return true;
96 }
97
update_backups(zcbor_state_t * state,uint8_t const * new_payload_end)98 static void update_backups(zcbor_state_t *state, uint8_t const *new_payload_end)
99 {
100 if (state->constant_state) {
101 for (unsigned int i = 0; i < state->constant_state->current_backup; i++) {
102 state->constant_state->backup_list[i].payload_end = new_payload_end;
103 state->constant_state->backup_list[i].payload_moved = true;
104 }
105 }
106 }
107
108
zcbor_union_start_code(zcbor_state_t * state)109 bool zcbor_union_start_code(zcbor_state_t *state)
110 {
111 if (!zcbor_new_backup(state, state->elem_count)) {
112 ZCBOR_FAIL();
113 }
114 return true;
115 }
116
117
zcbor_union_elem_code(zcbor_state_t * state)118 bool zcbor_union_elem_code(zcbor_state_t *state)
119 {
120 if (!zcbor_process_backup(state, ZCBOR_FLAG_RESTORE, state->elem_count)) {
121 ZCBOR_FAIL();
122 }
123 return true;
124 }
125
zcbor_union_end_code(zcbor_state_t * state)126 bool zcbor_union_end_code(zcbor_state_t *state)
127 {
128 if (!zcbor_process_backup(state, ZCBOR_FLAG_CONSUME, state->elem_count)) {
129 ZCBOR_FAIL();
130 }
131 return true;
132 }
133
zcbor_new_state(zcbor_state_t * state_array,size_t n_states,const uint8_t * payload,size_t payload_len,size_t elem_count,uint8_t * flags,size_t flags_bytes)134 void zcbor_new_state(zcbor_state_t *state_array, size_t n_states,
135 const uint8_t *payload, size_t payload_len, size_t elem_count,
136 uint8_t *flags, size_t flags_bytes)
137 {
138 state_array[0].payload = payload;
139 state_array[0].payload_end = payload + payload_len;
140 state_array[0].elem_count = elem_count;
141 state_array[0].payload_moved = false;
142 state_array[0].decode_state.indefinite_length_array = false;
143 #ifdef ZCBOR_MAP_SMART_SEARCH
144 state_array[0].decode_state.map_search_elem_state = flags;
145 state_array[0].decode_state.map_elem_count = 0;
146 #else
147 state_array[0].decode_state.map_elems_processed = 0;
148 (void)flags;
149 (void)flags_bytes;
150 #endif
151 state_array[0].constant_state = NULL;
152
153 if (n_states < 2) {
154 return;
155 }
156
157 /* Use the last state as a struct zcbor_state_constant object. */
158 state_array[0].constant_state = (struct zcbor_state_constant *)&state_array[n_states - 1];
159 state_array[0].constant_state->backup_list = NULL;
160 state_array[0].constant_state->num_backups = n_states - 2;
161 state_array[0].constant_state->current_backup = 0;
162 state_array[0].constant_state->error = ZCBOR_SUCCESS;
163 #ifdef ZCBOR_STOP_ON_ERROR
164 state_array[0].constant_state->stop_on_error = false;
165 #endif
166 state_array[0].constant_state->enforce_canonical = ZCBOR_ENFORCE_CANONICAL_DEFAULT;
167 state_array[0].constant_state->manually_process_elem = ZCBOR_MANUALLY_PROCESS_ELEM_DEFAULT;
168 #ifdef ZCBOR_MAP_SMART_SEARCH
169 state_array[0].constant_state->map_search_elem_state_end = flags + flags_bytes;
170 #endif
171 if (n_states > 2) {
172 state_array[0].constant_state->backup_list = &state_array[1];
173 }
174 }
175
zcbor_update_state(zcbor_state_t * state,const uint8_t * payload,size_t payload_len)176 void zcbor_update_state(zcbor_state_t *state,
177 const uint8_t *payload, size_t payload_len)
178 {
179 state->payload = payload;
180 state->payload_end = payload + payload_len;
181
182 update_backups(state, state->payload_end);
183 }
184
185
zcbor_validate_string_fragments(struct zcbor_string_fragment * fragments,size_t num_fragments)186 bool zcbor_validate_string_fragments(struct zcbor_string_fragment *fragments,
187 size_t num_fragments)
188 {
189 size_t total_len = 0;
190
191 if (fragments == NULL) {
192 return false;
193 }
194
195 for (size_t i = 0; i < num_fragments; i++) {
196 if (fragments[i].offset != total_len) {
197 return false;
198 }
199 if (fragments[i].fragment.value == NULL) {
200 return false;
201 }
202 if (fragments[i].total_len != fragments[0].total_len) {
203 return false;
204 }
205 total_len += fragments[i].fragment.len;
206 if (total_len > fragments[0].total_len) {
207 return false;
208 }
209 }
210
211 if (num_fragments && total_len != fragments[0].total_len) {
212 return false;
213 }
214
215 if (num_fragments && (fragments[0].total_len == ZCBOR_STRING_FRAGMENT_UNKNOWN_LENGTH)) {
216 for (size_t i = 0; i < num_fragments; i++) {
217 fragments[i].total_len = total_len;
218 }
219 }
220
221 return true;
222 }
223
zcbor_splice_string_fragments(struct zcbor_string_fragment * fragments,size_t num_fragments,uint8_t * result,size_t * result_len)224 bool zcbor_splice_string_fragments(struct zcbor_string_fragment *fragments,
225 size_t num_fragments, uint8_t *result, size_t *result_len)
226 {
227 size_t total_len = 0;
228
229 if (!fragments) {
230 return false;
231 }
232
233 for (size_t i = 0; i < num_fragments; i++) {
234 if ((total_len > *result_len)
235 || (fragments[i].fragment.len > (*result_len - total_len))) {
236 return false;
237 }
238 memcpy(&result[total_len],
239 fragments[i].fragment.value, fragments[i].fragment.len);
240 total_len += fragments[i].fragment.len;
241 }
242
243 *result_len = total_len;
244 return true;
245 }
246
247
zcbor_compare_strings(const struct zcbor_string * str1,const struct zcbor_string * str2)248 bool zcbor_compare_strings(const struct zcbor_string *str1,
249 const struct zcbor_string *str2)
250 {
251 return (str1 != NULL) && (str2 != NULL)
252 && (str1->value != NULL) && (str2->value != NULL) && (str1->len == str2->len)
253 && (memcmp(str1->value, str2->value, str1->len) == 0);
254 }
255
256
zcbor_header_len(uint64_t value)257 size_t zcbor_header_len(uint64_t value)
258 {
259 if (value <= ZCBOR_VALUE_IN_HEADER) {
260 return 1;
261 } else if (value <= 0xFF) {
262 return 2;
263 } else if (value <= 0xFFFF) {
264 return 3;
265 } else if (value <= 0xFFFFFFFF) {
266 return 5;
267 } else {
268 return 9;
269 }
270 }
271
272
zcbor_header_len_ptr(const void * const value,size_t value_len)273 size_t zcbor_header_len_ptr(const void *const value, size_t value_len)
274 {
275 uint64_t val64 = 0;
276
277 if (value_len > sizeof(val64)) {
278 return 0;
279 }
280
281 memcpy(((uint8_t*)&val64) + ZCBOR_ECPY_OFFS(sizeof(val64), value_len), value, value_len);
282 return zcbor_header_len(val64);
283 }
284
285
zcbor_remaining_str_len(zcbor_state_t * state)286 size_t zcbor_remaining_str_len(zcbor_state_t *state)
287 {
288 size_t max_len = (size_t)state->payload_end - (size_t)state->payload;
289
290 if (max_len == 0) {
291 return 0;
292 }
293
294 size_t max_header_len = zcbor_header_len(max_len);
295 size_t min_header_len = zcbor_header_len(max_len - max_header_len);
296 size_t header_len = zcbor_header_len(max_len - min_header_len);
297
298 return max_len - header_len;
299 }
300
301
zcbor_entry_function(const uint8_t * payload,size_t payload_len,void * result,size_t * payload_len_out,zcbor_state_t * state,zcbor_decoder_t func,size_t n_states,size_t elem_count)302 int zcbor_entry_function(const uint8_t *payload, size_t payload_len,
303 void *result, size_t *payload_len_out, zcbor_state_t *state, zcbor_decoder_t func,
304 size_t n_states, size_t elem_count)
305 {
306 zcbor_new_state(state, n_states, payload, payload_len, elem_count, NULL, 0);
307
308 bool ret = func(state, result);
309
310 if (!ret) {
311 int err = zcbor_pop_error(state);
312
313 err = (err == ZCBOR_SUCCESS) ? ZCBOR_ERR_UNKNOWN : err;
314 return err;
315 }
316
317 if (payload_len_out != NULL) {
318 *payload_len_out = MIN(payload_len,
319 (size_t)state[0].payload - (size_t)payload);
320 }
321 return ZCBOR_SUCCESS;
322 }
323
324
325 /* Float16: */
326 #define F16_SIGN_OFFS 15 /* Bit offset of the sign bit. */
327 #define F16_EXPO_OFFS 10 /* Bit offset of the exponent. */
328 #define F16_EXPO_MSK 0x1F /* Bitmask for the exponent (right shifted by F16_EXPO_OFFS). */
329 #define F16_MANTISSA_MSK 0x3FF /* Bitmask for the mantissa. */
330 #define F16_MAX 65520 /* Lowest float32 value that rounds up to float16 infinity.
331 * (65519.996 rounds to 65504) */
332 #define F16_MIN_EXPO 24 /* Negative exponent of the non-zero float16 value closest to 0 (2^-24) */
333 #define F16_MIN (1.0f / (1 << F16_MIN_EXPO)) /* The non-zero float16 value closest to 0 (2^-24) */
334 #define F16_MIN_NORM (1.0f / (1 << 14)) /* The normalized float16 value closest to 0 (2^-14) */
335 #define F16_BIAS 15 /* The exponent bias of normalized float16 values. */
336
337 /* Float32: */
338 #define F32_SIGN_OFFS 31 /* Bit offset of the sign bit. */
339 #define F32_EXPO_OFFS 23 /* Bit offset of the exponent. */
340 #define F32_EXPO_MSK 0xFF /* Bitmask for the exponent (right shifted by F32_EXPO_OFFS). */
341 #define F32_MANTISSA_MSK 0x7FFFFF /* Bitmask for the mantissa. */
342 #define F32_BIAS 127 /* The exponent bias of normalized float32 values. */
343
344 /* Rounding: */
345 #define SUBNORM_ROUND_MSK (F32_MANTISSA_MSK | (1 << F32_EXPO_OFFS)) /* mantissa + lsb of expo for
346 * tiebreak. */
347 #define SUBNORM_ROUND_BIT_MSK (1 << (F32_EXPO_OFFS - 1)) /* msb of mantissa (0x400000) */
348 #define NORM_ROUND_MSK (F32_MANTISSA_MSK >> (F16_EXPO_OFFS - 1)) /* excess mantissa when going from
349 * float32 to float16 + 1 extra bit
350 * for tiebreak. */
351 #define NORM_ROUND_BIT_MSK (1 << (F32_EXPO_OFFS - F16_EXPO_OFFS - 1)) /* bit 12 (0x1000) */
352
353
zcbor_float16_to_32(uint16_t input)354 float zcbor_float16_to_32(uint16_t input)
355 {
356 uint32_t sign = input >> F16_SIGN_OFFS;
357 uint32_t expo = (input >> F16_EXPO_OFFS) & F16_EXPO_MSK;
358 uint32_t mantissa = input & F16_MANTISSA_MSK;
359
360 if ((expo == 0) && (mantissa != 0)) {
361 /* Subnormal float16 - convert to normalized float32 */
362 return ((float)mantissa * F16_MIN) * (sign ? -1 : 1);
363 } else {
364 /* Normalized / zero / Infinity / NaN */
365 uint32_t new_expo = (expo == 0 /* zero */) ? 0
366 : (expo == F16_EXPO_MSK /* inf/NaN */) ? F32_EXPO_MSK
367 : (expo + (F32_BIAS - F16_BIAS));
368 uint32_t value32 = (sign << F32_SIGN_OFFS) | (new_expo << F32_EXPO_OFFS)
369 | (mantissa << (F32_EXPO_OFFS - F16_EXPO_OFFS));
370 return *(float *)&value32;
371 }
372 }
373
374
zcbor_float32_to_16(float input)375 uint16_t zcbor_float32_to_16(float input)
376 {
377 uint32_t value32 = *(uint32_t *)&input;
378
379 uint32_t sign = value32 >> F32_SIGN_OFFS;
380 uint32_t expo = (value32 >> F32_EXPO_OFFS) & F32_EXPO_MSK;
381 uint32_t mantissa = value32 & F32_MANTISSA_MSK;
382
383 uint16_t value16 = (uint16_t)(sign << F16_SIGN_OFFS);
384
385 float abs_input;
386 *(uint32_t *)&abs_input = value32 & ~(1 << F32_SIGN_OFFS);
387
388 if (abs_input <= (F16_MIN / 2)) {
389 /* 0 or too small for float16. Round down to 0. value16 is already correct. */
390 } else if (abs_input < F16_MIN) {
391 /* Round up to 2^(-24) (F16_MIN), has other rounding rules than larger values. */
392 value16 |= 0x0001;
393 } else if (abs_input < F16_MIN_NORM) {
394 /* Subnormal float16 (normal float32) */
395 uint32_t adjusted_mantissa =
396 /* Adjust for the purposes of checking rounding. */
397 /* The lsb of expo is needed for the cases where expo is 103 (minimum). */
398 ((value32 << (expo - (F32_BIAS - F16_MIN_EXPO))) & SUBNORM_ROUND_MSK);
399 uint16_t rounding_bit =
400 /* "Round to nearest, ties to even". */
401 /* 0x400000 means ties go down towards even. (0xC00000 means ties go up.) */
402 (adjusted_mantissa & SUBNORM_ROUND_BIT_MSK)
403 && (adjusted_mantissa != SUBNORM_ROUND_BIT_MSK);
404 value16 |= ((uint16_t)(abs_input * (1 << 24)) + rounding_bit); /* expo is 0 */
405 } else if (abs_input < F16_MAX) {
406 /* Normal float16 (normal float32) */
407 uint16_t rounding_bit =
408 /* Bit 13 of the mantissa represents which way to round, except for the */
409 /* special case where bits 0-12 and 14 are 0. */
410 /* This is because of "Round to nearest, ties to even". */
411 /* 0x1000 means ties go down towards even. (0x3000 means ties go up.) */
412 ((mantissa & NORM_ROUND_BIT_MSK)
413 && ((mantissa & NORM_ROUND_MSK) != NORM_ROUND_BIT_MSK));
414 value16 |= (uint16_t)((expo - (F32_BIAS - F16_BIAS)) << F16_EXPO_OFFS);
415 value16 |= (uint16_t)(mantissa >> (F32_EXPO_OFFS - F16_EXPO_OFFS));
416 value16 += rounding_bit; /* Might propagate to exponent. */
417 } else if (expo != F32_EXPO_MSK || !mantissa) {
418 /* Infinite, or finite normal float32 too large for float16. Round up to inf. */
419 value16 |= (F16_EXPO_MSK << F16_EXPO_OFFS);
420 } else {
421 /* NaN */
422 /* Preserve msbit of mantissa. */
423 uint16_t new_mantissa = (uint16_t)(mantissa >> (F32_EXPO_OFFS - F16_EXPO_OFFS));
424 value16 |= (F16_EXPO_MSK << F16_EXPO_OFFS) | (new_mantissa ? new_mantissa : 1);
425 }
426
427 return value16;
428 }
429
430
431 /** Weak strnlen() implementation in case it is not available.
432 *
433 * This function is in the public domain, according to:
434 * https://github.com/arm-embedded/gcc-arm-none-eabi.debian/blob/master/src/libiberty/strnlen.c
435 */
436 __attribute__((__weak__))
strnlen(const char * s,size_t maxlen)437 size_t strnlen (const char *s, size_t maxlen)
438 {
439 size_t i;
440
441 for (i = 0; i < maxlen; ++i) {
442 if (s[i] == '\0') {
443 break;
444 }
445 }
446 return i;
447 }
448
