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 <sys/cbprintf.h>
12 #include <sys/types.h>
13 #include <sys/util.h>
14 #include <sys/__assert.h>
15
16
17 /**
18 * @brief Check if address is in read only section.
19 *
20 * @param addr Address.
21 *
22 * @return True if address identified within read only section.
23 */
ptr_in_rodata(const char * addr)24 static inline bool ptr_in_rodata(const char *addr)
25 {
26 #if defined(CBPRINTF_VIA_UNIT_TEST)
27 /* Unit test is X86 (or other host) but not using Zephyr
28 * linker scripts.
29 */
30 #define RO_START 0
31 #define RO_END 0
32 #elif defined(CONFIG_ARC) || defined(CONFIG_ARM) || defined(CONFIG_X86) \
33 || defined(CONFIG_RISCV) || defined(CONFIG_ARM64) \
34 || defined(CONFIG_NIOS2)
35 extern char __rodata_region_start[];
36 extern char __rodata_region_end[];
37 #define RO_START __rodata_region_start
38 #define RO_END __rodata_region_end
39 #elif defined(CONFIG_XTENSA)
40 extern char _rodata_start[];
41 extern char _rodata_end[];
42 #define RO_START _rodata_start
43 #define RO_END _rodata_end
44 #else
45 #define RO_START 0
46 #define RO_END 0
47 #endif
48
49 return ((addr >= (const char *)RO_START) &&
50 (addr < (const char *)RO_END));
51 }
52
53 /*
54 * va_list creation
55 */
56
57 #if 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,void * ctx,const char * fmt,void * buf)75 static int cbprintf_via_va_list(cbprintf_cb out, void *ctx,
76 const char *fmt, void *buf)
77 {
78 union {
79 va_list ap;
80 struct __va_list __ap;
81 } u;
82
83 /* create a valid va_list with our buffer */
84 u.__ap.__stack = buf;
85 u.__ap.__gr_top = NULL;
86 u.__ap.__vr_top = NULL;
87 u.__ap.__gr_offs = 0;
88 u.__ap.__vr_offs = 0;
89
90 return cbvprintf(out, ctx, fmt, u.ap);
91 }
92
93 #elif defined(__x86_64__)
94 /*
95 * Reference:
96 *
97 * System V Application Binary Interface
98 * AMD64 Architecture Processor Supplement
99 */
100
101 struct __va_list {
102 unsigned int gp_offset;
103 unsigned int fp_offset;
104 void *overflow_arg_area;
105 void *reg_save_area;
106 };
107
108 BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
109 "architecture specific support is wrong");
110
cbprintf_via_va_list(cbprintf_cb out,void * ctx,const char * fmt,void * buf)111 static int cbprintf_via_va_list(cbprintf_cb out, void *ctx,
112 const char *fmt, void *buf)
113 {
114 union {
115 va_list ap;
116 struct __va_list __ap;
117 } u;
118
119 /* create a valid va_list with our buffer */
120 u.__ap.overflow_arg_area = buf;
121 u.__ap.reg_save_area = NULL;
122 u.__ap.gp_offset = (6 * 8);
123 u.__ap.fp_offset = (6 * 8 + 16 * 16);
124
125 return cbvprintf(out, ctx, fmt, u.ap);
126 }
127
128 #elif defined(__xtensa__)
129 /*
130 * Reference:
131 *
132 * gcc source code (gcc/config/xtensa/xtensa.c)
133 * xtensa_build_builtin_va_list(), xtensa_va_start(),
134 * xtensa_gimplify_va_arg_expr()
135 */
136
137 struct __va_list {
138 void *__va_stk;
139 void *__va_reg;
140 int __va_ndx;
141 };
142
143 BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
144 "architecture specific support is wrong");
145
cbprintf_via_va_list(cbprintf_cb out,void * ctx,const char * fmt,void * buf)146 static int cbprintf_via_va_list(cbprintf_cb out, void *ctx,
147 const char *fmt, void *buf)
148 {
149 union {
150 va_list ap;
151 struct __va_list __ap;
152 } u;
153
154 /* create a valid va_list with our buffer */
155 u.__ap.__va_stk = (char *)buf - 32;
156 u.__ap.__va_reg = NULL;
157 u.__ap.__va_ndx = (6 + 2) * 4;
158
159 return cbvprintf(out, ctx, fmt, u.ap);
160 }
161
162 #else
163 /*
164 * Default implementation shared by many architectures like
165 * 32-bit ARM and Intel.
166 *
167 * We assume va_list is a simple pointer.
168 */
169
170 BUILD_ASSERT(sizeof(va_list) == sizeof(void *),
171 "architecture specific support is needed");
172
cbprintf_via_va_list(cbprintf_cb out,void * ctx,const char * fmt,void * buf)173 static int cbprintf_via_va_list(cbprintf_cb out, void *ctx,
174 const char *fmt, void *buf)
175 {
176 union {
177 va_list ap;
178 void *ptr;
179 } u;
180
181 u.ptr = buf;
182
183 return cbvprintf(out, ctx, fmt, u.ap);
184 }
185
186 #endif
187
cbvprintf_package(void * packaged,size_t len,uint32_t flags,const char * fmt,va_list ap)188 int cbvprintf_package(void *packaged, size_t len, uint32_t flags,
189 const char *fmt, va_list ap)
190 {
191 /* Internally, byte is used to store location of a string argument within a
192 * package. MSB bit is set if string is read-only so effectively 7 bits are
193 * used for index, which should be enough.
194 */
195 #define CBPRINTF_STR_POS_RO_FLAG BIT(7)
196 #define CBPRINTF_STR_POS_MASK BIT_MASK(7)
197
198 char *buf = packaged, *buf0 = buf;
199 unsigned int align, size, i, s_idx = 0, s_rw_cnt = 0, s_ro_cnt = 0;
200 uint8_t str_ptr_pos[16];
201 const char *s;
202 bool parsing = false;
203
204 /* Buffer must be aligned at least to size of a pointer. */
205 if ((uintptr_t)packaged & (sizeof(void *) - 1)) {
206 return -EFAULT;
207 }
208
209 #if defined(__xtensa__)
210 /* Xtensa requires package to be 16 bytes aligned. */
211 if ((uintptr_t)packaged & (CBPRINTF_PACKAGE_ALIGNMENT - 1)) {
212 return -EFAULT;
213 }
214 #endif
215
216 /*
217 * Make room to store the arg list size and the number of
218 * appended strings. They both occupy 1 byte each.
219 *
220 * Given the next value to store is the format string pointer
221 * which is guaranteed to be at least 4 bytes, we just reserve
222 * a pointer size for the above to preserve alignment.
223 */
224 buf += sizeof(char *);
225
226 /*
227 * When buf0 is NULL we don't store anything.
228 * Instead we count the needed space to store the data.
229 * In this case, incoming len argument indicates the anticipated
230 * buffer "misalignment" offset.
231 */
232 if (!buf0) {
233 #if defined(__xtensa__)
234 if (len % CBPRINTF_PACKAGE_ALIGNMENT) {
235 return -EFAULT;
236 }
237 #endif
238 buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
239 len = -(len % CBPRINTF_PACKAGE_ALIGNMENT);
240 }
241
242 /*
243 * Otherwise we must ensure we can store at least
244 * thepointer to the format string itself.
245 */
246 if (buf0 && buf - buf0 + sizeof(char *) > len) {
247 return -ENOSPC;
248 }
249
250 /*
251 * Then process the format string itself.
252 * Here we branch directly into the code processing strings
253 * which is in the middle of the following while() loop. That's the
254 * reason for the post-decrement on fmt as it will be incremented
255 * prior to the next (actually first) round of that loop.
256 */
257 s = fmt--;
258 align = VA_STACK_ALIGN(char *);
259 size = sizeof(char *);
260 goto process_string;
261
262 /* Scan the format string */
263 while (*++fmt) {
264 if (!parsing) {
265 if (*fmt == '%') {
266 parsing = true;
267 align = VA_STACK_ALIGN(int);
268 size = sizeof(int);
269 }
270 continue;
271 }
272 switch (*fmt) {
273 case '%':
274 parsing = false;
275 continue;
276
277 case '#':
278 case '-':
279 case '+':
280 case ' ':
281 case '0':
282 case '1':
283 case '2':
284 case '3':
285 case '4':
286 case '5':
287 case '6':
288 case '7':
289 case '8':
290 case '9':
291 case '.':
292 case 'h':
293 case 'l':
294 case 'L':
295 continue;
296
297 case '*':
298 break;
299
300 case 'j':
301 align = VA_STACK_ALIGN(intmax_t);
302 size = sizeof(intmax_t);
303 continue;
304
305 case 'z':
306 align = VA_STACK_ALIGN(size_t);
307 size = sizeof(size_t);
308 continue;
309
310 case 't':
311 align = VA_STACK_ALIGN(ptrdiff_t);
312 size = sizeof(ptrdiff_t);
313 continue;
314
315 case 'c':
316 case 'd':
317 case 'i':
318 case 'o':
319 case 'u':
320 case 'x':
321 case 'X':
322 if (fmt[-1] == 'l') {
323 if (fmt[-2] == 'l') {
324 align = VA_STACK_ALIGN(long long);
325 size = sizeof(long long);
326 } else {
327 align = VA_STACK_ALIGN(long);
328 size = sizeof(long);
329 }
330 }
331 parsing = false;
332 break;
333
334 case 's':
335 case 'p':
336 case 'n':
337 align = VA_STACK_ALIGN(void *);
338 size = sizeof(void *);
339 parsing = false;
340 break;
341
342 case 'a':
343 case 'A':
344 case 'e':
345 case 'E':
346 case 'f':
347 case 'F':
348 case 'g':
349 case 'G': {
350 /*
351 * Handle floats separately as they may be
352 * held in a different register set.
353 */
354 union { double d; long double ld; } v;
355
356 if (fmt[-1] == 'L') {
357 v.ld = va_arg(ap, long double);
358 align = VA_STACK_ALIGN(long double);
359 size = sizeof(long double);
360 } else {
361 v.d = va_arg(ap, double);
362 align = VA_STACK_ALIGN(double);
363 size = sizeof(double);
364 }
365 /* align destination buffer location */
366 buf = (void *) ROUND_UP(buf, align);
367 if (buf0) {
368 /* make sure it fits */
369 if (buf - buf0 + size > len) {
370 return -ENOSPC;
371 }
372 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
373 memcpy(buf, &v, size);
374 } else if (fmt[-1] == 'L') {
375 *(long double *)buf = v.ld;
376 } else {
377 *(double *)buf = v.d;
378 }
379 }
380 buf += size;
381 parsing = false;
382 continue;
383 }
384
385 default:
386 parsing = false;
387 continue;
388 }
389
390 /* align destination buffer location */
391 buf = (void *) ROUND_UP(buf, align);
392
393 /* make sure the data fits */
394 if (buf0 && buf - buf0 + size > len) {
395 return -ENOSPC;
396 }
397
398 /* copy va_list data over to our buffer */
399 if (*fmt == 's') {
400 s = va_arg(ap, char *);
401 process_string:
402 if (buf0) {
403 *(const char **)buf = s;
404 }
405
406 /* Bother about read only strings only if storing
407 * string indexes is requested.
408 */
409 bool is_ro = ptr_in_rodata(s);
410 bool str_idxs = flags & CBPRINTF_PACKAGE_ADD_STRING_IDXS;
411 bool need_ro = is_ro && str_idxs;
412
413 if (ptr_in_rodata(s) && !str_idxs) {
414 /* do nothing special */
415 } else if (buf0) {
416
417 /*
418 * Remember string pointer location.
419 * We will append it later.
420 */
421 if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
422 __ASSERT(false, "str_ptr_pos[] too small");
423 return -EINVAL;
424 }
425
426 if ((buf - buf0) > CBPRINTF_STR_POS_MASK) {
427 __ASSERT(false, "String with too many arguments");
428 return -EINVAL;
429 }
430
431 /* Add marking to identify if read only string. */
432 uint8_t ro_flag = need_ro ?
433 CBPRINTF_STR_POS_RO_FLAG : 0;
434
435 if (ro_flag) {
436 s_ro_cnt++;
437 } else {
438 s_rw_cnt++;
439 }
440
441 /* Use same multiple as the arg list size. */
442 str_ptr_pos[s_idx++] = ro_flag |
443 (buf - buf0) / sizeof(int);
444 } else {
445 if (!is_ro) {
446 /*
447 * Add the string length, the final '\0'
448 * and size of the pointer position prefix.
449 */
450 len += strlen(s) + 1 + 1;
451 } else if (need_ro) {
452 /*
453 * Add only pointer position prefix for
454 * read only string is requested.
455 */
456 len += 1;
457 }
458 }
459 buf += sizeof(char *);
460 } else if (size == sizeof(int)) {
461 int v = va_arg(ap, int);
462
463 if (buf0) {
464 *(int *)buf = v;
465 }
466 buf += sizeof(int);
467 } else if (size == sizeof(long)) {
468 long v = va_arg(ap, long);
469
470 if (buf0) {
471 *(long *)buf = v;
472 }
473 buf += sizeof(long);
474 } else if (size == sizeof(long long)) {
475 long long v = va_arg(ap, long long);
476
477 if (buf0) {
478 if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
479 memcpy(buf, &v, sizeof(long long));
480 } else {
481 *(long long *)buf = v;
482 }
483 }
484 buf += sizeof(long long);
485 } else {
486 __ASSERT(false, "unexpected size %u", size);
487 return -EINVAL;
488 }
489 }
490
491 /*
492 * We remember the size of the argument list as a multiple of
493 * sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
494 * worth of va_list, or about 127 arguments on a 64-bit system
495 * (twice that on 32-bit systems). That ought to be good enough.
496 */
497 if ((buf - buf0) / sizeof(int) > 255) {
498 __ASSERT(false, "too many format args");
499 return -EINVAL;
500 }
501
502 /*
503 * If all we wanted was to count required buffer size
504 * then we have it now.
505 */
506 if (!buf0) {
507 return len + buf - buf0;
508 }
509
510 /* Clear our buffer header. We made room for it initially. */
511 *(char **)buf0 = NULL;
512
513 /* Record end of argument list and number of appended strings. */
514 buf0[0] = (buf - buf0) / sizeof(int);
515 buf0[1] = s_rw_cnt;
516 buf0[2] = s_ro_cnt;
517
518 /* Store strings pointer locations of read only strings. */
519 if (s_ro_cnt) {
520 for (i = 0; i < s_idx; i++) {
521 if (!(str_ptr_pos[i] & CBPRINTF_STR_POS_RO_FLAG)) {
522 continue;
523 }
524
525 uint8_t pos = str_ptr_pos[i] & CBPRINTF_STR_POS_MASK;
526
527 /* make sure it fits */
528 if (buf - buf0 + 1 > len) {
529 return -ENOSPC;
530 }
531 /* store the pointer position prefix */
532 *buf++ = pos;
533 }
534 }
535
536 /* Store strings prefixed by their pointer location. */
537 for (i = 0; i < s_idx; i++) {
538 /* Process only RW strings. */
539 if (str_ptr_pos[i] & CBPRINTF_STR_POS_RO_FLAG) {
540 continue;
541 }
542
543 /* retrieve the string pointer */
544 s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
545 /* clear the in-buffer pointer (less entropy if compressed) */
546 *(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
547 /* find the string length including terminating '\0' */
548 size = strlen(s) + 1;
549 /* make sure it fits */
550 if (buf - buf0 + 1 + size > len) {
551 return -ENOSPC;
552 }
553 /* store the pointer position prefix */
554 *buf++ = str_ptr_pos[i];
555 /* copy the string with its terminating '\0' */
556 memcpy(buf, s, size);
557 buf += size;
558 }
559
560 /*
561 * TODO: remove pointers for appended strings since they're useless.
562 * TODO: explore leveraging same mechanism to remove alignment padding
563 */
564
565 return buf - buf0;
566
567 #undef CBPRINTF_STR_POS_RO_FLAG
568 #undef CBPRINTF_STR_POS_MASK
569 }
570
cbprintf_package(void * packaged,size_t len,uint32_t flags,const char * format,...)571 int cbprintf_package(void *packaged, size_t len, uint32_t flags,
572 const char *format, ...)
573 {
574 va_list ap;
575 int ret;
576
577 va_start(ap, format);
578 ret = cbvprintf_package(packaged, len, flags, format, ap);
579 va_end(ap);
580 return ret;
581 }
582
cbpprintf(cbprintf_cb out,void * ctx,void * packaged)583 int cbpprintf(cbprintf_cb out, void *ctx, void *packaged)
584 {
585 char *buf = packaged, *fmt, *s, **ps;
586 unsigned int i, args_size, s_nbr, ros_nbr, s_idx;
587
588 if (!buf) {
589 return -EINVAL;
590 }
591
592 /* Retrieve the size of the arg list and number of strings. */
593 args_size = ((uint8_t *)buf)[0] * sizeof(int);
594 s_nbr = ((uint8_t *)buf)[1];
595 ros_nbr = ((uint8_t *)buf)[2];
596
597 /* Locate the string table */
598 s = buf + args_size + ros_nbr;
599
600 /*
601 * Patch in string pointers.
602 */
603 for (i = 0; i < s_nbr; i++) {
604 /* Locate pointer location for this string */
605 s_idx = *(uint8_t *)s++;
606 ps = (char **)(buf + s_idx * sizeof(int));
607 /* update the pointer with current string location */
608 *ps = s;
609 /* move to next string */
610 s += strlen(s) + 1;
611 }
612
613 /* Retrieve format string */
614 fmt = ((char **)buf)[1];
615
616 /* skip past format string pointer */
617 buf += sizeof(char *) * 2;
618
619 /* Turn this into a va_list and print it */
620 return cbprintf_via_va_list(out, ctx, fmt, buf);
621 }
622
cbprintf_fsc_package(void * in_packaged,size_t in_len,void * packaged,size_t len)623 int cbprintf_fsc_package(void *in_packaged,
624 size_t in_len,
625 void *packaged,
626 size_t len)
627 {
628 uint8_t *buf = in_packaged, *out = packaged;
629 char **ps;
630 unsigned int args_size, s_nbr, ros_nbr, s_idx;
631 size_t out_len;
632 size_t slen;
633
634 if (!buf) {
635 return -EINVAL;
636 }
637
638 if (packaged && (len < in_len)) {
639 return -ENOSPC;
640 }
641
642 /* Retrieve the size of the arg list and number of strings. */
643 args_size = buf[0] * sizeof(int);
644 s_nbr = buf[1];
645 ros_nbr = buf[2];
646
647 out_len = in_len;
648
649 if (packaged) {
650 unsigned int rw_strs_len = in_len - (args_size + ros_nbr);
651
652 memcpy(out, buf, args_size);
653 out[1] = s_nbr + ros_nbr;
654 out[2] = 0;
655 out += args_size;
656
657 /* Append all strings that were already part of the package. */
658 memcpy(out, &buf[args_size + ros_nbr], rw_strs_len);
659 out += rw_strs_len;
660 }
661
662 for (unsigned int i = 0; i < ros_nbr; i++) {
663 /* Get string address location */
664 s_idx = buf[args_size + i];
665 ps = (char **)(buf + s_idx * sizeof(int));
666
667 /* Get string length */
668 slen = strlen(*ps) + 1;
669 out_len += slen;
670
671 /* Copy string into the buffer (if provided) and enough space. */
672 if (packaged) {
673 if (out_len > len) {
674 return -ENOSPC;
675 }
676 *out++ = s_idx;
677 memcpy(out, *ps, slen);
678 out += slen;
679 }
680 }
681
682 return out_len;
683 }
684
