Branch data Line data Source code
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 : : */
33 : 514 : 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 : 514 : return linker_is_in_rodata(addr);
42 : : #endif
43 : : }
44 : :
45 : : /*
46 : : * va_list creation
47 : : */
48 : :
49 : : #if defined(__CHECKER__)
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 : :
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 : :
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 : :
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 : :
179 : 177 : 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 : 177 : union {
185 : : va_list ap;
186 : : void *ptr;
187 : : } u;
188 : :
189 : 177 : u.ptr = buf;
190 : :
191 : 177 : return formatter(out, ctx, fmt, u.ap);
192 : : }
193 : :
194 : : #endif
195 : :
196 : 0 : static size_t get_package_len(void *packaged)
197 : : {
198 [ # # ]: 0 : __ASSERT_NO_MSG(packaged != NULL);
199 : :
200 : 0 : uint8_t *buf = packaged;
201 : 0 : uint8_t *start = buf;
202 : 0 : unsigned int args_size, s_nbr, ros_nbr;
203 : :
204 : 0 : args_size = buf[0] * sizeof(int);
205 : 0 : s_nbr = buf[1];
206 : 0 : ros_nbr = buf[2];
207 : :
208 : : /* Move beyond args. */
209 : 0 : buf += args_size;
210 : :
211 : : /* Move beyond read-only string indexes array. */
212 : 0 : buf += ros_nbr;
213 : :
214 : : /* Move beyond strings appended to the package. */
215 [ # # ]: 0 : for (unsigned int i = 0; i < s_nbr; i++) {
216 : 0 : buf++;
217 : 0 : buf += strlen((const char *)buf) + 1;
218 : : }
219 : :
220 : 0 : return (size_t)(uintptr_t)(buf - start);
221 : : }
222 : :
223 : 0 : static int append_string(cbprintf_convert_cb cb, void *ctx, const char *str, uint16_t strl)
224 : : {
225 [ # # ]: 0 : if (cb == NULL) {
226 : 0 : return 1 + strlen(str);
227 : : }
228 : :
229 [ # # ]: 0 : strl = strl > 0 ? strl : strlen(str) + 1;
230 : 0 : return cb(str, strl, ctx);
231 : : }
232 : :
233 : 354 : 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 : 354 : uint8_t *buf0 = packaged; /* buffer start (may be NULL) */
248 : 354 : uint8_t *buf = buf0; /* current buffer position */
249 : 354 : unsigned int size; /* current argument's size */
250 : 354 : unsigned int align; /* current argument's required alignment */
251 : 354 : uint8_t str_ptr_pos[16]; /* string pointer positions */
252 : 354 : uint8_t str_ptr_arg[16]; /* string pointer argument index */
253 : 354 : unsigned int s_idx = 0; /* index into str_ptr_pos[] */
254 : 354 : unsigned int s_rw_cnt = 0; /* number of rw strings */
255 : 354 : unsigned int s_ro_cnt = 0; /* number of ro strings */
256 : 354 : int arg_idx = -1; /* Argument index. Preincremented thus starting from -1.*/
257 : 354 : unsigned int i;
258 : 354 : const char *s;
259 : 354 : 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 : 354 : 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 : 354 : int fros_cnt = 1 + Z_CBPRINTF_PACKAGE_FIRST_RO_STR_CNT_GET(flags);
269 : 354 : bool is_str_arg = false;
270 : 354 : union cbprintf_package_hdr *pkg_hdr = packaged;
271 : :
272 : : /* Buffer must be aligned at least to size of a pointer. */
273 [ + - ]: 354 : 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 : 354 : 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 [ + + ]: 354 : if (buf0 == NULL) {
306 : 177 : 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 : 177 : 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 [ + + + - ]: 354 : 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 : 354 : s = fmt;
339 : 354 : --fmt;
340 : 354 : align = VA_STACK_ALIGN(char *);
341 : 354 : size = sizeof(char *);
342 : 354 : goto process_string;
343 : :
344 : 13710 : 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 [ + + ]: 13710 : if (*++fmt == '\0') {
470 : : break;
471 : : }
472 : :
473 [ + + ]: 13356 : if (!parsing) {
474 [ + + ]: 12142 : if (*fmt == '%') {
475 : 868 : parsing = true;
476 : 868 : arg_idx++;
477 : 868 : align = VA_STACK_ALIGN(int);
478 : 868 : size = sizeof(int);
479 : : }
480 : 12142 : continue;
481 : : }
482 [ - + + - : 1214 : switch (*fmt) {
- - + + -
- - ]
483 : 2 : case '%':
484 : 2 : parsing = false;
485 : 2 : arg_idx--;
486 : 2 : continue;
487 : :
488 : 346 : 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 : 346 : continue;
507 : :
508 : : case '*':
509 : : break;
510 : :
511 : 0 : case 'j':
512 : 0 : align = VA_STACK_ALIGN(intmax_t);
513 : 0 : size = sizeof(intmax_t);
514 : 0 : continue;
515 : :
516 : 0 : case 'z':
517 : 0 : align = VA_STACK_ALIGN(size_t);
518 : 0 : size = sizeof(size_t);
519 : 0 : continue;
520 : :
521 : 0 : case 't':
522 : 0 : align = VA_STACK_ALIGN(ptrdiff_t);
523 : 0 : size = sizeof(ptrdiff_t);
524 : 0 : continue;
525 : :
526 : 352 : case 'c':
527 : : case 'd':
528 : : case 'i':
529 : : case 'o':
530 : : case 'u':
531 : : case 'x':
532 : : case 'X':
533 [ - + ]: 352 : if (fmt[-1] == 'l') {
534 [ # # ]: 0 : if (fmt[-2] == 'l') {
535 : : align = VA_STACK_ALIGN(long long);
536 : : size = sizeof(long long);
537 : : } else {
538 : 0 : align = VA_STACK_ALIGN(long);
539 : 0 : size = sizeof(long);
540 : : }
541 : : }
542 : : parsing = false;
543 : : break;
544 : :
545 : 514 : case 's':
546 : 514 : 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 : 0 : case 'a':
557 : : case 'A':
558 : : case 'e':
559 : : case 'E':
560 : : case 'f':
561 : : case 'F':
562 : : case 'g':
563 : 0 : case 'G': {
564 : : /*
565 : : * Handle floats separately as they may be
566 : : * held in a different register set.
567 : : */
568 : 0 : union { double d; long double ld; } v;
569 : :
570 [ # # ]: 0 : if (fmt[-1] == 'L') {
571 : 0 : v.ld = va_arg(ap, long double);
572 : 0 : align = VA_STACK_ALIGN(long double);
573 : 0 : size = sizeof(long double);
574 : : } else {
575 : 0 : v.d = va_arg(ap, double);
576 : 0 : align = VA_STACK_ALIGN(double);
577 : 0 : size = sizeof(double);
578 : : }
579 : : /* align destination buffer location */
580 : 0 : buf = (void *) ROUND_UP(buf, align);
581 [ # # ]: 0 : if (buf0 != NULL) {
582 : : /* make sure it fits */
583 [ # # ]: 0 : if (BUF_OFFSET + size > len) {
584 : 0 : return -ENOSPC;
585 : : }
586 : 0 : if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
587 : : memcpy(buf, (uint8_t *)&v, size);
588 [ # # ]: 0 : } else if (fmt[-1] == 'L') {
589 : 0 : *(long double *)buf = v.ld;
590 : : } else {
591 : 0 : *(double *)buf = v.d;
592 : : }
593 : : }
594 : 0 : buf += size;
595 : 0 : parsing = false;
596 : 0 : continue;
597 : : }
598 : :
599 : 0 : default:
600 : 0 : parsing = false;
601 : 0 : continue;
602 : : }
603 : : }
604 : :
605 : : /* align destination buffer location */
606 : 866 : buf = (void *) ROUND_UP(buf, align);
607 : :
608 : : /* make sure the data fits */
609 [ + + + - ]: 866 : if ((buf0 != NULL) && (BUF_OFFSET + size) > len) {
610 : : return -ENOSPC;
611 : : }
612 : :
613 : : /* copy va_list data over to our buffer */
614 [ + + ]: 866 : if (is_str_arg) {
615 : 514 : s = va_arg(ap, char *);
616 : 868 : process_string:
617 [ + + ]: 868 : if (buf0 != NULL) {
618 : 434 : *(const char **)buf = s;
619 : : }
620 : :
621 [ + + - + ]: 868 : bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
622 : 868 : bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
623 : :
624 [ + + ]: 868 : if (is_ro && !do_ro) {
625 : : /* nothing to do */
626 : : } else {
627 : 514 : 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 [ - + ]: 514 : if (do_ro && s_ptr_idx > STR_POS_MASK) {
636 : 0 : __ASSERT(false, "String with too many arguments");
637 : : return -EINVAL;
638 : : }
639 : :
640 [ - + ]: 514 : if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
641 : 0 : __ASSERT(false, "str_ptr_pos[] too small");
642 : : return -EINVAL;
643 : : }
644 : :
645 [ + + ]: 514 : if (buf0 != NULL) {
646 : : /*
647 : : * Remember string pointer location.
648 : : * We will append non-ro strings later.
649 : : */
650 : 257 : str_ptr_pos[s_idx] = s_ptr_idx;
651 : 257 : str_ptr_arg[s_idx] = arg_idx;
652 [ - + ]: 257 : if (is_ro) {
653 : : /* flag read-only string. */
654 : 0 : str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
655 : 0 : s_ro_cnt++;
656 : : } else {
657 : 257 : s_rw_cnt++;
658 : : }
659 [ - + ]: 257 : } else if (is_ro) {
660 : : /*
661 : : * Add only pointer position prefix
662 : : * when counting strings.
663 : : */
664 : 0 : len += 1;
665 [ - + ]: 257 : } else if (rws_pos_en) {
666 : : /*
667 : : * Add only pointer position prefix and
668 : : * argument index when counting strings.
669 : : */
670 : 0 : len += 2;
671 : : } else {
672 : : /*
673 : : * Add the string length, the final '\0'
674 : : * and size of the pointer position prefix.
675 : : */
676 : 257 : len += strlen(s) + 1 + 1;
677 : : }
678 : :
679 : 514 : s_idx++;
680 : : }
681 : 868 : buf += sizeof(char *);
682 : :
683 : 868 : is_str_arg = false;
684 [ + - ]: 352 : } else if (size == sizeof(int)) {
685 : 352 : int v = va_arg(ap, int);
686 : :
687 [ + + ]: 352 : if (buf0 != NULL) {
688 : 176 : *(int *)buf = v;
689 : : }
690 : 352 : buf += sizeof(int);
691 : 0 : } 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 [ # # ]: 0 : } else if (size == sizeof(long long)) {
699 : 0 : long long v = va_arg(ap, long long);
700 : :
701 [ # # ]: 0 : if (buf0 != NULL) {
702 : 0 : if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
703 : : memcpy(buf, (uint8_t *)&v, sizeof(long long));
704 : : } else {
705 : 0 : *(long long *)buf = v;
706 : : }
707 : : }
708 : 0 : buf += sizeof(long long);
709 : : } else {
710 : 0 : __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 [ - + ]: 354 : if ((BUF_OFFSET / sizeof(int)) > 255) {
722 : 0 : __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 [ + + ]: 354 : if (buf0 == NULL) {
731 : 177 : return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
732 : : }
733 : :
734 : : /* Clear our buffer header. We made room for it initially. */
735 : 177 : *(char **)buf0 = NULL;
736 : :
737 : : /* Record end of argument list. */
738 : 177 : pkg_hdr->desc.len = BUF_OFFSET / sizeof(int);
739 : :
740 [ - + ]: 177 : if (rws_pos_en) {
741 : : /* Strings are appended, update location counter. */
742 : 0 : pkg_hdr->desc.str_cnt = 0;
743 : 0 : pkg_hdr->desc.rw_str_cnt = s_rw_cnt;
744 : : } else {
745 : : /* Strings are appended, update append counter. */
746 : 177 : pkg_hdr->desc.str_cnt = s_rw_cnt;
747 : 177 : pkg_hdr->desc.rw_str_cnt = 0;
748 : : }
749 : :
750 : 177 : 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 [ - + ]: 177 : if (s_ro_cnt != 0U) {
758 [ # # ]: 0 : for (i = 0; i < s_idx; i++) {
759 [ # # ]: 0 : if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
760 : 0 : continue;
761 : : }
762 : :
763 : 0 : uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
764 : :
765 : : /* make sure it fits */
766 [ # # ]: 0 : if ((BUF_OFFSET + 1) > len) {
767 : : return -ENOSPC;
768 : : }
769 : : /* store the pointer position prefix */
770 : 0 : *buf = pos;
771 : 0 : ++buf;
772 : : }
773 : : }
774 : :
775 : : /* Store strings prefixed by their pointer location. */
776 [ + + ]: 434 : for (i = 0; i < s_idx; i++) {
777 : : /* Process only RW strings. */
778 [ - + - - ]: 257 : if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
779 : 0 : continue;
780 : : }
781 : :
782 [ - + ]: 257 : if (rws_pos_en) {
783 : 0 : size = 0;
784 : 0 : *buf++ = str_ptr_arg[i];
785 : : } else {
786 : : /* retrieve the string pointer */
787 : 257 : s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
788 : : /* clear the in-buffer pointer (less entropy if compressed) */
789 : 257 : *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
790 : : /* find the string length including terminating '\0' */
791 : 257 : size = strlen(s) + 1;
792 : : }
793 : :
794 : : /* make sure it fits */
795 [ + - ]: 257 : if ((BUF_OFFSET + 1 + size) > len) {
796 : : return -ENOSPC;
797 : : }
798 : : /* store the pointer position prefix */
799 : 257 : *buf = str_ptr_pos[i];
800 : 257 : ++buf;
801 : : /* copy the string with its terminating '\0' */
802 : 257 : memcpy(buf, (uint8_t *)s, size);
803 : 257 : 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 : 177 : return BUF_OFFSET;
812 : :
813 : : #undef BUF_OFFSET
814 : : #undef STR_POS_RO_FLAG
815 : : #undef STR_POS_MASK
816 : : }
817 : :
818 : 0 : int cbprintf_package(void *packaged, size_t len, uint32_t flags,
819 : : const char *format, ...)
820 : : {
821 : 0 : va_list ap;
822 : 0 : int ret;
823 : :
824 : 0 : va_start(ap, format);
825 : 0 : ret = cbvprintf_package(packaged, len, flags, format, ap);
826 : 0 : va_end(ap);
827 : 0 : return ret;
828 : : }
829 : :
830 : 177 : int cbpprintf_external(cbprintf_cb out,
831 : : cbvprintf_external_formatter_func formatter,
832 : : void *ctx, void *packaged)
833 : : {
834 : 177 : uint8_t *buf = packaged;
835 : 177 : struct cbprintf_package_hdr_ext *hdr = packaged;
836 : 177 : char *s, **ps;
837 : 177 : unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
838 : :
839 [ + - ]: 177 : if (buf == NULL) {
840 : : return -EINVAL;
841 : : }
842 : :
843 : : /* Retrieve the size of the arg list and number of strings. */
844 : 177 : args_size = hdr->hdr.desc.len * sizeof(int);
845 : 177 : s_nbr = hdr->hdr.desc.str_cnt;
846 : 177 : ros_nbr = hdr->hdr.desc.ro_str_cnt;
847 : 177 : rws_nbr = hdr->hdr.desc.rw_str_cnt;
848 : :
849 : : /* Locate the string table */
850 : 177 : s = (char *)(buf + args_size + ros_nbr + 2 * rws_nbr);
851 : :
852 : : /*
853 : : * Patch in string pointers.
854 : : */
855 [ + + ]: 434 : for (i = 0; i < s_nbr; i++) {
856 : : /* Locate pointer location for this string */
857 : 257 : s_idx = *(uint8_t *)s;
858 : 257 : ++s;
859 : 257 : ps = (char **)(buf + s_idx * sizeof(int));
860 : : /* update the pointer with current string location */
861 : 257 : *ps = s;
862 : : /* move to next string */
863 : 257 : s += strlen(s) + 1;
864 : : }
865 : :
866 : : /* Skip past the header */
867 : 177 : buf += sizeof(*hdr);
868 : :
869 : : /* Turn this into a va_list and print it */
870 : 177 : 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 : : */
877 : 0 : static bool is_fmt_spec(char c)
878 : : {
879 : 0 : 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 : : */
885 : 0 : bool is_ptr(const char *fmt, int n)
886 : : {
887 : 0 : char c;
888 : 0 : bool mod = false;
889 : 0 : int cnt = 0;
890 : :
891 [ # # ]: 0 : while ((c = *fmt++) != '\0') {
892 [ # # ]: 0 : if (mod) {
893 [ # # ]: 0 : if (cnt == n) {
894 [ # # ]: 0 : if (c == 'p') {
895 : : return true;
896 [ # # ]: 0 : } else if (is_fmt_spec(c)) {
897 : : return false;
898 : : }
899 [ # # ]: 0 : } else if (is_fmt_spec(c)) {
900 : 0 : cnt++;
901 : 0 : mod = false;
902 : : }
903 : : }
904 [ # # ]: 0 : if (c == '%') {
905 : 0 : mod = !mod;
906 : : }
907 : : }
908 : :
909 : : return false;
910 : : }
911 : :
912 : 0 : 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 [ # # ]: 0 : __ASSERT_NO_MSG(in_packaged != NULL);
921 : :
922 : 0 : uint8_t *buf = in_packaged;
923 : 0 : uint32_t *buf32 = in_packaged;
924 : 0 : unsigned int args_size, ros_nbr, rws_nbr;
925 : 0 : bool fmt_present = flags & CBPRINTF_PACKAGE_CONVERT_PTR_CHECK ? true : false;
926 : 0 : bool rw_cpy;
927 : 0 : bool ro_cpy;
928 : 0 : struct cbprintf_package_desc *in_desc = in_packaged;
929 : :
930 [ # # ]: 0 : 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 : 0 : ros_nbr = in_desc->ro_str_cnt;
936 [ # # ]: 0 : ro_cpy = ros_nbr &&
937 [ # # ]: 0 : (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 : 0 : rws_nbr = in_desc->rw_str_cnt;
943 [ # # ]: 0 : rw_cpy = rws_nbr > 0 &&
944 [ # # ]: 0 : (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 [ # # ]: 0 : if (!rw_cpy && !ro_cpy) {
951 [ # # ]: 0 : if (cb) {
952 : 0 : cb(in_packaged, in_len, ctx);
953 : : }
954 : :
955 : 0 : 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 : 0 : args_size = in_desc->len * sizeof(int);
963 : :
964 : 0 : int out_len;
965 : :
966 : : /* Pointer to array with string locations. Array starts with read-only
967 : : * string locations.
968 : : */
969 : 0 : const char *fmt = *(const char **)(buf + sizeof(void *));
970 : 0 : uint8_t *str_pos = &buf[args_size];
971 : 0 : size_t strl_cnt = 0;
972 : :
973 : : /* If null destination, just calculate output length. */
974 [ # # ]: 0 : if (cb == NULL) {
975 : 0 : out_len = (int)in_len;
976 [ # # ]: 0 : if (ro_cpy) {
977 [ # # ]: 0 : for (unsigned int i = 0; i < ros_nbr; i++) {
978 : 0 : const char *str = *(const char **)&buf32[*str_pos];
979 : 0 : int len = append_string(cb, NULL, str, 0);
980 : :
981 : : /* If possible store calculated string length. */
982 [ # # ]: 0 : if (strl && strl_cnt < strl_len) {
983 : 0 : strl[strl_cnt++] = (uint16_t)len;
984 : : }
985 : 0 : out_len += len;
986 : 0 : str_pos++;
987 : : }
988 : : } else {
989 : 0 : str_pos += ros_nbr;
990 : : }
991 : :
992 : 0 : 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 [ # # ]: 0 : for (unsigned int i = 0; i < rws_nbr; i++) {
998 : 0 : uint8_t arg_idx = *str_pos++;
999 : 0 : uint8_t arg_pos = *str_pos++;
1000 : 0 : const char *str = *(const char **)&buf32[arg_pos];
1001 : 0 : bool is_ro = ptr_in_rodata(str);
1002 : 0 : int len;
1003 : :
1004 [ # # ]: 0 : if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1005 [ # # ]: 0 : fmt_present && is_ptr(fmt, arg_idx)) {
1006 : 0 : 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 : 0 : out_len -= 2;
1014 : 0 : continue;
1015 : : }
1016 : :
1017 [ # # ]: 0 : if (is_ro) {
1018 [ # # ]: 0 : if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1019 : 0 : goto calculate_string_length;
1020 : : } else {
1021 [ # # ]: 0 : out_len -= drop_ro_str_pos ? 2 : 1;
1022 : : }
1023 [ # # ]: 0 : } else if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1024 : 0 : calculate_string_length:
1025 : 0 : len = append_string(cb, NULL, str, 0);
1026 : :
1027 : : /* If possible store calculated string length. */
1028 [ # # ]: 0 : if (strl && strl_cnt < strl_len) {
1029 : 0 : strl[strl_cnt++] = (uint16_t)len;
1030 : : }
1031 : : /* string length decremented by 1 because argument
1032 : : * index is dropped.
1033 : : */
1034 : 0 : out_len += (len - 1);
1035 : : }
1036 : : }
1037 : :
1038 : 0 : return out_len;
1039 : : }
1040 : :
1041 : 0 : struct cbprintf_package_desc out_desc;
1042 : : /* At least one is copied in. */
1043 : 0 : uint8_t cpy_str_pos[16];
1044 : : /* Up to one will be kept since if both types are kept it returns earlier. */
1045 : 0 : uint8_t keep_str_pos[16];
1046 : 0 : uint8_t scpy_cnt;
1047 : 0 : uint8_t keep_cnt;
1048 : 0 : uint8_t *dst;
1049 : 0 : 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 [ # # ]: 0 : if (ro_cpy) {
1056 : : scpy_cnt = ros_nbr;
1057 : : keep_cnt = 0;
1058 : : dst = cpy_str_pos;
1059 [ # # # # ]: 0 : } 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 : 0 : scpy_cnt = 0;
1065 : 0 : keep_cnt = 0;
1066 : 0 : dst = NULL;
1067 : : }
1068 [ # # ]: 0 : if (dst) {
1069 : 0 : memcpy(dst, str_pos, ros_nbr);
1070 : : }
1071 : 0 : 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 [ # # ]: 0 : for (unsigned int i = 0; i < rws_nbr; i++) {
1078 : 0 : uint8_t arg_idx = *str_pos++;
1079 : 0 : uint8_t arg_pos = *str_pos++;
1080 : 0 : const char *str = *(const char **)&buf32[arg_pos];
1081 : 0 : bool is_ro = ptr_in_rodata(str);
1082 : :
1083 [ # # ]: 0 : if (IS_ENABLED(CONFIG_CBPRINTF_CONVERT_CHECK_PTR) &&
1084 [ # # ]: 0 : fmt_present && is_ptr(fmt, arg_idx)) {
1085 : 0 : continue;
1086 : : }
1087 : :
1088 [ # # ]: 0 : if (is_ro) {
1089 [ # # ]: 0 : if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
1090 [ # # ]: 0 : __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1091 : 0 : cpy_str_pos[scpy_cnt++] = arg_pos;
1092 [ # # ]: 0 : } else if (flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
1093 [ # # ]: 0 : __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1094 : 0 : keep_str_pos[keep_cnt++] = arg_pos;
1095 : : } else {
1096 : : /* Drop information about ro_str location. */
1097 : : }
1098 : : } else {
1099 [ # # ]: 0 : if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
1100 [ # # ]: 0 : __ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
1101 : 0 : cpy_str_pos[scpy_cnt++] = arg_pos;
1102 : : } else {
1103 [ # # ]: 0 : __ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
1104 : 0 : keep_str_pos[keep_cnt++] = arg_idx;
1105 : 0 : keep_str_pos[keep_cnt++] = arg_pos;
1106 : : }
1107 : : }
1108 : : }
1109 : :
1110 : : /* Set amount of strings appended to the package. */
1111 : 0 : out_desc.len = in_desc->len;
1112 : 0 : out_desc.str_cnt = in_desc->str_cnt + scpy_cnt;
1113 [ # # ]: 0 : out_desc.rw_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) ? 0 : (keep_cnt / 2);
1114 [ # # ]: 0 : out_desc.ro_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) ? 0 :
1115 [ # # ]: 0 : ((flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) ? keep_cnt : 0);
1116 : :
1117 : : /* Temporary overwrite input descriptor to allow bulk transfer */
1118 : 0 : struct cbprintf_package_desc in_desc_backup = *in_desc;
1119 : 0 : *in_desc = out_desc;
1120 : :
1121 : : /* Copy package header and arguments. */
1122 : 0 : rv = cb(in_packaged, args_size, ctx);
1123 [ # # ]: 0 : if (rv < 0) {
1124 : : return rv;
1125 : : }
1126 : 0 : out_len = rv;
1127 : : /* Restore input descriptor. */
1128 : 0 : *in_desc = in_desc_backup;
1129 : :
1130 : : /* Copy string positions which are kept. */
1131 : 0 : rv = cb(keep_str_pos, keep_cnt, ctx);
1132 [ # # ]: 0 : if (rv < 0) {
1133 : : return rv;
1134 : : }
1135 : 0 : out_len += rv;
1136 : :
1137 : : /* Copy appended strings from source package to destination. */
1138 : 0 : size_t strs_len = in_len - (args_size + ros_nbr + 2 * rws_nbr);
1139 : :
1140 : 0 : rv = cb(str_pos, strs_len, ctx);
1141 [ # # ]: 0 : if (rv < 0) {
1142 : : return rv;
1143 : : }
1144 : 0 : out_len += rv;
1145 : :
1146 : : /* Append strings */
1147 [ # # ]: 0 : for (unsigned int i = 0; i < scpy_cnt; i++) {
1148 : 0 : uint8_t loc = cpy_str_pos[i];
1149 : 0 : const char *str = *(const char **)&buf32[loc];
1150 [ # # ]: 0 : uint16_t str_len = strl ? strl[i] : 0;
1151 : :
1152 : 0 : rv = cb(&loc, 1, ctx);
1153 [ # # ]: 0 : if (rv < 0) {
1154 : 0 : return rv;
1155 : : }
1156 : 0 : out_len += rv;
1157 : :
1158 : 0 : rv = append_string(cb, ctx, str, str_len);
1159 [ # # ]: 0 : if (rv < 0) {
1160 : 0 : return rv;
1161 : : }
1162 : 0 : out_len += rv;
1163 : : }
1164 : :
1165 : : /* Empty call (can be interpreted as flushing) */
1166 : 0 : (void)cb(NULL, 0, ctx);
1167 : :
1168 : 0 : return out_len;
1169 : : }
|
