1 /*
2 * This file has been copied from the zcbor library.
3 * Commit zcbor 0.7.0
4 */
5
6 /*
7 * Copyright (c) 2020 Nordic Semiconductor ASA
8 *
9 * SPDX-License-Identifier: Apache-2.0
10 */
11
12 #include <stdint.h>
13 #include <stdbool.h>
14 #include <stddef.h>
15 #include <string.h>
16 #include "zcbor_encode.h"
17 #include "zcbor_common.h"
18
19 _Static_assert((sizeof(size_t) == sizeof(void *)),
20 "This code needs size_t to be the same length as pointers.");
21
22
log2ceil(uint_fast32_t val)23 static uint8_t log2ceil(uint_fast32_t val)
24 {
25 switch(val) {
26 case 1: return 0;
27 case 2: return 1;
28 case 3: return 2;
29 case 4: return 2;
30 case 5: return 3;
31 case 6: return 3;
32 case 7: return 3;
33 case 8: return 3;
34 }
35
36 zcbor_print("Should not come here.\r\n");
37 return 0;
38 }
39
get_additional(uint_fast32_t len,uint8_t value0)40 static uint8_t get_additional(uint_fast32_t len, uint8_t value0)
41 {
42 return len == 0 ? value0 : (uint8_t)(24 + log2ceil(len));
43 }
44
encode_header_byte(zcbor_state_t * state,zcbor_major_type_t major_type,uint8_t additional)45 static bool encode_header_byte(zcbor_state_t *state,
46 zcbor_major_type_t major_type, uint8_t additional)
47 {
48 ZCBOR_CHECK_ERROR();
49 ZCBOR_CHECK_PAYLOAD();
50
51 zcbor_assert_state(additional < 32, NULL);
52
53 *(state->payload_mut++) = (uint8_t)((major_type << 5) | (additional & 0x1F));
54 return true;
55 }
56
57
58 static uint_fast32_t get_encoded_len(const void *const result, uint_fast32_t result_len);
59
60
61 /** Encode a single value.
62 */
value_encode_len(zcbor_state_t * state,zcbor_major_type_t major_type,const void * const result,uint_fast32_t result_len)63 static bool value_encode_len(zcbor_state_t *state, zcbor_major_type_t major_type,
64 const void *const result, uint_fast32_t result_len)
65 {
66 uint8_t *u8_result = (uint8_t *)result;
67 uint_fast32_t encoded_len = get_encoded_len(result, result_len);
68
69 if ((state->payload + 1 + encoded_len) > state->payload_end) {
70 ZCBOR_ERR(ZCBOR_ERR_NO_PAYLOAD);
71 }
72
73 if (!encode_header_byte(state, major_type,
74 get_additional(encoded_len, u8_result[0]))) {
75 ZCBOR_FAIL();
76 }
77 state->payload_mut--;
78 zcbor_trace();
79 state->payload_mut++;
80
81 #ifdef CONFIG_BIG_ENDIAN
82 memcpy(state->payload_mut, u8_result, encoded_len);
83 state->payload_mut += encoded_len;
84 #else
85 for (; encoded_len > 0; encoded_len--) {
86 *(state->payload_mut++) = u8_result[encoded_len - 1];
87 }
88 #endif /* CONFIG_BIG_ENDIAN */
89
90 state->elem_count++;
91 return true;
92 }
93
94
get_result_len(const void * const input,uint_fast32_t max_result_len)95 static uint_fast32_t get_result_len(const void *const input, uint_fast32_t max_result_len)
96 {
97 uint8_t *u8_result = (uint8_t *)input;
98 uint_fast32_t len = max_result_len;
99
100 for (; len > 0; len--) {
101 #ifdef CONFIG_BIG_ENDIAN
102 if (u8_result[max_result_len - len] != 0) {
103 #else
104 if (u8_result[len - 1] != 0) {
105 #endif /* CONFIG_BIG_ENDIAN */
106 break;
107 }
108 }
109
110 /* Round up to nearest power of 2. */
111 return len <= 2 ? len : (uint8_t)(1 << log2ceil(len));
112 }
113
114
115 static const void *get_result(const void *const input, uint_fast32_t max_result_len,
116 uint_fast32_t result_len)
117 {
118 #ifdef CONFIG_BIG_ENDIAN
119 return &((uint8_t *)input)[max_result_len - (result_len ? result_len : 1)];
120 #else
121 return input;
122 #endif
123 }
124
125
126 static uint_fast32_t get_encoded_len(const void *const result, uint_fast32_t result_len)
127 {
128 const uint8_t *u8_result = (const uint8_t *)result;
129
130 if ((result_len == 1) && (u8_result[0] <= ZCBOR_VALUE_IN_HEADER)) {
131 return 0;
132 }
133 return result_len;
134 }
135
136
137 static bool value_encode(zcbor_state_t *state, zcbor_major_type_t major_type,
138 const void *const input, uint_fast32_t max_result_len)
139 {
140 zcbor_assert_state(max_result_len != 0, "0-length result not supported.\r\n");
141
142 uint_fast32_t result_len = get_result_len(input, max_result_len);
143 const void *const result = get_result(input, max_result_len, result_len);
144
145 return value_encode_len(state, major_type, result, result_len);
146 }
147
148
149 bool zcbor_int_encode(zcbor_state_t *state, const void *input_int, size_t int_size)
150 {
151 zcbor_major_type_t major_type;
152 uint8_t input_buf[8];
153 const uint8_t *input_uint8 = input_int;
154 const int8_t *input_int8 = input_int;
155 const uint8_t *input = input_int;
156
157 if (int_size > sizeof(int64_t)) {
158 ZCBOR_ERR(ZCBOR_ERR_INT_SIZE);
159 }
160
161 #ifdef CONFIG_BIG_ENDIAN
162 if (input_int8[0] < 0) {
163 #else
164 if (input_int8[int_size - 1] < 0) {
165 #endif
166 major_type = ZCBOR_MAJOR_TYPE_NINT;
167
168 /* Convert to CBOR's representation by flipping all bits. */
169 for (int i = 0; i < int_size; i++) {
170 input_buf[i] = (uint8_t)~input_uint8[i];
171 }
172 input = input_buf;
173 } else {
174 major_type = ZCBOR_MAJOR_TYPE_PINT;
175 }
176
177 if (!value_encode(state, major_type, input, int_size)) {
178 ZCBOR_FAIL();
179 }
180
181 return true;
182 }
183
184
185 bool zcbor_int32_encode(zcbor_state_t *state, const int32_t *input)
186 {
187 return zcbor_int_encode(state, input, sizeof(*input));
188 }
189
190
191 bool zcbor_int64_encode(zcbor_state_t *state, const int64_t *input)
192 {
193 return zcbor_int_encode(state, input, sizeof(*input));
194 }
195
196
197 static bool uint32_encode(zcbor_state_t *state, const uint32_t *input,
198 zcbor_major_type_t major_type)
199 {
200 if (!value_encode(state, major_type, input, 4)) {
201 ZCBOR_FAIL();
202 }
203 return true;
204 }
205
206
207 bool zcbor_uint32_encode(zcbor_state_t *state, const uint32_t *input)
208 {
209 if (!uint32_encode(state, input, ZCBOR_MAJOR_TYPE_PINT)) {
210 ZCBOR_FAIL();
211 }
212 return true;
213 }
214
215
216 static bool uint64_encode(zcbor_state_t *state, const uint64_t *input,
217 zcbor_major_type_t major_type)
218 {
219 if (!value_encode(state, major_type, input, 8)) {
220 ZCBOR_FAIL();
221 }
222 return true;
223 }
224
225
226 bool zcbor_uint64_encode(zcbor_state_t *state, const uint64_t *input)
227 {
228 if (!uint64_encode(state, input, ZCBOR_MAJOR_TYPE_PINT)) {
229 ZCBOR_FAIL();
230 }
231 return true;
232 }
233
234
235 bool zcbor_int32_put(zcbor_state_t *state, int32_t input)
236 {
237 return zcbor_int_encode(state, &input, sizeof(input));
238 }
239
240
241 bool zcbor_int64_put(zcbor_state_t *state, int64_t input)
242 {
243 return zcbor_int_encode(state, &input, sizeof(input));
244 }
245
246
247 bool zcbor_uint32_put(zcbor_state_t *state, uint32_t input)
248 {
249 return zcbor_uint64_put(state, input);
250 }
251
252
253 bool zcbor_uint64_put(zcbor_state_t *state, uint64_t input)
254 {
255 if (!uint64_encode(state, &input, ZCBOR_MAJOR_TYPE_PINT)) {
256 ZCBOR_FAIL();
257 }
258 return true;
259 }
260
261
262 #ifdef ZCBOR_SUPPORTS_SIZE_T
263 bool zcbor_size_put(zcbor_state_t *state, size_t input)
264 {
265 return zcbor_uint64_put(state, input);
266 }
267
268
269 bool zcbor_size_encode(zcbor_state_t *state, const size_t *input)
270 {
271 return zcbor_size_put(state, *input);
272 }
273 #endif
274
275 static bool str_start_encode(zcbor_state_t *state,
276 const struct zcbor_string *input, zcbor_major_type_t major_type)
277 {
278 if (input->value && ((get_result_len(&input->len, sizeof(input->len))
279 + 1 + input->len + (size_t)state->payload)
280 > (size_t)state->payload_end)) {
281 ZCBOR_ERR(ZCBOR_ERR_NO_PAYLOAD);
282 }
283 if (!value_encode(state, major_type, &input->len, sizeof(input->len))) {
284 ZCBOR_FAIL();
285 }
286
287 return true;
288 }
289
290
291 static bool primitive_put(zcbor_state_t *state, uint32_t input)
292 {
293 if (!uint32_encode(state, &input, ZCBOR_MAJOR_TYPE_PRIM)) {
294 ZCBOR_FAIL();
295 }
296 return true;
297 }
298
299
300 static size_t remaining_str_len(zcbor_state_t *state)
301 {
302 size_t max_len = (size_t)state->payload_end - (size_t)state->payload;
303 size_t result_len = get_result_len(&max_len, sizeof(max_len));
304
305 return max_len - result_len - 1;
306 }
307
308
309 bool zcbor_bstr_start_encode(zcbor_state_t *state)
310 {
311 if (!zcbor_new_backup(state, 0)) {
312 ZCBOR_FAIL();
313 }
314
315 uint64_t max_len = remaining_str_len(state);
316
317 /* Encode a dummy header */
318 if (!uint64_encode(state, &max_len,
319 ZCBOR_MAJOR_TYPE_BSTR)) {
320 ZCBOR_FAIL();
321 }
322 return true;
323 }
324
325
326 bool zcbor_bstr_end_encode(zcbor_state_t *state, struct zcbor_string *result)
327 {
328 const uint8_t *payload = state->payload;
329 struct zcbor_string dummy_value;
330
331 if (result == NULL) {
332 /* Use a dummy value for the sake of the length calculation below.
333 * Will not be returned.
334 */
335 result = &dummy_value;
336 }
337
338 if (!zcbor_process_backup(state, ZCBOR_FLAG_RESTORE | ZCBOR_FLAG_CONSUME, 0xFFFFFFFF)) {
339 ZCBOR_FAIL();
340 }
341
342 result->value = state->payload_end - remaining_str_len(state);
343 result->len = (size_t)payload - (size_t)result->value;
344
345 /* Reencode header of list now that we know the number of elements. */
346 if (!zcbor_bstr_encode(state, result)) {
347 ZCBOR_FAIL();
348 }
349 return true;
350 }
351
352
353 static bool str_encode(zcbor_state_t *state,
354 const struct zcbor_string *input, zcbor_major_type_t major_type)
355 {
356 if (input->len > (state->payload_end - state->payload)) {
357 ZCBOR_ERR(ZCBOR_ERR_NO_PAYLOAD);
358 }
359 if (!str_start_encode(state, input, major_type)) {
360 ZCBOR_FAIL();
361 }
362 if (state->payload_mut != input->value) {
363 /* Use memmove since string might be encoded into the same space
364 * because of bstrx_cbor_start_encode/bstrx_cbor_end_encode. */
365 memmove(state->payload_mut, input->value, input->len);
366 }
367 state->payload += input->len;
368 return true;
369 }
370
371
372 bool zcbor_bstr_encode(zcbor_state_t *state, const struct zcbor_string *input)
373 {
374 return str_encode(state, input, ZCBOR_MAJOR_TYPE_BSTR);
375 }
376
377
378 bool zcbor_tstr_encode(zcbor_state_t *state, const struct zcbor_string *input)
379 {
380 return str_encode(state, input, ZCBOR_MAJOR_TYPE_TSTR);
381 }
382
383
384 static bool list_map_start_encode(zcbor_state_t *state, uint_fast32_t max_num,
385 zcbor_major_type_t major_type)
386 {
387 #ifdef ZCBOR_CANONICAL
388 if (!zcbor_new_backup(state, 0)) {
389 ZCBOR_FAIL();
390 }
391
392 /* Encode dummy header with max number of elements. */
393 if (!value_encode(state, major_type, &max_num, sizeof(max_num))) {
394 ZCBOR_FAIL();
395 }
396 state->elem_count--; /* Because of dummy header. */
397 #else
398 if (!encode_header_byte(state, major_type, ZCBOR_VALUE_IS_INDEFINITE_LENGTH)) {
399 ZCBOR_FAIL();
400 }
401 #endif
402 return true;
403 }
404
405
406 bool zcbor_list_start_encode(zcbor_state_t *state, uint_fast32_t max_num)
407 {
408 return list_map_start_encode(state, max_num, ZCBOR_MAJOR_TYPE_LIST);
409 }
410
411
412 bool zcbor_map_start_encode(zcbor_state_t *state, uint_fast32_t max_num)
413 {
414 return list_map_start_encode(state, max_num, ZCBOR_MAJOR_TYPE_MAP);
415 }
416
417
418 #ifdef ZCBOR_CANONICAL
419 static uint_fast32_t get_encoded_len2(const void *const input, uint_fast32_t max_result_len)
420 {
421 uint_fast32_t result_len = get_result_len(input, max_result_len);
422 const void *const result = get_result(input, max_result_len, result_len);
423
424 return get_encoded_len(result, result_len);
425 }
426 #endif
427
428
429 static bool list_map_end_encode(zcbor_state_t *state, uint_fast32_t max_num,
430 zcbor_major_type_t major_type)
431 {
432 #ifdef ZCBOR_CANONICAL
433 uint_fast32_t list_count = ((major_type == ZCBOR_MAJOR_TYPE_LIST) ?
434 state->elem_count
435 : (state->elem_count / 2));
436
437 const uint8_t *payload = state->payload;
438
439 uint_fast32_t max_header_len = get_encoded_len2(&max_num, 4);
440 uint_fast32_t header_len = get_encoded_len2(&list_count, 4);
441
442 if (!zcbor_process_backup(state, ZCBOR_FLAG_RESTORE | ZCBOR_FLAG_CONSUME, 0xFFFFFFFF)) {
443 ZCBOR_FAIL();
444 }
445
446 zcbor_print("list_count: %" PRIuFAST32 "\r\n", list_count);
447
448
449 /** If max_num is smaller than the actual number of encoded elements,
450 * the value_encode() below will corrupt the data if the encoded
451 * header is larger than the previously encoded header. */
452 if (header_len > max_header_len) {
453 zcbor_print("max_num too small.\r\n");
454 ZCBOR_ERR(ZCBOR_ERR_HIGH_ELEM_COUNT);
455 }
456
457 /* Reencode header of list now that we know the number of elements. */
458 if (!(value_encode(state, major_type, &list_count, sizeof(list_count)))) {
459 ZCBOR_FAIL();
460 }
461
462 if (max_header_len != header_len) {
463 const uint8_t *start = state->payload + max_header_len - header_len;
464 size_t body_size = (size_t)payload - (size_t)start;
465
466 memmove(state->payload_mut, start, body_size);
467 /* Reset payload pointer to end of list */
468 state->payload += body_size;
469 } else {
470 /* Reset payload pointer to end of list */
471 state->payload = payload;
472 }
473 #else
474 if (!encode_header_byte(state, ZCBOR_MAJOR_TYPE_PRIM, ZCBOR_VALUE_IS_INDEFINITE_LENGTH)) {
475 ZCBOR_FAIL();
476 }
477 #endif
478 return true;
479 }
480
481
482 bool zcbor_list_end_encode(zcbor_state_t *state, uint_fast32_t max_num)
483 {
484 return list_map_end_encode(state, max_num, ZCBOR_MAJOR_TYPE_LIST);
485 }
486
487
488 bool zcbor_map_end_encode(zcbor_state_t *state, uint_fast32_t max_num)
489 {
490 return list_map_end_encode(state, max_num, ZCBOR_MAJOR_TYPE_MAP);
491 }
492
493
494 bool zcbor_list_map_end_force_encode(zcbor_state_t *state)
495 {
496 #ifdef ZCBOR_CANONICAL
497 if (!zcbor_process_backup(state, ZCBOR_FLAG_RESTORE | ZCBOR_FLAG_CONSUME,
498 ZCBOR_MAX_ELEM_COUNT)) {
499 ZCBOR_FAIL();
500 }
501 #endif
502 return true;
503 }
504
505
506 bool zcbor_nil_put(zcbor_state_t *state, const void *unused)
507 {
508 (void)unused;
509 return primitive_put(state, 22);
510 }
511
512
513 bool zcbor_undefined_put(zcbor_state_t *state, const void *unused)
514 {
515 (void)unused;
516 return primitive_put(state, 23);
517 }
518
519
520 bool zcbor_bool_encode(zcbor_state_t *state, const bool *input)
521 {
522 if (!primitive_put(state, (uint32_t)(*input + ZCBOR_BOOL_TO_PRIM))) {
523 ZCBOR_FAIL();
524 }
525 return true;
526 }
527
528
529 bool zcbor_bool_put(zcbor_state_t *state, bool input)
530 {
531 if (!primitive_put(state, (uint32_t)(input + ZCBOR_BOOL_TO_PRIM))) {
532 ZCBOR_FAIL();
533 }
534 return true;
535 }
536
537
538 bool zcbor_float64_encode(zcbor_state_t *state, const double *input)
539 {
540 if (!value_encode_len(state, ZCBOR_MAJOR_TYPE_PRIM, input,
541 sizeof(*input))) {
542 ZCBOR_FAIL();
543 }
544
545 return true;
546 }
547
548
549 bool zcbor_float64_put(zcbor_state_t *state, double input)
550 {
551 return zcbor_float64_encode(state, &input);
552 }
553
554
555 bool zcbor_float32_encode(zcbor_state_t *state, const float *input)
556 {
557 if (!value_encode_len(state, ZCBOR_MAJOR_TYPE_PRIM, input,
558 sizeof(*input))) {
559 ZCBOR_FAIL();
560 }
561
562 return true;
563 }
564
565
566 bool zcbor_float32_put(zcbor_state_t *state, float input)
567 {
568 return zcbor_float32_encode(state, &input);
569 }
570
571
572 bool zcbor_tag_encode(zcbor_state_t *state, uint32_t tag)
573 {
574 if (!value_encode(state, ZCBOR_MAJOR_TYPE_TAG, &tag, sizeof(tag))) {
575 ZCBOR_FAIL();
576 }
577 state->elem_count--;
578
579 return true;
580 }
581
582
583 bool zcbor_multi_encode_minmax(uint_fast32_t min_encode,
584 uint_fast32_t max_encode,
585 const uint_fast32_t *num_encode,
586 zcbor_encoder_t encoder,
587 zcbor_state_t *state,
588 const void *input,
589 uint_fast32_t result_len)
590 {
591
592 if ((*num_encode >= min_encode) && (*num_encode <= max_encode)) {
593 return zcbor_multi_encode(*num_encode, encoder, state, input, result_len);
594 } else {
595 ZCBOR_ERR(ZCBOR_ERR_ITERATIONS);
596 }
597 }
598
599 bool zcbor_multi_encode(uint_fast32_t num_encode,
600 zcbor_encoder_t encoder,
601 zcbor_state_t *state,
602 const void *input,
603 uint_fast32_t result_len)
604 {
605 ZCBOR_CHECK_ERROR();
606 for (uint_fast32_t i = 0; i < num_encode; i++) {
607 if (!encoder(state, (const uint8_t *)input + i*result_len)) {
608 ZCBOR_FAIL();
609 }
610 }
611 zcbor_print("Encoded %" PRIuFAST32 " elements.\n", num_encode);
612 return true;
613 }
614
615
616 bool zcbor_present_encode(const uint_fast32_t *present,
617 zcbor_encoder_t encoder,
618 zcbor_state_t *state,
619 const void *input)
620 {
621 return zcbor_multi_encode(!!*present, encoder, state, input, 0);
622 }
623
624
625 void zcbor_new_encode_state(zcbor_state_t *state_array, uint_fast32_t n_states,
626 uint8_t *payload, size_t payload_len, uint_fast32_t elem_count)
627 {
628 zcbor_new_state(state_array, n_states, payload, payload_len, elem_count);
629 }
630
