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