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 ((uintptr_t)buf - (uintptr_t)buf0)
246
247 uint8_t *buf0 = packaged; /* buffer start (may be NULL) */
248 uint8_t *buf = 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 = (void *)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 = (void *) 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(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 = (void *) 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(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 = (void *) 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(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 *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 *buf++ = str_ptr_arg[i];
785 } else {
786 /* retrieve the string pointer */
787 s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
788 /* clear the in-buffer pointer (less entropy if compressed) */
789 *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
790 /* find the string length including terminating '\0' */
791 size = strlen(s) + 1;
792 }
793
794 /* make sure it fits */
795 if ((BUF_OFFSET + 1 + size) > len) {
796 return -ENOSPC;
797 }
798 /* store the pointer position prefix */
799 *buf = str_ptr_pos[i];
800 ++buf;
801 /* copy the string with its terminating '\0' */
802 memcpy(buf, (uint8_t *)s, size);
803 buf += size;
804 }
805
806 /*
807 * TODO: remove pointers for appended strings since they're useless.
808 * TODO: explore leveraging same mechanism to remove alignment padding
809 */
810
811 return BUF_OFFSET;
812
813 #undef BUF_OFFSET
814 #undef STR_POS_RO_FLAG
815 #undef STR_POS_MASK
816 }
817
cbprintf_package(void * packaged,size_t len,uint32_t flags,const char * format,...)818 int cbprintf_package(void *packaged, size_t len, uint32_t flags,
819 const char *format, ...)
820 {
821 va_list ap;
822 int ret;
823
824 va_start(ap, format);
825 ret = cbvprintf_package(packaged, len, flags, format, ap);
826 va_end(ap);
827 return ret;
828 }
829
cbpprintf_external(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,void * packaged)830 int cbpprintf_external(cbprintf_cb out,
831 cbvprintf_external_formatter_func formatter,
832 void *ctx, void *packaged)
833 {
834 uint8_t *buf = packaged;
835 struct cbprintf_package_hdr_ext *hdr = packaged;
836 char *s, **ps;
837 unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
838
839 if (buf == NULL) {
840 return -EINVAL;
841 }
842
843 /* Retrieve the size of the arg list and number of strings. */
844 args_size = hdr->hdr.desc.len * sizeof(int);
845 s_nbr = hdr->hdr.desc.str_cnt;
846 ros_nbr = hdr->hdr.desc.ro_str_cnt;
847 rws_nbr = hdr->hdr.desc.rw_str_cnt;
848
849 /* Locate the string table */
850 s = (char *)(buf + args_size + ros_nbr + 2 * rws_nbr);
851
852 /*
853 * Patch in string pointers.
854 */
855 for (i = 0; i < s_nbr; i++) {
856 /* Locate pointer location for this string */
857 s_idx = *(uint8_t *)s;
858 ++s;
859 ps = (char **)(buf + s_idx * sizeof(int));
860 /* update the pointer with current string location */
861 *ps = s;
862 /* move to next string */
863 s += strlen(s) + 1;
864 }
865
866 /* Skip past the header */
867 buf += sizeof(*hdr);
868
869 /* Turn this into a va_list and print it */
870 return cbprintf_via_va_list(out, formatter, ctx, hdr->fmt, buf);
871 }
872
873 /* Function checks if character might be format specifier. Check is relaxed since
874 * compiler ensures that correct format specifier is used so it is enough to check
875 * that character is not one of potential modifier (e.g. number, dot, etc.).
876 */
is_fmt_spec(char c)877 static bool is_fmt_spec(char c)
878 {
879 return (c >= 64) && (c <= 122);
880 }
881
882 /* Function checks if nth argument is a pointer (%p). Returns true is yes. Returns
883 * false if not or if string does not have nth argument.
884 */
is_ptr(const char * fmt,int n)885 bool is_ptr(const char *fmt, int n)
886 {
887 char c;
888 bool mod = false;
889 int cnt = 0;
890
891 while ((c = *fmt++) != '\0') {
892 if (mod) {
893 if (cnt == n) {
894 if (c == 'p') {
895 return true;
896 } else if (is_fmt_spec(c)) {
897 return false;
898 }
899 } else if (is_fmt_spec(c)) {
900 cnt++;
901 mod = false;
902 }
903 }
904 if (c == '%') {
905 mod = !mod;
906 }
907 }
908
909 return false;
910 }
911
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)912 int cbprintf_package_convert(void *in_packaged,
913 size_t in_len,
914 cbprintf_convert_cb cb,
915 void *ctx,
916 uint32_t flags,
917 uint16_t *strl,
918 size_t strl_len)
919 {
920 __ASSERT_NO_MSG(in_packaged != NULL);
921
922 uint8_t *buf = in_packaged;
923 uint32_t *buf32 = in_packaged;
924 unsigned int args_size, ros_nbr, rws_nbr;
925 bool fmt_present = flags & CBPRINTF_PACKAGE_CONVERT_PTR_CHECK ? true : false;
926 bool rw_cpy;
927 bool ro_cpy;
928 struct cbprintf_package_desc *in_desc = in_packaged;
929
930 in_len = in_len != 0 ? in_len : get_package_len(in_packaged);
931
932 /* Get number of RO string indexes in the package and check if copying
933 * includes appending those strings.
934 */
935 ros_nbr = in_desc->ro_str_cnt;
936 ro_cpy = ros_nbr &&
937 (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) == CBPRINTF_PACKAGE_CONVERT_RO_STR;
938
939 /* Get number of RW string indexes in the package and check if copying
940 * includes appending those strings.
941 */
942 rws_nbr = in_desc->rw_str_cnt;
943 rw_cpy = rws_nbr > 0 &&
944 (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) == CBPRINTF_PACKAGE_CONVERT_RW_STR;
945
946 /* If flags are not set or appending request without rw string indexes
947 * present is chosen, just do a simple copy (or length calculation).
948 * Assuming that it is the most common case.
949 */
950 if (!rw_cpy && !ro_cpy) {
951 if (cb) {
952 cb(in_packaged, in_len, ctx);
953 }
954
955 return in_len;
956 }
957
958 /* If we got here, it means that coping will be more complex and will be
959 * done with strings appending.
960 * Retrieve the size of the arg list.
961 */
962 args_size = in_desc->len * sizeof(int);
963
964 int out_len;
965
966 /* Pointer to array with string locations. Array starts with read-only
967 * string locations.
968 */
969 const char *fmt = *(const char **)(buf + sizeof(void *));
970 uint8_t *str_pos = &buf[args_size];
971 size_t strl_cnt = 0;
972
973 /* If null destination, just calculate output length. */
974 if (cb == NULL) {
975 out_len = (int)in_len;
976 if (ro_cpy) {
977 for (unsigned int i = 0; i < ros_nbr; i++) {
978 const char *str = *(const char **)&buf32[*str_pos];
979 int len = append_string(cb, NULL, str, 0);
980
981 /* If possible store calculated string length. */
982 if (strl && strl_cnt < strl_len) {
983 strl[strl_cnt++] = (uint16_t)len;
984 }
985 out_len += len;
986 str_pos++;
987 }
988 } else {
989 str_pos += ros_nbr;
990 }
991
992 bool drop_ro_str_pos = !(flags &
993 (CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR |
994 CBPRINTF_PACKAGE_CONVERT_RO_STR));
995
996 /* Handle RW strings. */
997 for (unsigned int i = 0; i < rws_nbr; i++) {
998 uint8_t arg_idx = *str_pos++;
999 uint8_t arg_pos = *str_pos++;
1000 const char *str = *(const char **)&buf32[arg_pos];
1001 bool is_ro = ptr_in_rodata(str);
1002 int len;
1003
1004 if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1005 fmt_present && is_ptr(fmt, arg_idx)) {
1006 LOG_WRN("(unsigned) char * used for %%p argument. "
1007 "It's recommended to cast it to void * because "
1008 "it may cause misbehavior in certain "
1009 "configurations. String:\"%s\" argument:%d", fmt, arg_idx);
1010 /* Since location is being dropped, decrement
1011 * output length by 2 (argument index + position)
1012 */
1013 out_len -= 2;
1014 continue;
1015 }
1016
1017 if (is_ro) {
1018 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1019 goto calculate_string_length;
1020 } else {
1021 out_len -= drop_ro_str_pos ? 2 : 1;
1022 }
1023 } else if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1024 calculate_string_length:
1025 len = append_string(cb, NULL, str, 0);
1026
1027 /* If possible store calculated string length. */
1028 if (strl && strl_cnt < strl_len) {
1029 strl[strl_cnt++] = (uint16_t)len;
1030 }
1031 /* string length decremented by 1 because argument
1032 * index is dropped.
1033 */
1034 out_len += (len - 1);
1035 }
1036 }
1037
1038 return out_len;
1039 }
1040
1041 struct cbprintf_package_desc out_desc;
1042 /* At least one is copied in. */
1043 uint8_t cpy_str_pos[16];
1044 /* Up to one will be kept since if both types are kept it returns earlier. */
1045 uint8_t keep_str_pos[16];
1046 uint8_t scpy_cnt;
1047 uint8_t keep_cnt;
1048 uint8_t *dst;
1049 int rv;
1050
1051 /* If read-only strings shall be appended to the output package copy
1052 * their indexes to the local array, otherwise indicate that indexes
1053 * shall remain in the output package.
1054 */
1055 if (ro_cpy) {
1056 scpy_cnt = ros_nbr;
1057 keep_cnt = 0;
1058 dst = cpy_str_pos;
1059 } else if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1060 scpy_cnt = 0;
1061 keep_cnt = ros_nbr;
1062 dst = keep_str_pos;
1063 } else {
1064 scpy_cnt = 0;
1065 keep_cnt = 0;
1066 dst = NULL;
1067 }
1068 if (dst) {
1069 memcpy(dst, str_pos, ros_nbr);
1070 }
1071 str_pos += ros_nbr;
1072
1073 /* Go through read-write strings and identify which shall be appended.
1074 * Note that there may be read-only strings there. Use address evaluation
1075 * to determine if strings is read-only.
1076 */
1077 for (unsigned int i = 0; i < rws_nbr; i++) {
1078 uint8_t arg_idx = *str_pos++;
1079 uint8_t arg_pos = *str_pos++;
1080 const char *str = *(const char **)&buf32[arg_pos];
1081 bool is_ro = ptr_in_rodata(str);
1082
1083 if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1084 fmt_present && is_ptr(fmt, arg_idx)) {
1085 continue;
1086 }
1087
1088 if (is_ro) {
1089 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1090 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1091 cpy_str_pos[scpy_cnt++] = arg_pos;
1092 } else if (flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1093 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1094 keep_str_pos[keep_cnt++] = arg_pos;
1095 } else {
1096 /* Drop information about ro_str location. */
1097 }
1098 } else {
1099 if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1100 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1101 cpy_str_pos[scpy_cnt++] = arg_pos;
1102 } else {
1103 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1104 keep_str_pos[keep_cnt++] = arg_idx;
1105 keep_str_pos[keep_cnt++] = arg_pos;
1106 }
1107 }
1108 }
1109
1110 /* Set amount of strings appended to the package. */
1111 out_desc.len = in_desc->len;
1112 out_desc.str_cnt = in_desc->str_cnt + scpy_cnt;
1113 out_desc.rw_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) ? 0 : (keep_cnt / 2);
1114 out_desc.ro_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) ? 0 :
1115 ((flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) ? keep_cnt : 0);
1116
1117 /* Temporary overwrite input descriptor to allow bulk transfer */
1118 struct cbprintf_package_desc in_desc_backup = *in_desc;
1119 *in_desc = out_desc;
1120
1121 /* Copy package header and arguments. */
1122 rv = cb(in_packaged, args_size, ctx);
1123 if (rv < 0) {
1124 return rv;
1125 }
1126 out_len = rv;
1127 /* Restore input descriptor. */
1128 *in_desc = in_desc_backup;
1129
1130 /* Copy string positions which are kept. */
1131 rv = cb(keep_str_pos, keep_cnt, ctx);
1132 if (rv < 0) {
1133 return rv;
1134 }
1135 out_len += rv;
1136
1137 /* Copy appended strings from source package to destination. */
1138 size_t strs_len = in_len - (args_size + ros_nbr + 2 * rws_nbr);
1139
1140 rv = cb(str_pos, strs_len, ctx);
1141 if (rv < 0) {
1142 return rv;
1143 }
1144 out_len += rv;
1145
1146 /* Append strings */
1147 for (unsigned int i = 0; i < scpy_cnt; i++) {
1148 uint8_t loc = cpy_str_pos[i];
1149 const char *str = *(const char **)&buf32[loc];
1150 uint16_t str_len = strl ? strl[i] : 0;
1151
1152 rv = cb(&loc, 1, ctx);
1153 if (rv < 0) {
1154 return rv;
1155 }
1156 out_len += rv;
1157
1158 rv = append_string(cb, ctx, str, str_len);
1159 if (rv < 0) {
1160 return rv;
1161 }
1162 out_len += rv;
1163 }
1164
1165 /* Empty call (can be interpreted as flushing) */
1166 (void)cb(NULL, 0, ctx);
1167
1168 return out_len;
1169 }
1170
