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 <linker/utils.h>
12 : : #include <sys/cbprintf.h>
13 : : #include <sys/types.h>
14 : : #include <sys/util.h>
15 : : #include <sys/__assert.h>
16 : :
17 : :
18 : : /**
19 : : * @brief Check if address is in read only section.
20 : : *
21 : : * @param addr Address.
22 : : *
23 : : * @return True if address identified within read only section.
24 : : */
25 : 0 : static inline bool ptr_in_rodata(const char *addr)
26 : : {
27 : : #if defined(CBPRINTF_VIA_UNIT_TEST)
28 : : /* Unit test is X86 (or other host) but not using Zephyr
29 : : * linker scripts.
30 : : */
31 : : return false;
32 : : #else
33 : 0 : return linker_is_in_rodata(addr);
34 : : #endif
35 : : }
36 : :
37 : : /*
38 : : * va_list creation
39 : : */
40 : :
41 : : #if defined(__aarch64__)
42 : : /*
43 : : * Reference:
44 : : *
45 : : * Procedure Call Standard for the ARM 64-bit Architecture
46 : : */
47 : :
48 : : struct __va_list {
49 : : void *__stack;
50 : : void *__gr_top;
51 : : void *__vr_top;
52 : : int __gr_offs;
53 : : int __vr_offs;
54 : : };
55 : :
56 : : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
57 : : "architecture specific support is wrong");
58 : :
59 : : static int cbprintf_via_va_list(cbprintf_cb out,
60 : : cbvprintf_exteral_formatter_func formatter,
61 : : void *ctx,
62 : : const char *fmt, void *buf)
63 : : {
64 : : union {
65 : : va_list ap;
66 : : struct __va_list __ap;
67 : : } u;
68 : :
69 : : /* create a valid va_list with our buffer */
70 : : u.__ap.__stack = buf;
71 : : u.__ap.__gr_top = NULL;
72 : : u.__ap.__vr_top = NULL;
73 : : u.__ap.__gr_offs = 0;
74 : : u.__ap.__vr_offs = 0;
75 : :
76 : : return formatter(out, ctx, fmt, u.ap);
77 : : }
78 : :
79 : : #elif defined(__x86_64__)
80 : : /*
81 : : * Reference:
82 : : *
83 : : * System V Application Binary Interface
84 : : * AMD64 Architecture Processor Supplement
85 : : */
86 : :
87 : : struct __va_list {
88 : : unsigned int gp_offset;
89 : : unsigned int fp_offset;
90 : : void *overflow_arg_area;
91 : : void *reg_save_area;
92 : : };
93 : :
94 : : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
95 : : "architecture specific support is wrong");
96 : :
97 : : static int cbprintf_via_va_list(cbprintf_cb out,
98 : : cbvprintf_exteral_formatter_func formatter,
99 : : void *ctx,
100 : : const char *fmt, void *buf)
101 : : {
102 : : union {
103 : : va_list ap;
104 : : struct __va_list __ap;
105 : : } u;
106 : :
107 : : /* create a valid va_list with our buffer */
108 : : u.__ap.overflow_arg_area = buf;
109 : : u.__ap.reg_save_area = NULL;
110 : : u.__ap.gp_offset = (6 * 8);
111 : : u.__ap.fp_offset = (6 * 8 + 16 * 16);
112 : :
113 : : return formatter(out, ctx, fmt, u.ap);
114 : : }
115 : :
116 : : #elif defined(__xtensa__)
117 : : /*
118 : : * Reference:
119 : : *
120 : : * gcc source code (gcc/config/xtensa/xtensa.c)
121 : : * xtensa_build_builtin_va_list(), xtensa_va_start(),
122 : : * xtensa_gimplify_va_arg_expr()
123 : : */
124 : :
125 : : struct __va_list {
126 : : void *__va_stk;
127 : : void *__va_reg;
128 : : int __va_ndx;
129 : : };
130 : :
131 : : BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
132 : : "architecture specific support is wrong");
133 : :
134 : : static int cbprintf_via_va_list(cbprintf_cb out,
135 : : cbvprintf_exteral_formatter_func formatter,
136 : : void *ctx,
137 : : const char *fmt, void *buf)
138 : : {
139 : : union {
140 : : va_list ap;
141 : : struct __va_list __ap;
142 : : } u;
143 : :
144 : : /* create a valid va_list with our buffer */
145 : : u.__ap.__va_stk = (char *)buf - 32;
146 : : u.__ap.__va_reg = NULL;
147 : : u.__ap.__va_ndx = (6 + 2) * 4;
148 : :
149 : : return formatter(out, ctx, fmt, u.ap);
150 : : }
151 : :
152 : : #else
153 : : /*
154 : : * Default implementation shared by many architectures like
155 : : * 32-bit ARM and Intel.
156 : : *
157 : : * We assume va_list is a simple pointer.
158 : : */
159 : :
160 : : BUILD_ASSERT(sizeof(va_list) == sizeof(void *),
161 : : "architecture specific support is needed");
162 : :
163 : 0 : static int cbprintf_via_va_list(cbprintf_cb out,
164 : : cbvprintf_exteral_formatter_func formatter,
165 : : void *ctx,
166 : : const char *fmt, void *buf)
167 : : {
168 : : union {
169 : : va_list ap;
170 : : void *ptr;
171 : : } u;
172 : :
173 : 0 : u.ptr = buf;
174 : :
175 : 0 : return formatter(out, ctx, fmt, u.ap);
176 : : }
177 : :
178 : : #endif
179 : :
180 : 0 : static int z_strncpy(char *dst, const char *src, size_t num)
181 : : {
182 [ # # ]: 0 : for (size_t i = 0; i < num; i++) {
183 : 0 : dst[i] = src[i];
184 [ # # ]: 0 : if (src[i] == '\0') {
185 : 0 : return i + 1;
186 : : }
187 : : }
188 : :
189 : 0 : return -ENOSPC;
190 : : }
191 : :
192 : 0 : static size_t get_package_len(void *packaged)
193 : : {
194 [ # # ]: 0 : __ASSERT_NO_MSG(packaged != NULL);
195 : :
196 : 0 : uint8_t *buf = packaged;
197 : 0 : uint8_t *start = buf;
198 : : unsigned int args_size, s_nbr, ros_nbr;
199 : :
200 : 0 : args_size = buf[0] * sizeof(int);
201 : 0 : s_nbr = buf[1];
202 : 0 : ros_nbr = buf[2];
203 : :
204 : : /* Move beyond args. */
205 : 0 : buf += args_size;
206 : :
207 : : /* Move beyond read-only string indexes array. */
208 : 0 : buf += ros_nbr;
209 : :
210 : : /* Move beyond strings appended to the package. */
211 [ # # ]: 0 : for (int i = 0; i < s_nbr; i++) {
212 : 0 : buf++;
213 : 0 : buf += strlen((const char *)buf) + 1;
214 : : }
215 : :
216 : 0 : return (size_t)(uintptr_t)(buf - start);
217 : : }
218 : :
219 : 0 : static int append_string(void *dst, size_t max, const char *str, uint16_t strl)
220 : : {
221 : 0 : char *buf = dst;
222 : :
223 [ # # ]: 0 : if (dst == NULL) {
224 : 0 : return 1 + strlen(str);
225 : : }
226 : :
227 [ # # ]: 0 : if (strl) {
228 : 0 : memcpy(dst, str, strl);
229 : :
230 : 0 : return strl;
231 : : }
232 : :
233 : 0 : return z_strncpy(buf, str, max);
234 : : }
235 : :
236 : 0 : int cbvprintf_package(void *packaged, size_t len, uint32_t flags,
237 : : const char *fmt, va_list ap)
238 : : {
239 : : /*
240 : : * Internally, a byte is used to store location of a string argument within a
241 : : * package. MSB bit is set if string is read-only so effectively 7 bits are
242 : : * used for index, which should be enough.
243 : : */
244 : : #define STR_POS_RO_FLAG BIT(7)
245 : : #define STR_POS_MASK BIT_MASK(7)
246 : :
247 : : /* Buffer offset abstraction for better code clarity. */
248 : : #define BUF_OFFSET ((uintptr_t)buf - (uintptr_t)buf0)
249 : :
250 : 0 : uint8_t *buf0 = packaged; /* buffer start (may be NULL) */
251 : 0 : uint8_t *buf = buf0; /* current buffer position */
252 : : unsigned int size; /* current argument's size */
253 : : unsigned int align; /* current argument's required alignment */
254 : : uint8_t str_ptr_pos[16]; /* string pointer positions */
255 : 0 : unsigned int s_idx = 0; /* index into str_ptr_pos[] */
256 : 0 : unsigned int s_rw_cnt = 0; /* number of rw strings */
257 : 0 : unsigned int s_ro_cnt = 0; /* number of ro strings */
258 : : unsigned int i;
259 : : const char *s;
260 : 0 : bool parsing = false;
261 : : /* Flag indicates that rw strings are stored as array with positions,
262 : : * instead of appending them to the package.
263 : : */
264 : 0 : bool rws_pos_en = !!(flags & CBPRINTF_PACKAGE_ADD_RW_STR_POS);
265 : : /* Get number of first read only strings present in the string.
266 : : * There is always at least 1 (fmt) but flags can indicate more, e.g
267 : : * fixed prefix appended to all strings.
268 : : */
269 : 0 : int fros_cnt = 1 + Z_CBPRINTF_PACKAGE_FIRST_RO_STR_CNT_GET(flags);
270 : :
271 : : /* Buffer must be aligned at least to size of a pointer. */
272 [ # # ]: 0 : if ((uintptr_t)packaged % sizeof(void *)) {
273 : 0 : return -EFAULT;
274 : : }
275 : :
276 : : #if defined(__xtensa__)
277 : : /* Xtensa requires package to be 16 bytes aligned. */
278 : : if ((uintptr_t)packaged % CBPRINTF_PACKAGE_ALIGNMENT) {
279 : : return -EFAULT;
280 : : }
281 : : #endif
282 : :
283 : : /*
284 : : * Make room to store the arg list size and the number of
285 : : * appended strings. They both occupy 1 byte each.
286 : : *
287 : : * Given the next value to store is the format string pointer
288 : : * which is guaranteed to be at least 4 bytes, we just reserve
289 : : * a pointer size for the above to preserve alignment.
290 : : */
291 : 0 : buf += sizeof(char *);
292 : :
293 : : /*
294 : : * When buf0 is NULL we don't store anything.
295 : : * Instead we count the needed space to store the data.
296 : : * In this case, incoming len argument indicates the anticipated
297 : : * buffer "misalignment" offset.
298 : : */
299 [ # # ]: 0 : if (buf0 == NULL) {
300 : 0 : buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
301 : : /*
302 : : * The space to store the data is represented by both the
303 : : * buffer offset as well as the extra string data to be
304 : : * appended. When only figuring out the needed space, we
305 : : * don't append anything. Instead, we reuse the len variable
306 : : * to sum the size of that data.
307 : : *
308 : : * Also, we subtract any initial misalignment offset from
309 : : * the total as this won't be part of the buffer. To avoid
310 : : * going negative with an unsigned variable, we add an offset
311 : : * (CBPRINTF_PACKAGE_ALIGNMENT) that will be removed before
312 : : * returning.
313 : : */
314 : 0 : len = CBPRINTF_PACKAGE_ALIGNMENT - (len % CBPRINTF_PACKAGE_ALIGNMENT);
315 : : }
316 : :
317 : : /*
318 : : * Otherwise we must ensure we can store at least
319 : : * the pointer to the format string itself.
320 : : */
321 [ # # # # ]: 0 : if (buf0 != NULL && BUF_OFFSET + sizeof(char *) > len) {
322 : 0 : return -ENOSPC;
323 : : }
324 : :
325 : : /*
326 : : * Then process the format string itself.
327 : : * Here we branch directly into the code processing strings
328 : : * which is in the middle of the following while() loop. That's the
329 : : * reason for the post-decrement on fmt as it will be incremented
330 : : * prior to the next (actually first) round of that loop.
331 : : */
332 : 0 : s = fmt--;
333 : 0 : align = VA_STACK_ALIGN(char *);
334 : 0 : size = sizeof(char *);
335 : 0 : goto process_string;
336 : :
337 : : /* Scan the format string */
338 [ # # ]: 0 : while (*++fmt != '\0') {
339 [ # # ]: 0 : if (!parsing) {
340 [ # # ]: 0 : if (*fmt == '%') {
341 : 0 : parsing = true;
342 : 0 : align = VA_STACK_ALIGN(int);
343 : 0 : size = sizeof(int);
344 : : }
345 : 0 : continue;
346 : : }
347 [ # # # # : 0 : switch (*fmt) {
# # # # #
# ]
348 : 0 : case '%':
349 : 0 : parsing = false;
350 : 0 : continue;
351 : :
352 : 0 : case '#':
353 : : case '-':
354 : : case '+':
355 : : case ' ':
356 : : case '0':
357 : : case '1':
358 : : case '2':
359 : : case '3':
360 : : case '4':
361 : : case '5':
362 : : case '6':
363 : : case '7':
364 : : case '8':
365 : : case '9':
366 : : case '.':
367 : : case 'h':
368 : : case 'l':
369 : : case 'L':
370 : 0 : continue;
371 : :
372 : 0 : case '*':
373 : 0 : break;
374 : :
375 : 0 : case 'j':
376 : 0 : align = VA_STACK_ALIGN(intmax_t);
377 : 0 : size = sizeof(intmax_t);
378 : 0 : continue;
379 : :
380 : 0 : case 'z':
381 : 0 : align = VA_STACK_ALIGN(size_t);
382 : 0 : size = sizeof(size_t);
383 : 0 : continue;
384 : :
385 : 0 : case 't':
386 : 0 : align = VA_STACK_ALIGN(ptrdiff_t);
387 : 0 : size = sizeof(ptrdiff_t);
388 : 0 : continue;
389 : :
390 : 0 : case 'c':
391 : : case 'd':
392 : : case 'i':
393 : : case 'o':
394 : : case 'u':
395 : : case 'x':
396 : : case 'X':
397 [ # # ]: 0 : if (fmt[-1] == 'l') {
398 [ # # ]: 0 : if (fmt[-2] == 'l') {
399 : 0 : align = VA_STACK_ALIGN(long long);
400 : 0 : size = sizeof(long long);
401 : : } else {
402 : 0 : align = VA_STACK_ALIGN(long);
403 : 0 : size = sizeof(long);
404 : : }
405 : : }
406 : 0 : parsing = false;
407 : 0 : break;
408 : :
409 : 0 : case 's':
410 : : case 'p':
411 : : case 'n':
412 : 0 : align = VA_STACK_ALIGN(void *);
413 : 0 : size = sizeof(void *);
414 : 0 : parsing = false;
415 : 0 : break;
416 : :
417 : 0 : case 'a':
418 : : case 'A':
419 : : case 'e':
420 : : case 'E':
421 : : case 'f':
422 : : case 'F':
423 : : case 'g':
424 : : case 'G': {
425 : : /*
426 : : * Handle floats separately as they may be
427 : : * held in a different register set.
428 : : */
429 : : union { double d; long double ld; } v;
430 : :
431 [ # # ]: 0 : if (fmt[-1] == 'L') {
432 : 0 : v.ld = va_arg(ap, long double);
433 : 0 : align = VA_STACK_ALIGN(long double);
434 : 0 : size = sizeof(long double);
435 : : } else {
436 : 0 : v.d = va_arg(ap, double);
437 : 0 : align = VA_STACK_ALIGN(double);
438 : 0 : size = sizeof(double);
439 : : }
440 : : /* align destination buffer location */
441 : 0 : buf = (void *) ROUND_UP(buf, align);
442 [ # # ]: 0 : if (buf0 != NULL) {
443 : : /* make sure it fits */
444 [ # # ]: 0 : if (BUF_OFFSET + size > len) {
445 : 0 : return -ENOSPC;
446 : : }
447 : : if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
448 : : memcpy(buf, &v, size);
449 [ # # ]: 0 : } else if (fmt[-1] == 'L') {
450 : 0 : *(long double *)buf = v.ld;
451 : : } else {
452 : 0 : *(double *)buf = v.d;
453 : : }
454 : : }
455 : 0 : buf += size;
456 : 0 : parsing = false;
457 : 0 : continue;
458 : : }
459 : :
460 : 0 : default:
461 : 0 : parsing = false;
462 : 0 : continue;
463 : : }
464 : :
465 : : /* align destination buffer location */
466 : 0 : buf = (void *) ROUND_UP(buf, align);
467 : :
468 : : /* make sure the data fits */
469 [ # # # # ]: 0 : if (buf0 != NULL && BUF_OFFSET + size > len) {
470 : 0 : return -ENOSPC;
471 : : }
472 : :
473 : : /* copy va_list data over to our buffer */
474 [ # # ]: 0 : if (*fmt == 's') {
475 : 0 : s = va_arg(ap, char *);
476 : 0 : process_string:
477 [ # # ]: 0 : if (buf0 != NULL) {
478 : 0 : *(const char **)buf = s;
479 : : }
480 : :
481 [ # # # # ]: 0 : bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
482 : 0 : bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
483 : :
484 [ # # # # ]: 0 : if (is_ro && !do_ro) {
485 : : /* nothing to do */
486 : : } else {
487 : 0 : uint32_t s_ptr_idx = BUF_OFFSET / sizeof(int);
488 : :
489 : : /*
490 : : * In the do_ro case we must consider
491 : : * room for possible STR_POS_RO_FLAG.
492 : : * Otherwise the index range is 8 bits
493 : : * and any overflow is caught later.
494 : : */
495 [ # # # # ]: 0 : if (do_ro && s_ptr_idx > STR_POS_MASK) {
496 : 0 : __ASSERT(false, "String with too many arguments");
497 : 0 : return -EINVAL;
498 : : }
499 : :
500 [ # # ]: 0 : if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
501 : 0 : __ASSERT(false, "str_ptr_pos[] too small");
502 : 0 : return -EINVAL;
503 : : }
504 : :
505 [ # # ]: 0 : if (buf0 != NULL) {
506 : : /*
507 : : * Remember string pointer location.
508 : : * We will append non-ro strings later.
509 : : */
510 : 0 : str_ptr_pos[s_idx] = s_ptr_idx;
511 [ # # ]: 0 : if (is_ro) {
512 : : /* flag read-only string. */
513 : 0 : str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
514 : 0 : s_ro_cnt++;
515 : : } else {
516 : 0 : s_rw_cnt++;
517 : : }
518 [ # # # # ]: 0 : } else if (is_ro || rws_pos_en) {
519 : : /*
520 : : * Add only pointer position prefix
521 : : * when counting strings.
522 : : */
523 : 0 : len += 1;
524 : : } else {
525 : : /*
526 : : * Add the string length, the final '\0'
527 : : * and size of the pointer position prefix.
528 : : */
529 : 0 : len += strlen(s) + 1 + 1;
530 : : }
531 : :
532 : 0 : s_idx++;
533 : : }
534 : 0 : buf += sizeof(char *);
535 [ # # ]: 0 : } else if (size == sizeof(int)) {
536 : 0 : int v = va_arg(ap, int);
537 : :
538 [ # # ]: 0 : if (buf0 != NULL) {
539 : 0 : *(int *)buf = v;
540 : : }
541 : 0 : buf += sizeof(int);
542 [ # # ]: 0 : } else if (size == sizeof(long)) {
543 : 0 : long v = va_arg(ap, long);
544 : :
545 [ # # ]: 0 : if (buf0 != NULL) {
546 : 0 : *(long *)buf = v;
547 : : }
548 : 0 : buf += sizeof(long);
549 [ # # ]: 0 : } else if (size == sizeof(long long)) {
550 : 0 : long long v = va_arg(ap, long long);
551 : :
552 [ # # ]: 0 : if (buf0 != NULL) {
553 : : if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
554 : : memcpy(buf, &v, sizeof(long long));
555 : : } else {
556 : 0 : *(long long *)buf = v;
557 : : }
558 : : }
559 : 0 : buf += sizeof(long long);
560 : : } else {
561 : 0 : __ASSERT(false, "unexpected size %u", size);
562 : 0 : return -EINVAL;
563 : : }
564 : : }
565 : :
566 : : /*
567 : : * We remember the size of the argument list as a multiple of
568 : : * sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
569 : : * worth of va_list, or about 127 arguments on a 64-bit system
570 : : * (twice that on 32-bit systems). That ought to be good enough.
571 : : */
572 [ # # ]: 0 : if (BUF_OFFSET / sizeof(int) > 255) {
573 : 0 : __ASSERT(false, "too many format args");
574 : 0 : return -EINVAL;
575 : : }
576 : :
577 : : /*
578 : : * If all we wanted was to count required buffer size
579 : : * then we have it now.
580 : : */
581 [ # # ]: 0 : if (buf0 == NULL) {
582 : 0 : return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
583 : : }
584 : :
585 : : /* Clear our buffer header. We made room for it initially. */
586 : 0 : *(char **)buf0 = NULL;
587 : :
588 : : /* Record end of argument list. */
589 : 0 : buf0[0] = BUF_OFFSET / sizeof(int);
590 : :
591 [ # # ]: 0 : if (rws_pos_en) {
592 : : /* Strings are appended, update location counter. */
593 : 0 : buf0[1] = 0;
594 : 0 : buf0[3] = s_rw_cnt;
595 : : } else {
596 : : /* Strings are appended, update append counter. */
597 : 0 : buf0[1] = s_rw_cnt;
598 : 0 : buf0[3] = 0;
599 : : }
600 : :
601 : 0 : buf0[2] = s_ro_cnt;
602 : :
603 : : /* Store strings pointer locations of read only strings. */
604 [ # # ]: 0 : if (s_ro_cnt) {
605 [ # # ]: 0 : for (i = 0; i < s_idx; i++) {
606 [ # # ]: 0 : if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
607 : 0 : continue;
608 : : }
609 : :
610 : 0 : uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
611 : :
612 : : /* make sure it fits */
613 [ # # ]: 0 : if (BUF_OFFSET + 1 > len) {
614 : 0 : return -ENOSPC;
615 : : }
616 : : /* store the pointer position prefix */
617 : 0 : *buf++ = pos;
618 : : }
619 : : }
620 : :
621 : : /* Store strings prefixed by their pointer location. */
622 [ # # ]: 0 : for (i = 0; i < s_idx; i++) {
623 : : /* Process only RW strings. */
624 [ # # # # ]: 0 : if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
625 : 0 : continue;
626 : : }
627 : :
628 [ # # ]: 0 : if (rws_pos_en) {
629 : 0 : size = 0;
630 : : } else {
631 : : /* retrieve the string pointer */
632 : 0 : s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
633 : : /* clear the in-buffer pointer (less entropy if compressed) */
634 : 0 : *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
635 : : /* find the string length including terminating '\0' */
636 : 0 : size = strlen(s) + 1;
637 : : }
638 : :
639 : : /* make sure it fits */
640 [ # # ]: 0 : if (BUF_OFFSET + 1 + size > len) {
641 : 0 : return -ENOSPC;
642 : : }
643 : : /* store the pointer position prefix */
644 : 0 : *buf++ = str_ptr_pos[i];
645 : : /* copy the string with its terminating '\0' */
646 : 0 : memcpy(buf, s, size);
647 : 0 : buf += size;
648 : : }
649 : :
650 : : /*
651 : : * TODO: remove pointers for appended strings since they're useless.
652 : : * TODO: explore leveraging same mechanism to remove alignment padding
653 : : */
654 : :
655 : 0 : return BUF_OFFSET;
656 : :
657 : : #undef BUF_OFFSET
658 : : #undef STR_POS_RO_FLAG
659 : : #undef STR_POS_MASK
660 : : }
661 : :
662 : 0 : int cbprintf_package(void *packaged, size_t len, uint32_t flags,
663 : : const char *format, ...)
664 : : {
665 : : va_list ap;
666 : : int ret;
667 : :
668 : 0 : va_start(ap, format);
669 : 0 : ret = cbvprintf_package(packaged, len, flags, format, ap);
670 : 0 : va_end(ap);
671 : 0 : return ret;
672 : : }
673 : :
674 : 0 : int cbpprintf_external(cbprintf_cb out,
675 : : cbvprintf_exteral_formatter_func formatter,
676 : : void *ctx, void *packaged)
677 : : {
678 : 0 : uint8_t *buf = packaged;
679 : : char *fmt, *s, **ps;
680 : : unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
681 : :
682 [ # # ]: 0 : if (buf == NULL) {
683 : 0 : return -EINVAL;
684 : : }
685 : :
686 : : /* Retrieve the size of the arg list and number of strings. */
687 : 0 : args_size = buf[0] * sizeof(int);
688 : 0 : s_nbr = buf[1];
689 : 0 : ros_nbr = buf[2];
690 : 0 : rws_nbr = buf[3];
691 : :
692 : : /* Locate the string table */
693 : 0 : s = (char *)(buf + args_size + ros_nbr + rws_nbr);
694 : :
695 : : /*
696 : : * Patch in string pointers.
697 : : */
698 [ # # ]: 0 : for (i = 0; i < s_nbr; i++) {
699 : : /* Locate pointer location for this string */
700 : 0 : s_idx = *(uint8_t *)s++;
701 : 0 : ps = (char **)(buf + s_idx * sizeof(int));
702 : : /* update the pointer with current string location */
703 : 0 : *ps = s;
704 : : /* move to next string */
705 : 0 : s += strlen(s) + 1;
706 : : }
707 : :
708 : : /* Retrieve format string */
709 : 0 : fmt = ((char **)buf)[1];
710 : :
711 : : /* skip past format string pointer */
712 : 0 : buf += sizeof(char *) * 2;
713 : :
714 : : /* Turn this into a va_list and print it */
715 : 0 : return cbprintf_via_va_list(out, formatter, ctx, fmt, buf);
716 : : }
717 : :
718 : 0 : int cbprintf_package_copy(void *in_packaged,
719 : : size_t in_len,
720 : : void *packaged,
721 : : size_t len,
722 : : uint32_t flags,
723 : : uint16_t *strl,
724 : : size_t strl_len)
725 : : {
726 [ # # ]: 0 : __ASSERT_NO_MSG(in_packaged != NULL);
727 : :
728 : 0 : uint8_t *buf = in_packaged;
729 : 0 : uint32_t *buf32 = in_packaged;
730 : : unsigned int args_size, ros_nbr, rws_nbr;
731 : : bool rw_cpy;
732 : : bool ro_cpy;
733 : :
734 [ # # ]: 0 : in_len != 0 ? in_len : get_package_len(in_packaged);
735 : :
736 : : /* Get number of RO string indexes in the package and check if copying
737 : : * includes appending those strings.
738 : : */
739 : 0 : ros_nbr = buf[2];
740 [ # # ]: 0 : ro_cpy = ros_nbr &&
741 [ # # ]: 0 : (flags & CBPRINTF_PACKAGE_COPY_RO_STR) == CBPRINTF_PACKAGE_COPY_RO_STR;
742 : :
743 : : /* Get number of RW string indexes in the package and check if copying
744 : : * includes appending those strings.
745 : : */
746 : 0 : rws_nbr = buf[3];
747 [ # # ]: 0 : rw_cpy = rws_nbr > 0 &&
748 [ # # ]: 0 : (flags & CBPRINTF_PACKAGE_COPY_RW_STR) == CBPRINTF_PACKAGE_COPY_RW_STR;
749 : :
750 : :
751 : : /* If flags are not set or appending request without rw string indexes
752 : : * present is chosen, just do a simple copy (or length calculation).
753 : : * Assuming that it is the most common case.
754 : : */
755 [ # # # # ]: 0 : if (!rw_cpy && !ro_cpy) {
756 [ # # ]: 0 : if (packaged) {
757 : 0 : memcpy(packaged, in_packaged, in_len);
758 : : }
759 : :
760 : 0 : return in_len;
761 : : }
762 : :
763 : : /* If we got here, it means that coping will be more complex and will be
764 : : * done with strings appending.
765 : : * Retrieve the size of the arg list.
766 : : */
767 : 0 : args_size = buf[0] * sizeof(int);
768 : :
769 : 0 : size_t out_len = in_len;
770 : :
771 : : /* Pointer to array with string locations. Array starts with read-only
772 : : * string locations.
773 : : */
774 : 0 : uint8_t *str_pos = &buf[args_size];
775 : 0 : size_t strl_cnt = 0;
776 : :
777 : : /* If null destination, just calculate output length. */
778 [ # # ]: 0 : if (packaged == NULL) {
779 [ # # ]: 0 : if (ro_cpy) {
780 [ # # ]: 0 : for (int i = 0; i < ros_nbr; i++) {
781 : 0 : const char *str = *(const char **)&buf32[*str_pos];
782 : 0 : int len = append_string(NULL, 0, str, 0);
783 : :
784 : : /* If possible store calculated string length. */
785 [ # # # # ]: 0 : if (strl && strl_cnt < strl_len) {
786 : 0 : strl[strl_cnt++] = (uint16_t)len;
787 : : }
788 : 0 : out_len += len;
789 : 0 : str_pos++;
790 : : }
791 : : } else {
792 [ # # # # ]: 0 : if (ros_nbr && flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
793 : 0 : str_pos += ros_nbr;
794 : : }
795 : : }
796 : :
797 : 0 : bool drop_ro_str_pos = !(flags &
798 : : (CBPRINTF_PACKAGE_COPY_KEEP_RO_STR |
799 : : CBPRINTF_PACKAGE_COPY_RO_STR));
800 : :
801 : : /* Handle RW strings. */
802 [ # # ]: 0 : for (int i = 0; i < rws_nbr; i++) {
803 : 0 : const char *str = *(const char **)&buf32[*str_pos];
804 : 0 : bool is_ro = ptr_in_rodata(str);
805 : :
806 [ # # # # ]: 0 : if ((is_ro && flags & CBPRINTF_PACKAGE_COPY_RO_STR) ||
807 [ # # # # ]: 0 : (!is_ro && flags & CBPRINTF_PACKAGE_COPY_RW_STR)) {
808 : 0 : int len = append_string(NULL, 0, str, 0);
809 : :
810 : : /* If possible store calculated string length. */
811 [ # # # # ]: 0 : if (strl && strl_cnt < strl_len) {
812 : 0 : strl[strl_cnt++] = (uint16_t)len;
813 : : }
814 : 0 : out_len += len;
815 : : }
816 : :
817 [ # # # # ]: 0 : if (is_ro && drop_ro_str_pos) {
818 : : /* If read-only string location is dropped decreased
819 : : * length.
820 : : */
821 : 0 : out_len--;
822 : : }
823 : :
824 : 0 : str_pos++;
825 : : }
826 : :
827 : 0 : return out_len;
828 : : }
829 : :
830 : : uint8_t cpy_str_pos[16];
831 : : uint8_t scpy_cnt;
832 : 0 : uint8_t *dst = packaged;
833 : 0 : uint8_t *dst_hdr = packaged;
834 : :
835 : 0 : memcpy(dst, in_packaged, args_size);
836 : 0 : dst += args_size;
837 : :
838 : : /* Pointer to the beginning of string locations in the destination package. */
839 : 0 : uint8_t *dst_str_loc = dst;
840 : :
841 : : /* If read-only strings shall be appended to the output package copy
842 : : * their indexes to the local array, otherwise indicate that indexes
843 : : * shall remain in the output package.
844 : : */
845 [ # # ]: 0 : if (ro_cpy) {
846 : 0 : memcpy(cpy_str_pos, str_pos, ros_nbr);
847 : 0 : scpy_cnt = ros_nbr;
848 : : /* Read only string indexes removed from package. */
849 : 0 : dst_hdr[2] = 0;
850 : 0 : str_pos += ros_nbr;
851 : : } else {
852 : 0 : scpy_cnt = 0;
853 [ # # # # ]: 0 : if (ros_nbr && flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
854 : 0 : memcpy(dst, str_pos, ros_nbr);
855 : 0 : dst += ros_nbr;
856 : 0 : str_pos += ros_nbr;
857 : : } else {
858 : 0 : dst_hdr[2] = 0;
859 : : }
860 : : }
861 : :
862 : : /* Go through read-write strings and identify which shall be appended.
863 : : * Note that there may be read-only strings there. Use address evaluation
864 : : * to determine if strings is read-only.
865 : : */
866 [ # # ]: 0 : for (int i = 0; i < rws_nbr; i++) {
867 : 0 : const char *str = *(const char **)&buf32[*str_pos];
868 : 0 : bool is_ro = ptr_in_rodata(str);
869 : :
870 [ # # ]: 0 : if (is_ro) {
871 [ # # ]: 0 : if (flags & CBPRINTF_PACKAGE_COPY_RO_STR) {
872 : 0 : cpy_str_pos[scpy_cnt++] = *str_pos;
873 [ # # ]: 0 : } else if (flags & CBPRINTF_PACKAGE_COPY_KEEP_RO_STR) {
874 : 0 : *dst++ = *str_pos;
875 : : /* Increment amount of ro locations. */
876 : 0 : dst_hdr[2]++;
877 : : } else {
878 : : /* Drop information about ro_str location. */
879 : : }
880 : : } else {
881 [ # # ]: 0 : if (flags & CBPRINTF_PACKAGE_COPY_RW_STR) {
882 : 0 : cpy_str_pos[scpy_cnt++] = *str_pos;
883 : : } else {
884 : 0 : *dst++ = *str_pos;
885 : : }
886 : : }
887 : 0 : str_pos++;
888 : : }
889 : :
890 : : /* Increment amount of strings appended to the package. */
891 : 0 : dst_hdr[1] += scpy_cnt;
892 : : /* Update number of rw string locations in the package. */
893 : 0 : dst_hdr[3] = (uint8_t)(uintptr_t)(dst - dst_str_loc) - dst_hdr[2];
894 : :
895 : : /* Copy appended strings from source package to destination. */
896 : 0 : size_t strs_len = in_len - (args_size + ros_nbr + rws_nbr);
897 : :
898 : 0 : memcpy(dst, str_pos, strs_len);
899 : :
900 : 0 : dst += strs_len;
901 : :
902 [ # # ]: 0 : if (scpy_cnt == 0) {
903 : 0 : return dst - dst_hdr;
904 : : }
905 : :
906 : : /* Calculate remaining space in the buffer. */
907 : 0 : size_t rem = len - ((size_t)(uintptr_t)(dst - dst_hdr));
908 : :
909 [ # # ]: 0 : if (rem <= scpy_cnt) {
910 : 0 : return -ENOSPC;
911 : : }
912 : :
913 : : /* Append strings */
914 [ # # ]: 0 : for (int i = 0; i < scpy_cnt; i++) {
915 : 0 : uint8_t loc = cpy_str_pos[i];
916 : 0 : const char *str = *(const char **)&buf32[loc];
917 : : int cpy_len;
918 [ # # ]: 0 : uint16_t str_len = strl ? strl[i] : 0;
919 : :
920 : 0 : *dst = loc;
921 : 0 : rem--;
922 : 0 : dst++;
923 : 0 : cpy_len = append_string(dst, rem, str, str_len);
924 : :
925 [ # # ]: 0 : if (cpy_len < 0) {
926 : 0 : return -ENOSPC;
927 : : }
928 : :
929 : 0 : rem -= cpy_len;
930 : 0 : dst += cpy_len;
931 : : }
932 : :
933 : 0 : return len - rem;
934 : : }
|