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 align = VA_STACK_ALIGN(char *);
340 size = sizeof(char *);
341 goto process_string;
342
343 while (true) {
344
345 #if defined(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS)
346 if ((flags & CBPRINTF_PACKAGE_ARGS_ARE_TAGGED)
347 == CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) {
348 int arg_tag = va_arg(ap, int);
349
350 /*
351 * Here we copy the tag over to the package.
352 */
353 align = VA_STACK_ALIGN(int);
354 size = sizeof(int);
355
356 /* align destination buffer location */
357 buf = (void *)ROUND_UP(buf, align);
358
359 /* make sure the data fits */
360 if (buf0 != NULL && BUF_OFFSET + size > len) {
361 return -ENOSPC;
362 }
363
364 if (buf0 != NULL) {
365 *(int *)buf = arg_tag;
366 }
367
368 buf += sizeof(int);
369
370 if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_END) {
371 /* End of arguments */
372 break;
373 }
374
375 /*
376 * There are lots of __fallthrough here since
377 * quite a few of the data types have the same
378 * storage size.
379 */
380 switch (arg_tag) {
381 case CBPRINTF_PACKAGE_ARG_TYPE_CHAR:
382 __fallthrough;
383 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_CHAR:
384 __fallthrough;
385 case CBPRINTF_PACKAGE_ARG_TYPE_SHORT:
386 __fallthrough;
387 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_SHORT:
388 __fallthrough;
389 case CBPRINTF_PACKAGE_ARG_TYPE_INT:
390 __fallthrough;
391 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_INT:
392 align = VA_STACK_ALIGN(int);
393 size = sizeof(int);
394 break;
395
396 case CBPRINTF_PACKAGE_ARG_TYPE_LONG:
397 __fallthrough;
398 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG:
399 align = VA_STACK_ALIGN(long);
400 size = sizeof(long);
401 break;
402
403 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_LONG:
404 __fallthrough;
405 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG_LONG:
406 align = VA_STACK_ALIGN(long long);
407 size = sizeof(long long);
408 break;
409
410 case CBPRINTF_PACKAGE_ARG_TYPE_FLOAT:
411 __fallthrough;
412 case CBPRINTF_PACKAGE_ARG_TYPE_DOUBLE:
413 __fallthrough;
414 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE: {
415 /*
416 * Handle floats separately as they may be
417 * held in a different register set.
418 */
419 union { double d; long double ld; } v;
420
421 if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE) {
422 v.ld = va_arg(ap, long double);
423 align = VA_STACK_ALIGN(long double);
424 size = sizeof(long double);
425 } else {
426 v.d = va_arg(ap, double);
427 align = VA_STACK_ALIGN(double);
428 size = sizeof(double);
429 }
430
431 /* align destination buffer location */
432 buf = (void *) ROUND_UP(buf, align);
433 if (buf0 != NULL) {
434 /* make sure it fits */
435 if (BUF_OFFSET + size > len) {
436 return -ENOSPC;
437 }
438 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
439 memcpy(buf, &v, size);
440 } else if (fmt[-1] == 'L') {
441 *(long double *)buf = v.ld;
442 } else {
443 *(double *)buf = v.d;
444 }
445 }
446 buf += size;
447 parsing = false;
448 continue;
449 }
450
451 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_CHAR:
452 is_str_arg = true;
453
454 __fallthrough;
455 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_VOID:
456 align = VA_STACK_ALIGN(void *);
457 size = sizeof(void *);
458 break;
459
460 default:
461 return -EINVAL;
462 }
463
464 } else
465 #endif /* CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS */
466 {
467 /* Scan the format string */
468 if (*++fmt == '\0') {
469 break;
470 }
471
472 if (!parsing) {
473 if (*fmt == '%') {
474 parsing = true;
475 arg_idx++;
476 align = VA_STACK_ALIGN(int);
477 size = sizeof(int);
478 }
479 continue;
480 }
481 switch (*fmt) {
482 case '%':
483 parsing = false;
484 arg_idx--;
485 continue;
486
487 case '#':
488 case '-':
489 case '+':
490 case ' ':
491 case '0':
492 case '1':
493 case '2':
494 case '3':
495 case '4':
496 case '5':
497 case '6':
498 case '7':
499 case '8':
500 case '9':
501 case '.':
502 case 'h':
503 case 'l':
504 case 'L':
505 continue;
506
507 case '*':
508 break;
509
510 case 'j':
511 align = VA_STACK_ALIGN(intmax_t);
512 size = sizeof(intmax_t);
513 continue;
514
515 case 'z':
516 align = VA_STACK_ALIGN(size_t);
517 size = sizeof(size_t);
518 continue;
519
520 case 't':
521 align = VA_STACK_ALIGN(ptrdiff_t);
522 size = sizeof(ptrdiff_t);
523 continue;
524
525 case 'c':
526 case 'd':
527 case 'i':
528 case 'o':
529 case 'u':
530 case 'x':
531 case 'X':
532 if (fmt[-1] == 'l') {
533 if (fmt[-2] == 'l') {
534 align = VA_STACK_ALIGN(long long);
535 size = sizeof(long long);
536 } else {
537 align = VA_STACK_ALIGN(long);
538 size = sizeof(long);
539 }
540 }
541 parsing = false;
542 break;
543
544 case 's':
545 is_str_arg = true;
546
547 __fallthrough;
548 case 'p':
549 case 'n':
550 align = VA_STACK_ALIGN(void *);
551 size = sizeof(void *);
552 parsing = false;
553 break;
554
555 case 'a':
556 case 'A':
557 case 'e':
558 case 'E':
559 case 'f':
560 case 'F':
561 case 'g':
562 case 'G': {
563 /*
564 * Handle floats separately as they may be
565 * held in a different register set.
566 */
567 union { double d; long double ld; } v;
568
569 if (fmt[-1] == 'L') {
570 v.ld = va_arg(ap, long double);
571 align = VA_STACK_ALIGN(long double);
572 size = sizeof(long double);
573 } else {
574 v.d = va_arg(ap, double);
575 align = VA_STACK_ALIGN(double);
576 size = sizeof(double);
577 }
578 /* align destination buffer location */
579 buf = (void *) ROUND_UP(buf, align);
580 if (buf0 != NULL) {
581 /* make sure it fits */
582 if (BUF_OFFSET + size > len) {
583 return -ENOSPC;
584 }
585 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
586 memcpy(buf, &v, size);
587 } else if (fmt[-1] == 'L') {
588 *(long double *)buf = v.ld;
589 } else {
590 *(double *)buf = v.d;
591 }
592 }
593 buf += size;
594 parsing = false;
595 continue;
596 }
597
598 default:
599 parsing = false;
600 continue;
601 }
602 }
603
604 /* align destination buffer location */
605 buf = (void *) ROUND_UP(buf, align);
606
607 /* make sure the data fits */
608 if (buf0 != NULL && BUF_OFFSET + size > len) {
609 return -ENOSPC;
610 }
611
612 /* copy va_list data over to our buffer */
613 if (is_str_arg) {
614 s = va_arg(ap, char *);
615 process_string:
616 if (buf0 != NULL) {
617 *(const char **)buf = s;
618 }
619
620 bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
621 bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
622
623 if (is_ro && !do_ro) {
624 /* nothing to do */
625 } else {
626 uint32_t s_ptr_idx = BUF_OFFSET / sizeof(int);
627
628 /*
629 * In the do_ro case we must consider
630 * room for possible STR_POS_RO_FLAG.
631 * Otherwise the index range is 8 bits
632 * and any overflow is caught later.
633 */
634 if (do_ro && s_ptr_idx > STR_POS_MASK) {
635 __ASSERT(false, "String with too many arguments");
636 return -EINVAL;
637 }
638
639 if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
640 __ASSERT(false, "str_ptr_pos[] too small");
641 return -EINVAL;
642 }
643
644 if (buf0 != NULL) {
645 /*
646 * Remember string pointer location.
647 * We will append non-ro strings later.
648 */
649 str_ptr_pos[s_idx] = s_ptr_idx;
650 str_ptr_arg[s_idx] = arg_idx;
651 if (is_ro) {
652 /* flag read-only string. */
653 str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
654 s_ro_cnt++;
655 } else {
656 s_rw_cnt++;
657 }
658 } else if (is_ro) {
659 /*
660 * Add only pointer position prefix
661 * when counting strings.
662 */
663 len += 1;
664 } else if (rws_pos_en) {
665 /*
666 * Add only pointer position prefix and
667 * argument index when counting strings.
668 */
669 len += 2;
670 } else {
671 /*
672 * Add the string length, the final '\0'
673 * and size of the pointer position prefix.
674 */
675 len += strlen(s) + 1 + 1;
676 }
677
678 s_idx++;
679 }
680 buf += sizeof(char *);
681
682 is_str_arg = false;
683 } else if (size == sizeof(int)) {
684 int v = va_arg(ap, int);
685
686 if (buf0 != NULL) {
687 *(int *)buf = v;
688 }
689 buf += sizeof(int);
690 } else if (size == sizeof(long)) {
691 long v = va_arg(ap, long);
692
693 if (buf0 != NULL) {
694 *(long *)buf = v;
695 }
696 buf += sizeof(long);
697 } else if (size == sizeof(long long)) {
698 long long v = va_arg(ap, long long);
699
700 if (buf0 != NULL) {
701 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
702 memcpy(buf, &v, sizeof(long long));
703 } else {
704 *(long long *)buf = v;
705 }
706 }
707 buf += sizeof(long long);
708 } else {
709 __ASSERT(false, "unexpected size %u", size);
710 return -EINVAL;
711 }
712 }
713
714 /*
715 * We remember the size of the argument list as a multiple of
716 * sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
717 * worth of va_list, or about 127 arguments on a 64-bit system
718 * (twice that on 32-bit systems). That ought to be good enough.
719 */
720 if (BUF_OFFSET / sizeof(int) > 255) {
721 __ASSERT(false, "too many format args");
722 return -EINVAL;
723 }
724
725 /*
726 * If all we wanted was to count required buffer size
727 * then we have it now.
728 */
729 if (buf0 == NULL) {
730 return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
731 }
732
733 /* Clear our buffer header. We made room for it initially. */
734 *(char **)buf0 = NULL;
735
736 /* Record end of argument list. */
737 pkg_hdr->desc.len = BUF_OFFSET / sizeof(int);
738
739 if (rws_pos_en) {
740 /* Strings are appended, update location counter. */
741 pkg_hdr->desc.str_cnt = 0;
742 pkg_hdr->desc.rw_str_cnt = s_rw_cnt;
743 } else {
744 /* Strings are appended, update append counter. */
745 pkg_hdr->desc.str_cnt = s_rw_cnt;
746 pkg_hdr->desc.rw_str_cnt = 0;
747 }
748
749 pkg_hdr->desc.ro_str_cnt = s_ro_cnt;
750
751 #ifdef CONFIG_CBPRINTF_PACKAGE_HEADER_STORE_CREATION_FLAGS
752 pkg_hdr->desc.pkg_flags = flags;
753 #endif
754
755 /* Store strings pointer locations of read only strings. */
756 if (s_ro_cnt) {
757 for (i = 0; i < s_idx; i++) {
758 if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
759 continue;
760 }
761
762 uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
763
764 /* make sure it fits */
765 if (BUF_OFFSET + 1 > len) {
766 return -ENOSPC;
767 }
768 /* store the pointer position prefix */
769 *buf++ = pos;
770 }
771 }
772
773 /* Store strings prefixed by their pointer location. */
774 for (i = 0; i < s_idx; i++) {
775 /* Process only RW strings. */
776 if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
777 continue;
778 }
779
780 if (rws_pos_en) {
781 size = 0;
782 *buf++ = str_ptr_arg[i];
783 } else {
784 /* retrieve the string pointer */
785 s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
786 /* clear the in-buffer pointer (less entropy if compressed) */
787 *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
788 /* find the string length including terminating '\0' */
789 size = strlen(s) + 1;
790 }
791
792 /* make sure it fits */
793 if (BUF_OFFSET + 1 + size > len) {
794 return -ENOSPC;
795 }
796 /* store the pointer position prefix */
797 *buf++ = str_ptr_pos[i];
798 /* copy the string with its terminating '\0' */
799 memcpy(buf, s, size);
800 buf += size;
801 }
802
803 /*
804 * TODO: remove pointers for appended strings since they're useless.
805 * TODO: explore leveraging same mechanism to remove alignment padding
806 */
807
808 return BUF_OFFSET;
809
810 #undef BUF_OFFSET
811 #undef STR_POS_RO_FLAG
812 #undef STR_POS_MASK
813 }
814
cbprintf_package(void * packaged,size_t len,uint32_t flags,const char * format,...)815 int cbprintf_package(void *packaged, size_t len, uint32_t flags,
816 const char *format, ...)
817 {
818 va_list ap;
819 int ret;
820
821 va_start(ap, format);
822 ret = cbvprintf_package(packaged, len, flags, format, ap);
823 va_end(ap);
824 return ret;
825 }
826
cbpprintf_external(cbprintf_cb out,cbvprintf_external_formatter_func formatter,void * ctx,void * packaged)827 int cbpprintf_external(cbprintf_cb out,
828 cbvprintf_external_formatter_func formatter,
829 void *ctx, void *packaged)
830 {
831 uint8_t *buf = packaged;
832 struct cbprintf_package_hdr_ext *hdr = packaged;
833 char *s, **ps;
834 unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
835
836 if (buf == NULL) {
837 return -EINVAL;
838 }
839
840 /* Retrieve the size of the arg list and number of strings. */
841 args_size = hdr->hdr.desc.len * sizeof(int);
842 s_nbr = hdr->hdr.desc.str_cnt;
843 ros_nbr = hdr->hdr.desc.ro_str_cnt;
844 rws_nbr = hdr->hdr.desc.rw_str_cnt;
845
846 /* Locate the string table */
847 s = (char *)(buf + args_size + ros_nbr + 2 * rws_nbr);
848
849 /*
850 * Patch in string pointers.
851 */
852 for (i = 0; i < s_nbr; i++) {
853 /* Locate pointer location for this string */
854 s_idx = *(uint8_t *)s++;
855 ps = (char **)(buf + s_idx * sizeof(int));
856 /* update the pointer with current string location */
857 *ps = s;
858 /* move to next string */
859 s += strlen(s) + 1;
860 }
861
862 /* Skip past the header */
863 buf += sizeof(*hdr);
864
865 /* Turn this into a va_list and print it */
866 return cbprintf_via_va_list(out, formatter, ctx, hdr->fmt, buf);
867 }
868
869 /* Function checks if character might be format specifier. Check is relaxed since
870 * compiler ensures that correct format specifier is used so it is enough to check
871 * that character is not one of potential modifier (e.g. number, dot, etc.).
872 */
is_fmt_spec(char c)873 static bool is_fmt_spec(char c)
874 {
875 return (c >= 64) && (c <= 122);
876 }
877
878 /* Function checks if nth argument is a pointer (%p). Returns true is yes. Returns
879 * false if not or if string does not have nth argument.
880 */
is_ptr(const char * fmt,int n)881 bool is_ptr(const char *fmt, int n)
882 {
883 char c;
884 bool mod = false;
885 int cnt = 0;
886
887 while ((c = *fmt++) != '\0') {
888 if (mod) {
889 if (cnt == n) {
890 if (c == 'p') {
891 return true;
892 } else if (is_fmt_spec(c)) {
893 return false;
894 }
895 } else if (is_fmt_spec(c)) {
896 cnt++;
897 mod = false;
898 }
899 }
900 if (c == '%') {
901 mod = !mod;
902 }
903 }
904
905 return false;
906 }
907
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)908 int cbprintf_package_convert(void *in_packaged,
909 size_t in_len,
910 cbprintf_convert_cb cb,
911 void *ctx,
912 uint32_t flags,
913 uint16_t *strl,
914 size_t strl_len)
915 {
916 __ASSERT_NO_MSG(in_packaged != NULL);
917
918 uint8_t *buf = in_packaged;
919 uint32_t *buf32 = in_packaged;
920 unsigned int args_size, ros_nbr, rws_nbr;
921 bool fmt_present = flags & CBPRINTF_PACKAGE_CONVERT_PTR_CHECK ? true : false;
922 bool rw_cpy;
923 bool ro_cpy;
924 struct cbprintf_package_desc *in_desc = in_packaged;
925
926 in_len = in_len != 0 ? in_len : get_package_len(in_packaged);
927
928 /* Get number of RO string indexes in the package and check if copying
929 * includes appending those strings.
930 */
931 ros_nbr = in_desc->ro_str_cnt;
932 ro_cpy = ros_nbr &&
933 (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) == CBPRINTF_PACKAGE_CONVERT_RO_STR;
934
935 /* Get number of RW string indexes in the package and check if copying
936 * includes appending those strings.
937 */
938 rws_nbr = in_desc->rw_str_cnt;
939 rw_cpy = rws_nbr > 0 &&
940 (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) == CBPRINTF_PACKAGE_CONVERT_RW_STR;
941
942 /* If flags are not set or appending request without rw string indexes
943 * present is chosen, just do a simple copy (or length calculation).
944 * Assuming that it is the most common case.
945 */
946 if (!rw_cpy && !ro_cpy) {
947 if (cb) {
948 cb(in_packaged, in_len, ctx);
949 }
950
951 return in_len;
952 }
953
954 /* If we got here, it means that coping will be more complex and will be
955 * done with strings appending.
956 * Retrieve the size of the arg list.
957 */
958 args_size = in_desc->len * sizeof(int);
959
960 int out_len;
961
962 /* Pointer to array with string locations. Array starts with read-only
963 * string locations.
964 */
965 const char *fmt = *(const char **)(buf + sizeof(void *));
966 uint8_t *str_pos = &buf[args_size];
967 size_t strl_cnt = 0;
968
969 /* If null destination, just calculate output length. */
970 if (cb == NULL) {
971 out_len = (int)in_len;
972 if (ro_cpy) {
973 for (unsigned int i = 0; i < ros_nbr; i++) {
974 const char *str = *(const char **)&buf32[*str_pos];
975 int len = append_string(cb, NULL, str, 0);
976
977 /* If possible store calculated string length. */
978 if (strl && strl_cnt < strl_len) {
979 strl[strl_cnt++] = (uint16_t)len;
980 }
981 out_len += len;
982 str_pos++;
983 }
984 } else {
985 if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
986 str_pos += ros_nbr;
987 }
988 }
989
990 bool drop_ro_str_pos = !(flags &
991 (CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR |
992 CBPRINTF_PACKAGE_CONVERT_RO_STR));
993
994 /* Handle RW strings. */
995 for (unsigned int i = 0; i < rws_nbr; i++) {
996 uint8_t arg_idx = *str_pos++;
997 uint8_t arg_pos = *str_pos++;
998 const char *str = *(const char **)&buf32[arg_pos];
999 bool is_ro = ptr_in_rodata(str);
1000 int len;
1001
1002 if (fmt_present && is_ptr(fmt, arg_idx)) {
1003 LOG_WRN("(unsigned) char * used for %%p argument. "
1004 "It's recommended to cast it to void * because "
1005 "it may cause misbehavior in certain "
1006 "configurations. String:\"%s\" argument:%d", fmt, arg_idx);
1007 /* Since location is being dropped, decrement
1008 * output length by 2 (argument index + position)
1009 */
1010 out_len -= 2;
1011 continue;
1012 }
1013
1014 if (is_ro) {
1015 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1016 goto calculate_string_length;
1017 } else {
1018 out_len -= drop_ro_str_pos ? 2 : 1;
1019 }
1020 } else if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1021 calculate_string_length:
1022 len = append_string(cb, NULL, str, 0);
1023
1024 /* If possible store calculated string length. */
1025 if (strl && strl_cnt < strl_len) {
1026 strl[strl_cnt++] = (uint16_t)len;
1027 }
1028 /* string length decremented by 1 because argument
1029 * index is dropped.
1030 */
1031 out_len += (len - 1);
1032 }
1033 }
1034
1035 return out_len;
1036 }
1037
1038 struct cbprintf_package_desc out_desc;
1039 /* At least one is copied in. */
1040 uint8_t cpy_str_pos[16];
1041 /* Up to one will be kept since if both types are kept it returns earlier. */
1042 uint8_t keep_str_pos[16];
1043 uint8_t scpy_cnt;
1044 uint8_t keep_cnt;
1045 uint8_t *dst;
1046 int rv;
1047
1048 /* If read-only strings shall be appended to the output package copy
1049 * their indexes to the local array, otherwise indicate that indexes
1050 * shall remain in the output package.
1051 */
1052 if (ro_cpy) {
1053 scpy_cnt = ros_nbr;
1054 keep_cnt = 0;
1055 dst = cpy_str_pos;
1056 } else if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1057 scpy_cnt = 0;
1058 keep_cnt = ros_nbr;
1059 dst = keep_str_pos;
1060 } else {
1061 scpy_cnt = 0;
1062 keep_cnt = 0;
1063 dst = NULL;
1064 }
1065 if (dst) {
1066 memcpy(dst, str_pos, ros_nbr);
1067 }
1068 str_pos += ros_nbr;
1069
1070 /* Go through read-write strings and identify which shall be appended.
1071 * Note that there may be read-only strings there. Use address evaluation
1072 * to determine if strings is read-only.
1073 */
1074 for (unsigned int i = 0; i < rws_nbr; i++) {
1075 uint8_t arg_idx = *str_pos++;
1076 uint8_t arg_pos = *str_pos++;
1077 const char *str = *(const char **)&buf32[arg_pos];
1078 bool is_ro = ptr_in_rodata(str);
1079
1080 if (fmt_present && is_ptr(fmt, arg_idx)) {
1081 continue;
1082 }
1083
1084 if (is_ro) {
1085 if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1086 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1087 cpy_str_pos[scpy_cnt++] = arg_pos;
1088 } else if (flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1089 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1090 keep_str_pos[keep_cnt++] = arg_pos;
1091 } else {
1092 /* Drop information about ro_str location. */
1093 }
1094 } else {
1095 if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1096 __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1097 cpy_str_pos[scpy_cnt++] = arg_pos;
1098 } else {
1099 __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1100 keep_str_pos[keep_cnt++] = arg_idx;
1101 keep_str_pos[keep_cnt++] = arg_pos;
1102 }
1103 }
1104 }
1105
1106 /* Set amount of strings appended to the package. */
1107 out_desc.len = in_desc->len;
1108 out_desc.str_cnt = in_desc->str_cnt + scpy_cnt;
1109 out_desc.rw_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) ? 0 : (keep_cnt / 2);
1110 out_desc.ro_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) ? 0 :
1111 ((flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) ? keep_cnt : 0);
1112
1113 /* Temporary overwrite input descriptor to allow bulk transfer */
1114 struct cbprintf_package_desc in_desc_backup = *in_desc;
1115 *in_desc = out_desc;
1116
1117 /* Copy package header and arguments. */
1118 rv = cb(in_packaged, args_size, ctx);
1119 if (rv < 0) {
1120 return rv;
1121 }
1122 out_len = rv;
1123 /* Restore input descriptor. */
1124 *in_desc = in_desc_backup;
1125
1126 /* Copy string positions which are kept. */
1127 rv = cb(keep_str_pos, keep_cnt, ctx);
1128 if (rv < 0) {
1129 return rv;
1130 }
1131 out_len += rv;
1132
1133 /* Copy appended strings from source package to destination. */
1134 size_t strs_len = in_len - (args_size + ros_nbr + 2 * rws_nbr);
1135
1136 rv = cb(str_pos, strs_len, ctx);
1137 if (rv < 0) {
1138 return rv;
1139 }
1140 out_len += rv;
1141
1142 /* Append strings */
1143 for (unsigned int i = 0; i < scpy_cnt; i++) {
1144 uint8_t loc = cpy_str_pos[i];
1145 const char *str = *(const char **)&buf32[loc];
1146 uint16_t str_len = strl ? strl[i] : 0;
1147
1148 rv = cb(&loc, 1, ctx);
1149 if (rv < 0) {
1150 return rv;
1151 }
1152 out_len += rv;
1153
1154 rv = append_string(cb, ctx, str, str_len);
1155 if (rv < 0) {
1156 return rv;
1157 }
1158 out_len += rv;
1159 }
1160
1161 /* Empty call (can be interpreted as flushing) */
1162 (void)cb(NULL, 0, ctx);
1163
1164 return out_len;
1165 }
1166
