1 /*
2 * Copyright (c) 2021 BayLibre, SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <errno.h>
8 #include <stdarg.h>
9 #include <stdint.h>
10 #include <string.h>
11 #include <zephyr/toolchain.h>
12 #include <zephyr/linker/utils.h>
13 #include <zephyr/sys/cbprintf.h>
14 #include <sys/types.h>
15 #include <zephyr/sys/util.h>
16 #include <zephyr/sys/__assert.h>
17 #include <zephyr/logging/log.h>
18 LOG_MODULE_REGISTER(cbprintf_package, CONFIG_CBPRINTF_PACKAGE_LOG_LEVEL);
19
20 #if defined(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS) && \
21 !Z_C_GENERIC
22 #error "CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS " \
23 "requires toolchain to support _Generic!"
24 #endif
25
26 /**
27 * @brief Check if address is in read only section.
28 *
29 * @param addr Address.
30 *
31 * @return True if address identified within read only section.
32 */
ptr_in_rodata(const char * addr)33 static inline bool ptr_in_rodata(const char *addr)
34 {
35 #if defined(CBPRINTF_VIA_UNIT_TEST)
36 /* Unit test is X86 (or other host) but not using Zephyr
37 * linker scripts.
38 */
39 return false;
40 #else
41 return linker_is_in_rodata(addr);
42 #endif
43 }
44
45 /*
46 * va_list creation
47 */
48
49 #if defined(__CHECKER__)
cbprintf_via_va_list(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,const char * fmt,void * buf)50 static int cbprintf_via_va_list(cbprintf_cb out,
51 cbvprintf_external_formatter_func formatter,
52 void *ctx,
53 const char *fmt, void *buf)
54 {
55 return 0;
56 }
57 #elif defined(__aarch64__)
58 /*
59 * Reference:
60 *
61 * Procedure Call Standard for the ARM 64-bit Architecture
62 */
63
64 struct __va_list {
65 void *__stack;
66 void *__gr_top;
67 void *__vr_top;
68 int __gr_offs;
69 int __vr_offs;
70 };
71
72 BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
73 "architecture specific support is wrong");
74
cbprintf_via_va_list(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,const char * fmt,void * buf)75 static int cbprintf_via_va_list(cbprintf_cb out,
76 cbvprintf_external_formatter_func formatter,
77 void *ctx,
78 const char *fmt, void *buf)
79 {
80 union {
81 va_list ap;
82 struct __va_list __ap;
83 } u;
84
85 /* create a valid va_list with our buffer */
86 u.__ap.__stack = buf;
87 u.__ap.__gr_top = NULL;
88 u.__ap.__vr_top = NULL;
89 u.__ap.__gr_offs = 0;
90 u.__ap.__vr_offs = 0;
91
92 return formatter(out, ctx, fmt, u.ap);
93 }
94
95 #elif defined(__x86_64__)
96 /*
97 * Reference:
98 *
99 * System V Application Binary Interface
100 * AMD64 Architecture Processor Supplement
101 */
102
103 struct __va_list {
104 unsigned int gp_offset;
105 unsigned int fp_offset;
106 void *overflow_arg_area;
107 void *reg_save_area;
108 };
109
110 BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
111 "architecture specific support is wrong");
112
cbprintf_via_va_list(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,const char * fmt,void * buf)113 static int cbprintf_via_va_list(cbprintf_cb out,
114 cbvprintf_external_formatter_func formatter,
115 void *ctx,
116 const char *fmt, void *buf)
117 {
118 union {
119 va_list ap;
120 struct __va_list __ap;
121 } u;
122
123 /* create a valid va_list with our buffer */
124 u.__ap.overflow_arg_area = buf;
125 u.__ap.reg_save_area = NULL;
126 u.__ap.gp_offset = (6 * 8);
127 u.__ap.fp_offset = (6 * 8 + 16 * 16);
128
129 return formatter(out, ctx, fmt, u.ap);
130 }
131
132 #elif defined(__xtensa__)
133 /*
134 * Reference:
135 *
136 * gcc source code (gcc/config/xtensa/xtensa.c)
137 * xtensa_build_builtin_va_list(), xtensa_va_start(),
138 * xtensa_gimplify_va_arg_expr()
139 */
140
141 struct __va_list {
142 void *__va_stk;
143 void *__va_reg;
144 int __va_ndx;
145 };
146
147 BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
148 "architecture specific support is wrong");
149
cbprintf_via_va_list(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,const char * fmt,void * buf)150 static int cbprintf_via_va_list(cbprintf_cb out,
151 cbvprintf_external_formatter_func formatter,
152 void *ctx,
153 const char *fmt, void *buf)
154 {
155 union {
156 va_list ap;
157 struct __va_list __ap;
158 } u;
159
160 /* create a valid va_list with our buffer */
161 u.__ap.__va_stk = (char *)buf - 32;
162 u.__ap.__va_reg = NULL;
163 u.__ap.__va_ndx = (6 + 2) * 4;
164
165 return formatter(out, ctx, fmt, u.ap);
166 }
167
168 #else
169 /*
170 * Default implementation shared by many architectures like
171 * 32-bit ARM and Intel.
172 *
173 * We assume va_list is a simple pointer.
174 */
175
176 BUILD_ASSERT(sizeof(va_list) == sizeof(void *),
177 "architecture specific support is needed");
178
cbprintf_via_va_list(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,const char * fmt,void * buf)179 static int cbprintf_via_va_list(cbprintf_cb out,
180 cbvprintf_external_formatter_func formatter,
181 void *ctx,
182 const char *fmt, void *buf)
183 {
184 union {
185 va_list ap;
186 void *ptr;
187 } u;
188
189 u.ptr = buf;
190
191 return formatter(out, ctx, fmt, u.ap);
192 }
193
194 #endif
195
get_package_len(void * packaged)196 static size_t get_package_len(void *packaged)
197 {
198 __ASSERT_NO_MSG(packaged != NULL);
199
200 uint8_t *buf = packaged;
201 uint8_t *start = buf;
202 unsigned int args_size, s_nbr, ros_nbr;
203
204 args_size = buf[0] * sizeof(int);
205 s_nbr = buf[1];
206 ros_nbr = buf[2];
207
208 /* Move beyond args. */
209 buf += args_size;
210
211 /* Move beyond read-only string indexes array. */
212 buf += ros_nbr;
213
214 /* Move beyond strings appended to the package. */
215 for (unsigned int i = 0; i < s_nbr; i++) {
216 buf++;
217 buf += strlen((const char *)buf) + 1;
218 }
219
220 return (size_t)(uintptr_t)(buf - start);
221 }
222
append_string(cbprintf_convert_cb cb,void * ctx,const char * str,uint16_t strl)223 static int append_string(cbprintf_convert_cb cb, void *ctx, const char *str, uint16_t strl)
224 {
225 if (cb == NULL) {
226 return 1 + strlen(str);
227 }
228
229 strl = strl > 0 ? strl : strlen(str) + 1;
230 return cb(str, strl, ctx);
231 }
232
cbvprintf_package(void * packaged,size_t len,uint32_t flags,const char * fmt,va_list ap)233 int cbvprintf_package(void *packaged, size_t len, uint32_t flags,
234 const char *fmt, va_list ap)
235 {
236 /*
237 * Internally, a byte is used to store location of a string argument within a
238 * package. MSB bit is set if string is read-only so effectively 7 bits are
239 * used for index, which should be enough.
240 */
241 #define STR_POS_RO_FLAG BIT(7)
242 #define STR_POS_MASK BIT_MASK(7)
243
244 /* Buffer offset abstraction for better code clarity. */
245 #define BUF_OFFSET (buf - (uintptr_t)buf0)
246
247 uint8_t *buf0 = packaged; /* buffer start (may be NULL) */
248 uintptr_t buf = (uintptr_t)buf0; /* current buffer position */
249 unsigned int size; /* current argument's size */
250 unsigned int align; /* current argument's required alignment */
251 uint8_t str_ptr_pos[16]; /* string pointer positions */
252 uint8_t str_ptr_arg[16]; /* string pointer argument index */
253 unsigned int s_idx = 0; /* index into str_ptr_pos[] */
254 unsigned int s_rw_cnt = 0; /* number of rw strings */
255 unsigned int s_ro_cnt = 0; /* number of ro strings */
256 int arg_idx = -1; /* Argument index. Preincremented thus starting from -1.*/
257 unsigned int i;
258 const char *s;
259 bool parsing = false;
260 /* Flag indicates that rw strings are stored as array with positions,
261 * instead of appending them to the package.
262 */
263 bool rws_pos_en = !!(flags & CBPRINTF_PACKAGE_ADD_RW_STR_POS);
264 /* Get number of first read only strings present in the string.
265 * There is always at least 1 (fmt) but flags can indicate more, e.g
266 * fixed prefix appended to all strings.
267 */
268 int fros_cnt = 1 + Z_CBPRINTF_PACKAGE_FIRST_RO_STR_CNT_GET(flags);
269 bool is_str_arg = false;
270 union cbprintf_package_hdr *pkg_hdr = packaged;
271
272 /* Buffer must be aligned at least to size of a pointer. */
273 if ((uintptr_t)packaged % sizeof(void *)) {
274 return -EFAULT;
275 }
276
277 #if defined(__xtensa__)
278 /* Xtensa requires package to be 16 bytes aligned. */
279 if ((uintptr_t)packaged % CBPRINTF_PACKAGE_ALIGNMENT) {
280 return -EFAULT;
281 }
282 #endif
283
284 /*
285 * Make room to store the arg list size, the number of
286 * appended writable strings and the number of appended
287 * read-only strings. They both occupy 1 byte each.
288 * Skip a byte. Then a uint32_t to store flags used to
289 * create the package.
290 *
291 * Given the next value to store is the format string pointer
292 * which is guaranteed to be at least 4 bytes, we just reserve
293 * multiple of pointer size for the above to preserve alignment.
294 *
295 * Refer to union cbprintf_package_hdr for more details.
296 */
297 buf += sizeof(*pkg_hdr);
298
299 /*
300 * When buf0 is NULL we don't store anything.
301 * Instead we count the needed space to store the data.
302 * In this case, incoming len argument indicates the anticipated
303 * buffer "misalignment" offset.
304 */
305 if (buf0 == NULL) {
306 buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
307 /*
308 * The space to store the data is represented by both the
309 * buffer offset as well as the extra string data to be
310 * appended. When only figuring out the needed space, we
311 * don't append anything. Instead, we reuse the len variable
312 * to sum the size of that data.
313 *
314 * Also, we subtract any initial misalignment offset from
315 * the total as this won't be part of the buffer. To avoid
316 * going negative with an unsigned variable, we add an offset
317 * (CBPRINTF_PACKAGE_ALIGNMENT) that will be removed before
318 * returning.
319 */
320 len = CBPRINTF_PACKAGE_ALIGNMENT - (len % CBPRINTF_PACKAGE_ALIGNMENT);
321 }
322
323 /*
324 * Otherwise we must ensure we can store at least
325 * the pointer to the format string itself.
326 */
327 if ((buf0 != NULL) && (BUF_OFFSET + sizeof(char *)) > len) {
328 return -ENOSPC;
329 }
330
331 /*
332 * Then process the format string itself.
333 * Here we branch directly into the code processing strings
334 * which is in the middle of the following while() loop. That's the
335 * reason for the post-decrement on fmt as it will be incremented
336 * prior to the next (actually first) round of that loop.
337 */
338 s = fmt;
339 --fmt;
340 align = VA_STACK_ALIGN(char *);
341 size = sizeof(char *);
342 goto process_string;
343
344 while (true) {
345
346 #if defined(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS)
347 if ((flags & CBPRINTF_PACKAGE_ARGS_ARE_TAGGED)
348 == CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) {
349 int arg_tag = va_arg(ap, int);
350
351 /*
352 * Here we copy the tag over to the package.
353 */
354 align = VA_STACK_ALIGN(int);
355 size = sizeof(int);
356
357 /* align destination buffer location */
358 buf = ROUND_UP(buf, align);
359
360 /* make sure the data fits */
361 if (buf0 != NULL && BUF_OFFSET + size > len) {
362 return -ENOSPC;
363 }
364
365 if (buf0 != NULL) {
366 *(int *)buf = arg_tag;
367 }
368
369 buf += sizeof(int);
370
371 if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_END) {
372 /* End of arguments */
373 break;
374 }
375
376 /*
377 * There are lots of __fallthrough here since
378 * quite a few of the data types have the same
379 * storage size.
380 */
381 switch (arg_tag) {
382 case CBPRINTF_PACKAGE_ARG_TYPE_CHAR:
383 __fallthrough;
384 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_CHAR:
385 __fallthrough;
386 case CBPRINTF_PACKAGE_ARG_TYPE_SHORT:
387 __fallthrough;
388 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_SHORT:
389 __fallthrough;
390 case CBPRINTF_PACKAGE_ARG_TYPE_INT:
391 __fallthrough;
392 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_INT:
393 align = VA_STACK_ALIGN(int);
394 size = sizeof(int);
395 break;
396
397 case CBPRINTF_PACKAGE_ARG_TYPE_LONG:
398 __fallthrough;
399 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG:
400 align = VA_STACK_ALIGN(long);
401 size = sizeof(long);
402 break;
403
404 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_LONG:
405 __fallthrough;
406 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG_LONG:
407 align = VA_STACK_ALIGN(long long);
408 size = sizeof(long long);
409 break;
410
411 case CBPRINTF_PACKAGE_ARG_TYPE_FLOAT:
412 __fallthrough;
413 case CBPRINTF_PACKAGE_ARG_TYPE_DOUBLE:
414 __fallthrough;
415 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE: {
416 /*
417 * Handle floats separately as they may be
418 * held in a different register set.
419 */
420 union { double d; long double ld; } v;
421
422 if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE) {
423 v.ld = va_arg(ap, long double);
424 align = VA_STACK_ALIGN(long double);
425 size = sizeof(long double);
426 } else {
427 v.d = va_arg(ap, double);
428 align = VA_STACK_ALIGN(double);
429 size = sizeof(double);
430 }
431
432 /* align destination buffer location */
433 buf = ROUND_UP(buf, align);
434 if (buf0 != NULL) {
435 /* make sure it fits */
436 if ((BUF_OFFSET + size) > len) {
437 return -ENOSPC;
438 }
439 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
440 memcpy((void *)buf, (uint8_t *)&v, size);
441 } else if (fmt[-1] == 'L') {
442 *(long double *)buf = v.ld;
443 } else {
444 *(double *)buf = v.d;
445 }
446 }
447 buf += size;
448 parsing = false;
449 continue;
450 }
451
452 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_CHAR:
453 is_str_arg = true;
454
455 __fallthrough;
456 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_VOID:
457 align = VA_STACK_ALIGN(void *);
458 size = sizeof(void *);
459 break;
460
461 default:
462 return -EINVAL;
463 }
464
465 } else
466 #endif /* CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS */
467 {
468 /* Scan the format string */
469 if (*++fmt == '\0') {
470 break;
471 }
472
473 if (!parsing) {
474 if (*fmt == '%') {
475 parsing = true;
476 arg_idx++;
477 align = VA_STACK_ALIGN(int);
478 size = sizeof(int);
479 }
480 continue;
481 }
482 switch (*fmt) {
483 case '%':
484 parsing = false;
485 arg_idx--;
486 continue;
487
488 case '#':
489 case '-':
490 case '+':
491 case ' ':
492 case '0':
493 case '1':
494 case '2':
495 case '3':
496 case '4':
497 case '5':
498 case '6':
499 case '7':
500 case '8':
501 case '9':
502 case '.':
503 case 'h':
504 case 'l':
505 case 'L':
506 continue;
507
508 case '*':
509 break;
510
511 case 'j':
512 align = VA_STACK_ALIGN(intmax_t);
513 size = sizeof(intmax_t);
514 continue;
515
516 case 'z':
517 align = VA_STACK_ALIGN(size_t);
518 size = sizeof(size_t);
519 continue;
520
521 case 't':
522 align = VA_STACK_ALIGN(ptrdiff_t);
523 size = sizeof(ptrdiff_t);
524 continue;
525
526 case 'c':
527 case 'd':
528 case 'i':
529 case 'o':
530 case 'u':
531 case 'x':
532 case 'X':
533 if (fmt[-1] == 'l') {
534 if (fmt[-2] == 'l') {
535 align = VA_STACK_ALIGN(long long);
536 size = sizeof(long long);
537 } else {
538 align = VA_STACK_ALIGN(long);
539 size = sizeof(long);
540 }
541 }
542 parsing = false;
543 break;
544
545 case 's':
546 is_str_arg = true;
547
548 __fallthrough;
549 case 'p':
550 case 'n':
551 align = VA_STACK_ALIGN(void *);
552 size = sizeof(void *);
553 parsing = false;
554 break;
555
556 case 'a':
557 case 'A':
558 case 'e':
559 case 'E':
560 case 'f':
561 case 'F':
562 case 'g':
563 case 'G': {
564 /*
565 * Handle floats separately as they may be
566 * held in a different register set.
567 */
568 union { double d; long double ld; } v;
569
570 if (fmt[-1] == 'L') {
571 v.ld = va_arg(ap, long double);
572 align = VA_STACK_ALIGN(long double);
573 size = sizeof(long double);
574 } else {
575 v.d = va_arg(ap, double);
576 align = VA_STACK_ALIGN(double);
577 size = sizeof(double);
578 }
579 /* align destination buffer location */
580 buf = ROUND_UP(buf, align);
581 if (buf0 != NULL) {
582 /* make sure it fits */
583 if (BUF_OFFSET + size > len) {
584 return -ENOSPC;
585 }
586 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
587 memcpy((void *)buf, (uint8_t *)&v, size);
588 } else if (fmt[-1] == 'L') {
589 *(long double *)buf = v.ld;
590 } else {
591 *(double *)buf = v.d;
592 }
593 }
594 buf += size;
595 parsing = false;
596 continue;
597 }
598
599 default:
600 parsing = false;
601 continue;
602 }
603 }
604
605 /* align destination buffer location */
606 buf = ROUND_UP(buf, align);
607
608 /* make sure the data fits */
609 if ((buf0 != NULL) && (BUF_OFFSET + size) > len) {
610 return -ENOSPC;
611 }
612
613 /* copy va_list data over to our buffer */
614 if (is_str_arg) {
615 s = va_arg(ap, char *);
616 process_string:
617 if (buf0 != NULL) {
618 *(const char **)buf = s;
619 }
620
621 bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
622 bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
623
624 if (is_ro && !do_ro) {
625 /* nothing to do */
626 } else {
627 uint32_t s_ptr_idx = BUF_OFFSET / sizeof(int);
628
629 /*
630 * In the do_ro case we must consider
631 * room for possible STR_POS_RO_FLAG.
632 * Otherwise the index range is 8 bits
633 * and any overflow is caught later.
634 */
635 if (do_ro && s_ptr_idx > STR_POS_MASK) {
636 __ASSERT(false, "String with too many arguments");
637 return -EINVAL;
638 }
639
640 if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
641 __ASSERT(false, "str_ptr_pos[] too small");
642 return -EINVAL;
643 }
644
645 if (buf0 != NULL) {
646 /*
647 * Remember string pointer location.
648 * We will append non-ro strings later.
649 */
650 str_ptr_pos[s_idx] = s_ptr_idx;
651 str_ptr_arg[s_idx] = arg_idx;
652 if (is_ro) {
653 /* flag read-only string. */
654 str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
655 s_ro_cnt++;
656 } else {
657 s_rw_cnt++;
658 }
659 } else if (is_ro) {
660 /*
661 * Add only pointer position prefix
662 * when counting strings.
663 */
664 len += 1;
665 } else if (rws_pos_en) {
666 /*
667 * Add only pointer position prefix and
668 * argument index when counting strings.
669 */
670 len += 2;
671 } else {
672 /*
673 * Add the string length, the final '\0'
674 * and size of the pointer position prefix.
675 */
676 len += strlen(s) + 1 + 1;
677 }
678
679 s_idx++;
680 }
681 buf += sizeof(char *);
682
683 is_str_arg = false;
684 } else if (size == sizeof(int)) {
685 int v = va_arg(ap, int);
686
687 if (buf0 != NULL) {
688 *(int *)buf = v;
689 }
690 buf += sizeof(int);
691 } else if (size == sizeof(long)) {
692 long v = va_arg(ap, long);
693
694 if (buf0 != NULL) {
695 *(long *)buf = v;
696 }
697 buf += sizeof(long);
698 } else if (size == sizeof(long long)) {
699 long long v = va_arg(ap, long long);
700
701 if (buf0 != NULL) {
702 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
703 memcpy((void *)buf, (uint8_t *)&v, sizeof(long long));
704 } else {
705 *(long long *)buf = v;
706 }
707 }
708 buf += sizeof(long long);
709 } else {
710 __ASSERT(false, "unexpected size %u", size);
711 return -EINVAL;
712 }
713 }
714
715 /*
716 * We remember the size of the argument list as a multiple of
717 * sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
718 * worth of va_list, or about 127 arguments on a 64-bit system
719 * (twice that on 32-bit systems). That ought to be good enough.
720 */
721 if ((BUF_OFFSET / sizeof(int)) > 255) {
722 __ASSERT(false, "too many format args");
723 return -EINVAL;
724 }
725
726 /*
727 * If all we wanted was to count required buffer size
728 * then we have it now.
729 */
730 if (buf0 == NULL) {
731 return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
732 }
733
734 /* Clear our buffer header. We made room for it initially. */
735 *(char **)buf0 = NULL;
736
737 /* Record end of argument list. */
738 pkg_hdr->desc.len = BUF_OFFSET / sizeof(int);
739
740 if (rws_pos_en) {
741 /* Strings are appended, update location counter. */
742 pkg_hdr->desc.str_cnt = 0;
743 pkg_hdr->desc.rw_str_cnt = s_rw_cnt;
744 } else {
745 /* Strings are appended, update append counter. */
746 pkg_hdr->desc.str_cnt = s_rw_cnt;
747 pkg_hdr->desc.rw_str_cnt = 0;
748 }
749
750 pkg_hdr->desc.ro_str_cnt = s_ro_cnt;
751
752 #ifdef CONFIG_CBPRINTF_PACKAGE_HEADER_STORE_CREATION_FLAGS
753 pkg_hdr->desc.pkg_flags = flags;
754 #endif
755
756 /* Store strings pointer locations of read only strings. */
757 if (s_ro_cnt != 0U) {
758 for (i = 0; i < s_idx; i++) {
759 if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
760 continue;
761 }
762
763 uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
764
765 /* make sure it fits */
766 if ((BUF_OFFSET + 1) > len) {
767 return -ENOSPC;
768 }
769 /* store the pointer position prefix */
770 *(uint8_t *)buf = pos;
771 ++buf;
772 }
773 }
774
775 /* Store strings prefixed by their pointer location. */
776 for (i = 0; i < s_idx; i++) {
777 /* Process only RW strings. */
778 if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
779 continue;
780 }
781
782 if (rws_pos_en) {
783 size = 0;
784 *(uint8_t *)buf = str_ptr_arg[i];
785 ++buf;
786 } else {
787 /* retrieve the string pointer */
788 s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
789 /* clear the in-buffer pointer (less entropy if compressed) */
790 *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
791 /* find the string length including terminating '\0' */
792 size = strlen(s) + 1;
793 }
794
795 /* make sure it fits */
796 if ((BUF_OFFSET + 1 + size) > len) {
797 return -ENOSPC;
798 }
799 /* store the pointer position prefix */
800 *(uint8_t *)buf = str_ptr_pos[i];
801 ++buf;
802 /* copy the string with its terminating '\0' */
803 memcpy((void *)buf, (uint8_t *)s, size);
804 buf += size;
805 }
806
807 /*
808 * TODO: remove pointers for appended strings since they're useless.
809 * TODO: explore leveraging same mechanism to remove alignment padding
810 */
811
812 return BUF_OFFSET;
813
814 #undef BUF_OFFSET
815 #undef STR_POS_RO_FLAG
816 #undef STR_POS_MASK
817 }
818
cbprintf_package(void * packaged,size_t len,uint32_t flags,const char * format,...)819 int cbprintf_package(void *packaged, size_t len, uint32_t flags,
820 const char *format, ...)
821 {
822 va_list ap;
823 int ret;
824
825 va_start(ap, format);
826 ret = cbvprintf_package(packaged, len, flags, format, ap);
827 va_end(ap);
828 return ret;
829 }
830
cbpprintf_external(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,void * packaged)831 int cbpprintf_external(cbprintf_cb out,
832 cbvprintf_external_formatter_func formatter,
833 void *ctx, void *packaged)
834 {
835 uint8_t *buf = packaged;
836 struct cbprintf_package_hdr_ext *hdr = packaged;
837 char *s, **ps;
838 unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
839
840 if (buf == NULL) {
841 return -EINVAL;
842 }
843
844 /* Retrieve the size of the arg list and number of strings. */
845 args_size = hdr->hdr.desc.len * sizeof(int);
846 s_nbr = hdr->hdr.desc.str_cnt;
847 ros_nbr = hdr->hdr.desc.ro_str_cnt;
848 rws_nbr = hdr->hdr.desc.rw_str_cnt;
849
850 /* Locate the string table */
851 s = (char *)(buf + args_size + ros_nbr + 2 * rws_nbr);
852
853 /*
854 * Patch in string pointers.
855 */
856 for (i = 0; i < s_nbr; i++) {
857 /* Locate pointer location for this string */
858 s_idx = *(uint8_t *)s;
859 ++s;
860 ps = (char **)(buf + s_idx * sizeof(int));
861 /* update the pointer with current string location */
862 *ps = s;
863 /* move to next string */
864 s += strlen(s) + 1;
865 }
866
867 /* Skip past the header */
868 buf += sizeof(*hdr);
869
870 /* Turn this into a va_list and print it */
871 return cbprintf_via_va_list(out, formatter, ctx, hdr->fmt, buf);
872 }
873
874 /* Function checks if character might be format specifier. Check is relaxed since
875 * compiler ensures that correct format specifier is used so it is enough to check
876 * that character is not one of potential modifier (e.g. number, dot, etc.).
877 */
is_fmt_spec(char c)878 static bool is_fmt_spec(char c)
879 {
880 return (c >= 64) && (c <= 122);
881 }
882
883 /* Function checks if nth argument is a pointer (%p). Returns true is yes. Returns
884 * false if not or if string does not have nth argument.
885 */
is_ptr(const char * fmt,int n)886 bool is_ptr(const char *fmt, int n)
887 {
888 char c;
889 bool mod = false;
890 int cnt = 0;
891
892 while ((c = *fmt++) != '\0') {
893 if (mod) {
894 if (cnt == n) {
895 if (c == 'p') {
896 return true;
897 } else if (is_fmt_spec(c)) {
898 return false;
899 }
900 } else if (is_fmt_spec(c)) {
901 cnt++;
902 mod = false;
903 }
904 }
905 if (c == '%') {
906 mod = !mod;
907 }
908 }
909
910 return false;
911 }
912
cbprintf_package_convert(void * in_packaged,size_t in_len,cbprintf_convert_cb cb,void * ctx,uint32_t flags,uint16_t * strl,size_t strl_len)913 int cbprintf_package_convert(void *in_packaged,
914 size_t in_len,
915 cbprintf_convert_cb cb,
916 void *ctx,
917 uint32_t flags,
918 uint16_t *strl,
919 size_t strl_len)
920 {
921 __ASSERT_NO_MSG(in_packaged != NULL);
922
923 uint8_t *buf = in_packaged;
924 uint32_t *buf32 = in_packaged;
925 unsigned int args_size, ros_nbr, rws_nbr;
926 bool fmt_present = flags & CBPRINTF_PACKAGE_CONVERT_PTR_CHECK ? true : false;
927 bool rw_cpy;
928 bool ro_cpy;
929 struct cbprintf_package_desc *in_desc = in_packaged;
930
931 in_len = in_len != 0 ? in_len : get_package_len(in_packaged);
932
933 /* Get number of RO string indexes in the package and check if copying
934 * includes appending those strings.
935 */
936 ros_nbr = in_desc->ro_str_cnt;
937 ro_cpy = ros_nbr &&
938 (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) == CBPRINTF_PACKAGE_CONVERT_RO_STR;
939
940 /* Get number of RW string indexes in the package and check if copying
941 * includes appending those strings.
942 */
943 rws_nbr = in_desc->rw_str_cnt;
944 rw_cpy = rws_nbr > 0 &&
945 (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) == CBPRINTF_PACKAGE_CONVERT_RW_STR;
946
947 /* If flags are not set or appending request without rw string indexes
948 * present is chosen, just do a simple copy (or length calculation).
949 * Assuming that it is the most common case.
950 */
951 if (!rw_cpy && !ro_cpy) {
952 if (cb) {
953 cb(in_packaged, in_len, ctx);
954 }
955
956 return in_len;
957 }
958
959 /* If we got here, it means that coping will be more complex and will be
960 * done with strings appending.
961 * Retrieve the size of the arg list.
962 */
963 args_size = in_desc->len * sizeof(int);
964
965 int out_len;
966
967 /* Pointer to array with string locations. Array starts with read-only
968 * string locations.
969 */
970 const char *fmt = *(const char **)(buf + sizeof(void *));
971 uint8_t *str_pos = &buf[args_size];
972 size_t strl_cnt = 0;
973
974 /* If null destination, just calculate output length. */
975 if (cb == NULL) {
976 out_len = (int)in_len;
977 if (ro_cpy) {
978 for (unsigned int i = 0; i < ros_nbr; i++) {
979 const char *str = *(const char **)&buf32[*str_pos];
980 int len = append_string(cb, NULL, str, 0);
981
982 /* If possible store calculated string length. */
983 if (strl && strl_cnt < strl_len) {
984 strl[strl_cnt++] = (uint16_t)len;
985 }
986 out_len += len;
987 str_pos++;
988 }
989 } else {
990 str_pos += ros_nbr;
991 }
992
993 bool drop_ro_str_pos = !(flags &
994 (CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR |
995 CBPRINTF_PACKAGE_CONVERT_RO_STR));
996
997 /* Handle RW strings. */
998 for (unsigned int i = 0; i < rws_nbr; i++) {
999 uint8_t arg_idx = *str_pos++;
1000 uint8_t arg_pos = *str_pos++;
1001 const char *str = *(const char **)&buf32[arg_pos];
1002 bool is_ro = ptr_in_rodata(str);
1003 int len;
1004
1005 if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1006 fmt_present && is_ptr(fmt, arg_idx)) {
1007 LOG_WRN("(unsigned) char * used for %%p argument. "
1008 "It's recommended to cast it to void * because "
1009 "it may cause misbehavior in certain "
1010 "configurations. String:\"%s\" argument:%d", fmt, arg_idx);
1011 /* Since location is being dropped, decrement
1012 * output length by 2 (argument index + position)
1013 */
1014 out_len -= 2;
1015 continue;
1016 }
1017
1018 if (is_ro) {
1019 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1020 goto calculate_string_length;
1021 } else {
1022 out_len -= drop_ro_str_pos ? 2 : 1;
1023 }
1024 } else if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1025 calculate_string_length:
1026 len = append_string(cb, NULL, str, 0);
1027
1028 /* If possible store calculated string length. */
1029 if (strl && strl_cnt < strl_len) {
1030 strl[strl_cnt++] = (uint16_t)len;
1031 }
1032 /* string length decremented by 1 because argument
1033 * index is dropped.
1034 */
1035 out_len += (len - 1);
1036 }
1037 }
1038
1039 return out_len;
1040 }
1041
1042 struct cbprintf_package_desc out_desc;
1043 /* At least one is copied in. */
1044 uint8_t cpy_str_pos[16];
1045 /* Up to one will be kept since if both types are kept it returns earlier. */
1046 uint8_t keep_str_pos[16];
1047 uint8_t scpy_cnt;
1048 uint8_t keep_cnt;
1049 uint8_t *dst;
1050 int rv;
1051
1052 /* If read-only strings shall be appended to the output package copy
1053 * their indexes to the local array, otherwise indicate that indexes
1054 * shall remain in the output package.
1055 */
1056 if (ro_cpy) {
1057 scpy_cnt = ros_nbr;
1058 keep_cnt = 0;
1059 dst = cpy_str_pos;
1060 } else if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1061 scpy_cnt = 0;
1062 keep_cnt = ros_nbr;
1063 dst = keep_str_pos;
1064 } else {
1065 scpy_cnt = 0;
1066 keep_cnt = 0;
1067 dst = NULL;
1068 }
1069 if (dst) {
1070 memcpy(dst, str_pos, ros_nbr);
1071 }
1072 str_pos += ros_nbr;
1073
1074 /* Go through read-write strings and identify which shall be appended.
1075 * Note that there may be read-only strings there. Use address evaluation
1076 * to determine if strings is read-only.
1077 */
1078 for (unsigned int i = 0; i < rws_nbr; i++) {
1079 uint8_t arg_idx = *str_pos++;
1080 uint8_t arg_pos = *str_pos++;
1081 const char *str = *(const char **)&buf32[arg_pos];
1082 bool is_ro = ptr_in_rodata(str);
1083
1084 if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1085 fmt_present && is_ptr(fmt, arg_idx)) {
1086 continue;
1087 }
1088
1089 if (is_ro) {
1090 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1091 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1092 cpy_str_pos[scpy_cnt++] = arg_pos;
1093 } else if (flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1094 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1095 keep_str_pos[keep_cnt++] = arg_pos;
1096 } else {
1097 /* Drop information about ro_str location. */
1098 }
1099 } else {
1100 if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1101 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1102 cpy_str_pos[scpy_cnt++] = arg_pos;
1103 } else {
1104 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1105 keep_str_pos[keep_cnt++] = arg_idx;
1106 keep_str_pos[keep_cnt++] = arg_pos;
1107 }
1108 }
1109 }
1110
1111 /* Set amount of strings appended to the package. */
1112 out_desc.len = in_desc->len;
1113 out_desc.str_cnt = in_desc->str_cnt + scpy_cnt;
1114 out_desc.rw_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) ? 0 : (keep_cnt / 2);
1115 out_desc.ro_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) ? 0 :
1116 ((flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) ? keep_cnt : 0);
1117
1118 /* Temporary overwrite input descriptor to allow bulk transfer */
1119 struct cbprintf_package_desc in_desc_backup = *in_desc;
1120 *in_desc = out_desc;
1121
1122 /* Copy package header and arguments. */
1123 rv = cb(in_packaged, args_size, ctx);
1124 if (rv < 0) {
1125 return rv;
1126 }
1127 out_len = rv;
1128 /* Restore input descriptor. */
1129 *in_desc = in_desc_backup;
1130
1131 /* Copy string positions which are kept. */
1132 rv = cb(keep_str_pos, keep_cnt, ctx);
1133 if (rv < 0) {
1134 return rv;
1135 }
1136 out_len += rv;
1137
1138 /* Copy appended strings from source package to destination. */
1139 size_t strs_len = in_len - (args_size + ros_nbr + 2 * rws_nbr);
1140
1141 rv = cb(str_pos, strs_len, ctx);
1142 if (rv < 0) {
1143 return rv;
1144 }
1145 out_len += rv;
1146
1147 /* Append strings */
1148 for (unsigned int i = 0; i < scpy_cnt; i++) {
1149 uint8_t loc = cpy_str_pos[i];
1150 const char *str = *(const char **)&buf32[loc];
1151 uint16_t str_len = (strl && (i < strl_len)) ? strl[i] : 0;
1152
1153 rv = cb(&loc, 1, ctx);
1154 if (rv < 0) {
1155 return rv;
1156 }
1157 out_len += rv;
1158
1159 rv = append_string(cb, ctx, str, str_len);
1160 if (rv < 0) {
1161 return rv;
1162 }
1163 out_len += rv;
1164 }
1165
1166 /* Empty call (can be interpreted as flushing) */
1167 (void)cb(NULL, 0, ctx);
1168
1169 return out_len;
1170 }
1171
