OrgZephyr/zephyr
OrgZephyr
/
zephyr
mirror of https://github.com/zephyrproject-rtos/zephyr.git
1
0
Fork 0
Code Issues Releases Wiki Activity
zephyr/lib/os/cbprintf_packaged.c

1166 lines
28 KiB
C
Raw Blame History

/*
* Copyright (c) 2021 BayLibre, SAS
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <errno.h>
#include <stdarg.h>
#include <stdint.h>
#include <string.h>
#include <zephyr/toolchain.h>
#include <zephyr/linker/utils.h>
#include <zephyr/sys/cbprintf.h>
#include <sys/types.h>
#include <zephyr/sys/util.h>
#include <zephyr/sys/__assert.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(cbprintf_package, CONFIG_CBPRINTF_PACKAGE_LOG_LEVEL);
#if defined(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS) && \
!Z_C_GENERIC
#error "CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS " \
"requires toolchain to support _Generic!"
#endif
/**
* @brief Check if address is in read only section.
*
* @param addr Address.
*
* @return True if address identified within read only section.
*/
static inline bool ptr_in_rodata(const char *addr)
{
#if defined(CBPRINTF_VIA_UNIT_TEST)
/* Unit test is X86 (or other host) but not using Zephyr
* linker scripts.
*/
return false;
#else
return linker_is_in_rodata(addr);
#endif
}
/*
* va_list creation
*/
#if defined(__CHECKER__)
static int cbprintf_via_va_list(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx,
const char *fmt, void *buf)
{
return 0;
}
#elif defined(__aarch64__)
/*
* Reference:
*
* Procedure Call Standard for the ARM 64-bit Architecture
*/
struct __va_list {
void *__stack;
void *__gr_top;
void *__vr_top;
int __gr_offs;
int __vr_offs;
};
BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
"architecture specific support is wrong");
static int cbprintf_via_va_list(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx,
const char *fmt, void *buf)
{
union {
va_list ap;
struct __va_list __ap;
} u;
/* create a valid va_list with our buffer */
u.__ap.__stack = buf;
u.__ap.__gr_top = NULL;
u.__ap.__vr_top = NULL;
u.__ap.__gr_offs = 0;
u.__ap.__vr_offs = 0;
return formatter(out, ctx, fmt, u.ap);
}
#elif defined(__x86_64__)
/*
* Reference:
*
* System V Application Binary Interface
* AMD64 Architecture Processor Supplement
*/
struct __va_list {
unsigned int gp_offset;
unsigned int fp_offset;
void *overflow_arg_area;
void *reg_save_area;
};
BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
"architecture specific support is wrong");
static int cbprintf_via_va_list(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx,
const char *fmt, void *buf)
{
union {
va_list ap;
struct __va_list __ap;
} u;
/* create a valid va_list with our buffer */
u.__ap.overflow_arg_area = buf;
u.__ap.reg_save_area = NULL;
u.__ap.gp_offset = (6 * 8);
u.__ap.fp_offset = (6 * 8 + 16 * 16);
return formatter(out, ctx, fmt, u.ap);
}
#elif defined(__xtensa__)
/*
* Reference:
*
* gcc source code (gcc/config/xtensa/xtensa.c)
* xtensa_build_builtin_va_list(), xtensa_va_start(),
* xtensa_gimplify_va_arg_expr()
*/
struct __va_list {
void *__va_stk;
void *__va_reg;
int __va_ndx;
};
BUILD_ASSERT(sizeof(va_list) == sizeof(struct __va_list),
"architecture specific support is wrong");
static int cbprintf_via_va_list(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx,
const char *fmt, void *buf)
{
union {
va_list ap;
struct __va_list __ap;
} u;
/* create a valid va_list with our buffer */
u.__ap.__va_stk = (char *)buf - 32;
u.__ap.__va_reg = NULL;
u.__ap.__va_ndx = (6 + 2) * 4;
return formatter(out, ctx, fmt, u.ap);
}
#else
/*
* Default implementation shared by many architectures like
* 32-bit ARM and Intel.
*
* We assume va_list is a simple pointer.
*/
BUILD_ASSERT(sizeof(va_list) == sizeof(void *),
"architecture specific support is needed");
static int cbprintf_via_va_list(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx,
const char *fmt, void *buf)
{
union {
va_list ap;
void *ptr;
} u;
u.ptr = buf;
return formatter(out, ctx, fmt, u.ap);
}
#endif
static size_t get_package_len(void *packaged)
{
__ASSERT_NO_MSG(packaged != NULL);
uint8_t *buf = packaged;
uint8_t *start = buf;
unsigned int args_size, s_nbr, ros_nbr;
args_size = buf[0] * sizeof(int);
s_nbr = buf[1];
ros_nbr = buf[2];
/* Move beyond args. */
buf += args_size;
/* Move beyond read-only string indexes array. */
buf += ros_nbr;
/* Move beyond strings appended to the package. */
for (int i = 0; i < s_nbr; i++) {
buf++;
buf += strlen((const char *)buf) + 1;
}
return (size_t)(uintptr_t)(buf - start);
}
static int append_string(cbprintf_convert_cb cb, void *ctx, const char *str, uint16_t strl)
{
if (cb == NULL) {
return 1 + strlen(str);
}
strl = strl > 0 ? strl : strlen(str) + 1;
return cb(str, strl, ctx);
}
int cbvprintf_package(void *packaged, size_t len, uint32_t flags,
const char *fmt, va_list ap)
{
/*
* Internally, a byte is used to store location of a string argument within a
* package. MSB bit is set if string is read-only so effectively 7 bits are
* used for index, which should be enough.
*/
#define STR_POS_RO_FLAG BIT(7)
#define STR_POS_MASK BIT_MASK(7)
/* Buffer offset abstraction for better code clarity. */
#define BUF_OFFSET ((uintptr_t)buf - (uintptr_t)buf0)
uint8_t *buf0 = packaged; /* buffer start (may be NULL) */
uint8_t *buf = buf0; /* current buffer position */
unsigned int size; /* current argument's size */
unsigned int align; /* current argument's required alignment */
uint8_t str_ptr_pos[16]; /* string pointer positions */
uint8_t str_ptr_arg[16]; /* string pointer argument index */
unsigned int s_idx = 0; /* index into str_ptr_pos[] */
unsigned int s_rw_cnt = 0; /* number of rw strings */
unsigned int s_ro_cnt = 0; /* number of ro strings */
int arg_idx = -1; /* Argument index. Preincremented thus starting from -1.*/
unsigned int i;
const char *s;
bool parsing = false;
/* Flag indicates that rw strings are stored as array with positions,
* instead of appending them to the package.
*/
bool rws_pos_en = !!(flags & CBPRINTF_PACKAGE_ADD_RW_STR_POS);
/* Get number of first read only strings present in the string.
* There is always at least 1 (fmt) but flags can indicate more, e.g
* fixed prefix appended to all strings.
*/
int fros_cnt = 1 + Z_CBPRINTF_PACKAGE_FIRST_RO_STR_CNT_GET(flags);
bool is_str_arg = false;
union cbprintf_package_hdr *pkg_hdr = packaged;
/* Buffer must be aligned at least to size of a pointer. */
if ((uintptr_t)packaged % sizeof(void *)) {
return -EFAULT;
}
#if defined(__xtensa__)
/* Xtensa requires package to be 16 bytes aligned. */
if ((uintptr_t)packaged % CBPRINTF_PACKAGE_ALIGNMENT) {
return -EFAULT;
}
#endif
/*
* Make room to store the arg list size, the number of
* appended writable strings and the number of appended
* read-only strings. They both occupy 1 byte each.
* Skip a byte. Then a uint32_t to store flags used to
* create the package.
*
* Given the next value to store is the format string pointer
* which is guaranteed to be at least 4 bytes, we just reserve
* multiple of pointer size for the above to preserve alignment.
*
* Refer to union cbprintf_package_hdr for more details.
*/
buf += sizeof(*pkg_hdr);
/*
* When buf0 is NULL we don't store anything.
* Instead we count the needed space to store the data.
* In this case, incoming len argument indicates the anticipated
* buffer "misalignment" offset.
*/
if (buf0 == NULL) {
buf += len % CBPRINTF_PACKAGE_ALIGNMENT;
/*
* The space to store the data is represented by both the
* buffer offset as well as the extra string data to be
* appended. When only figuring out the needed space, we
* don't append anything. Instead, we reuse the len variable
* to sum the size of that data.
*
* Also, we subtract any initial misalignment offset from
* the total as this won't be part of the buffer. To avoid
* going negative with an unsigned variable, we add an offset
* (CBPRINTF_PACKAGE_ALIGNMENT) that will be removed before
* returning.
*/
len = CBPRINTF_PACKAGE_ALIGNMENT - (len % CBPRINTF_PACKAGE_ALIGNMENT);
}
/*
* Otherwise we must ensure we can store at least
* the pointer to the format string itself.
*/
if (buf0 != NULL && BUF_OFFSET + sizeof(char *) > len) {
return -ENOSPC;
}
/*
* Then process the format string itself.
* Here we branch directly into the code processing strings
* which is in the middle of the following while() loop. That's the
* reason for the post-decrement on fmt as it will be incremented
* prior to the next (actually first) round of that loop.
*/
s = fmt--;
align = VA_STACK_ALIGN(char *);
size = sizeof(char *);
goto process_string;
while (true) {
#if defined(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS)
if ((flags & CBPRINTF_PACKAGE_ARGS_ARE_TAGGED)
== CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) {
int arg_tag = va_arg(ap, int);
/*
* Here we copy the tag over to the package.
*/
align = VA_STACK_ALIGN(int);
size = sizeof(int);
/* align destination buffer location */
buf = (void *)ROUND_UP(buf, align);
/* make sure the data fits */
if (buf0 != NULL && BUF_OFFSET + size > len) {
return -ENOSPC;
}
if (buf0 != NULL) {
*(int *)buf = arg_tag;
}
buf += sizeof(int);
if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_END) {
/* End of arguments */
break;
}
/*
* There are lots of __fallthrough here since
* quite a few of the data types have the same
* storage size.
*/
switch (arg_tag) {
case CBPRINTF_PACKAGE_ARG_TYPE_CHAR:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_CHAR:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_SHORT:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_SHORT:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_INT:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_INT:
align = VA_STACK_ALIGN(int);
size = sizeof(int);
break;
case CBPRINTF_PACKAGE_ARG_TYPE_LONG:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG:
align = VA_STACK_ALIGN(long);
size = sizeof(long);
break;
case CBPRINTF_PACKAGE_ARG_TYPE_LONG_LONG:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG_LONG:
align = VA_STACK_ALIGN(long long);
size = sizeof(long long);
break;
case CBPRINTF_PACKAGE_ARG_TYPE_FLOAT:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_DOUBLE:
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE: {
/*
* Handle floats separately as they may be
* held in a different register set.
*/
union { double d; long double ld; } v;
if (arg_tag == CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE) {
v.ld = va_arg(ap, long double);
align = VA_STACK_ALIGN(long double);
size = sizeof(long double);
} else {
v.d = va_arg(ap, double);
align = VA_STACK_ALIGN(double);
size = sizeof(double);
}
/* align destination buffer location */
buf = (void *) ROUND_UP(buf, align);
if (buf0 != NULL) {
/* make sure it fits */
if (BUF_OFFSET + size > len) {
return -ENOSPC;
}
if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
memcpy(buf, &v, size);
} else if (fmt[-1] == 'L') {
*(long double *)buf = v.ld;
} else {
*(double *)buf = v.d;
}
}
buf += size;
parsing = false;
continue;
}
case CBPRINTF_PACKAGE_ARG_TYPE_PTR_CHAR:
is_str_arg = true;
__fallthrough;
case CBPRINTF_PACKAGE_ARG_TYPE_PTR_VOID:
align = VA_STACK_ALIGN(void *);
size = sizeof(void *);
break;
default:
return -EINVAL;
}
} else
#endif /* CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS */
{
/* Scan the format string */
if (*++fmt == '\0') {
break;
}
if (!parsing) {
if (*fmt == '%') {
parsing = true;
arg_idx++;
align = VA_STACK_ALIGN(int);
size = sizeof(int);
}
continue;
}
switch (*fmt) {
case '%':
parsing = false;
arg_idx--;
continue;
case '#':
case '-':
case '+':
case ' ':
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
case '.':
case 'h':
case 'l':
case 'L':
continue;
case '*':
break;
case 'j':
align = VA_STACK_ALIGN(intmax_t);
size = sizeof(intmax_t);
continue;
case 'z':
align = VA_STACK_ALIGN(size_t);
size = sizeof(size_t);
continue;
case 't':
align = VA_STACK_ALIGN(ptrdiff_t);
size = sizeof(ptrdiff_t);
continue;
case 'c':
case 'd':
case 'i':
case 'o':
case 'u':
case 'x':
case 'X':
if (fmt[-1] == 'l') {
if (fmt[-2] == 'l') {
align = VA_STACK_ALIGN(long long);
size = sizeof(long long);
} else {
align = VA_STACK_ALIGN(long);
size = sizeof(long);
}
}
parsing = false;
break;
case 's':
is_str_arg = true;
__fallthrough;
case 'p':
case 'n':
align = VA_STACK_ALIGN(void *);
size = sizeof(void *);
parsing = false;
break;
case 'a':
case 'A':
case 'e':
case 'E':
case 'f':
case 'F':
case 'g':
case 'G': {
/*
* Handle floats separately as they may be
* held in a different register set.
*/
union { double d; long double ld; } v;
if (fmt[-1] == 'L') {
v.ld = va_arg(ap, long double);
align = VA_STACK_ALIGN(long double);
size = sizeof(long double);
} else {
v.d = va_arg(ap, double);
align = VA_STACK_ALIGN(double);
size = sizeof(double);
}
/* align destination buffer location */
buf = (void *) ROUND_UP(buf, align);
if (buf0 != NULL) {
/* make sure it fits */
if (BUF_OFFSET + size > len) {
return -ENOSPC;
}
if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
memcpy(buf, &v, size);
} else if (fmt[-1] == 'L') {
*(long double *)buf = v.ld;
} else {
*(double *)buf = v.d;
}
}
buf += size;
parsing = false;
continue;
}
default:
parsing = false;
continue;
}
}
/* align destination buffer location */
buf = (void *) ROUND_UP(buf, align);
/* make sure the data fits */
if (buf0 != NULL && BUF_OFFSET + size > len) {
return -ENOSPC;
}
/* copy va_list data over to our buffer */
if (is_str_arg) {
s = va_arg(ap, char *);
process_string:
if (buf0 != NULL) {
*(const char **)buf = s;
}
bool is_ro = (fros_cnt-- > 0) ? true : ptr_in_rodata(s);
bool do_ro = !!(flags & CBPRINTF_PACKAGE_ADD_RO_STR_POS);
if (is_ro && !do_ro) {
/* nothing to do */
} else {
uint32_t s_ptr_idx = BUF_OFFSET / sizeof(int);
/*
* In the do_ro case we must consider
* room for possible STR_POS_RO_FLAG.
* Otherwise the index range is 8 bits
* and any overflow is caught later.
*/
if (do_ro && s_ptr_idx > STR_POS_MASK) {
__ASSERT(false, "String with too many arguments");
return -EINVAL;
}
if (s_idx >= ARRAY_SIZE(str_ptr_pos)) {
__ASSERT(false, "str_ptr_pos[] too small");
return -EINVAL;
}
if (buf0 != NULL) {
/*
* Remember string pointer location.
* We will append non-ro strings later.
*/
str_ptr_pos[s_idx] = s_ptr_idx;
str_ptr_arg[s_idx] = arg_idx;
if (is_ro) {
/* flag read-only string. */
str_ptr_pos[s_idx] |= STR_POS_RO_FLAG;
s_ro_cnt++;
} else {
s_rw_cnt++;
}
} else if (is_ro) {
/*
* Add only pointer position prefix
* when counting strings.
*/
len += 1;
} else if (rws_pos_en) {
/*
* Add only pointer position prefix and
* argument index when counting strings.
*/
len += 2;
} else {
/*
* Add the string length, the final '\0'
* and size of the pointer position prefix.
*/
len += strlen(s) + 1 + 1;
}
s_idx++;
}
buf += sizeof(char *);
is_str_arg = false;
} else if (size == sizeof(int)) {
int v = va_arg(ap, int);
if (buf0 != NULL) {
*(int *)buf = v;
}
buf += sizeof(int);
} else if (size == sizeof(long)) {
long v = va_arg(ap, long);
if (buf0 != NULL) {
*(long *)buf = v;
}
buf += sizeof(long);
} else if (size == sizeof(long long)) {
long long v = va_arg(ap, long long);
if (buf0 != NULL) {
if (Z_CBPRINTF_VA_STACK_LL_DBL_MEMCPY) {
memcpy(buf, &v, sizeof(long long));
} else {
*(long long *)buf = v;
}
}
buf += sizeof(long long);
} else {
__ASSERT(false, "unexpected size %u", size);
return -EINVAL;
}
}
/*
* We remember the size of the argument list as a multiple of
* sizeof(int) and limit it to a 8-bit field. That means 1020 bytes
* worth of va_list, or about 127 arguments on a 64-bit system
* (twice that on 32-bit systems). That ought to be good enough.
*/
if (BUF_OFFSET / sizeof(int) > 255) {
__ASSERT(false, "too many format args");
return -EINVAL;
}
/*
* If all we wanted was to count required buffer size
* then we have it now.
*/
if (buf0 == NULL) {
return BUF_OFFSET + len - CBPRINTF_PACKAGE_ALIGNMENT;
}
/* Clear our buffer header. We made room for it initially. */
*(char **)buf0 = NULL;
/* Record end of argument list. */
pkg_hdr->desc.len = BUF_OFFSET / sizeof(int);
if (rws_pos_en) {
/* Strings are appended, update location counter. */
pkg_hdr->desc.str_cnt = 0;
pkg_hdr->desc.rw_str_cnt = s_rw_cnt;
} else {
/* Strings are appended, update append counter. */
pkg_hdr->desc.str_cnt = s_rw_cnt;
pkg_hdr->desc.rw_str_cnt = 0;
}
pkg_hdr->desc.ro_str_cnt = s_ro_cnt;
#ifdef CONFIG_CBPRINTF_PACKAGE_HEADER_STORE_CREATION_FLAGS
pkg_hdr->desc.pkg_flags = flags;
#endif
/* Store strings pointer locations of read only strings. */
if (s_ro_cnt) {
for (i = 0; i < s_idx; i++) {
if (!(str_ptr_pos[i] & STR_POS_RO_FLAG)) {
continue;
}
uint8_t pos = str_ptr_pos[i] & STR_POS_MASK;
/* make sure it fits */
if (BUF_OFFSET + 1 > len) {
return -ENOSPC;
}
/* store the pointer position prefix */
*buf++ = pos;
}
}
/* Store strings prefixed by their pointer location. */
for (i = 0; i < s_idx; i++) {
/* Process only RW strings. */
if (s_ro_cnt && str_ptr_pos[i] & STR_POS_RO_FLAG) {
continue;
}
if (rws_pos_en) {
size = 0;
*buf++ = str_ptr_arg[i];
} else {
/* retrieve the string pointer */
s = *(char **)(buf0 + str_ptr_pos[i] * sizeof(int));
/* clear the in-buffer pointer (less entropy if compressed) */
*(char **)(buf0 + str_ptr_pos[i] * sizeof(int)) = NULL;
/* find the string length including terminating '\0' */
size = strlen(s) + 1;
}
/* make sure it fits */
if (BUF_OFFSET + 1 + size > len) {
return -ENOSPC;
}
/* store the pointer position prefix */
*buf++ = str_ptr_pos[i];
/* copy the string with its terminating '\0' */
memcpy(buf, s, size);
buf += size;
}
/*
* TODO: remove pointers for appended strings since they're useless.
* TODO: explore leveraging same mechanism to remove alignment padding
*/
return BUF_OFFSET;
#undef BUF_OFFSET
#undef STR_POS_RO_FLAG
#undef STR_POS_MASK
}
int cbprintf_package(void *packaged, size_t len, uint32_t flags,
const char *format, ...)
{
va_list ap;
int ret;
va_start(ap, format);
ret = cbvprintf_package(packaged, len, flags, format, ap);
va_end(ap);
return ret;
}
int cbpprintf_external(cbprintf_cb out,
cbvprintf_external_formatter_func formatter,
void *ctx, void *packaged)
{
uint8_t *buf = packaged;
struct cbprintf_package_hdr_ext *hdr = packaged;
char *s, **ps;
unsigned int i, args_size, s_nbr, ros_nbr, rws_nbr, s_idx;
if (buf == NULL) {
return -EINVAL;
}
/* Retrieve the size of the arg list and number of strings. */
args_size = hdr->hdr.desc.len * sizeof(int);
s_nbr = hdr->hdr.desc.str_cnt;
ros_nbr = hdr->hdr.desc.ro_str_cnt;
rws_nbr = hdr->hdr.desc.rw_str_cnt;
/* Locate the string table */
s = (char *)(buf + args_size + ros_nbr + 2 * rws_nbr);
/*
* Patch in string pointers.
*/
for (i = 0; i < s_nbr; i++) {
/* Locate pointer location for this string */
s_idx = *(uint8_t *)s++;
ps = (char **)(buf + s_idx * sizeof(int));
/* update the pointer with current string location */
*ps = s;
/* move to next string */
s += strlen(s) + 1;
}
/* Skip past the header */
buf += sizeof(*hdr);
/* Turn this into a va_list and print it */
return cbprintf_via_va_list(out, formatter, ctx, hdr->fmt, buf);
}
/* Function checks if character might be format specifier. Check is relaxed since
* compiler ensures that correct format specifier is used so it is enough to check
* that character is not one of potential modifier (e.g. number, dot, etc.).
*/
static bool is_fmt_spec(char c)
{
return (c >= 64) && (c <= 122);
}
/* Function checks if nth argument is a pointer (%p). Returns true is yes. Returns
* false if not or if string does not have nth argument.
*/
bool is_ptr(const char *fmt, int n)
{
char c;
bool mod = false;
int cnt = 0;
while ((c = *fmt++) != '\0') {
if (mod) {
if (cnt == n) {
if (c == 'p') {
return true;
} else if (is_fmt_spec(c)) {
return false;
}
} else if (is_fmt_spec(c)) {
cnt++;
mod = false;
}
}
if (c == '%') {
mod = !mod;
}
}
return false;
}
int 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)
{
__ASSERT_NO_MSG(in_packaged != NULL);
uint8_t *buf = in_packaged;
uint32_t *buf32 = in_packaged;
unsigned int args_size, ros_nbr, rws_nbr;
bool fmt_present = flags & CBPRINTF_PACKAGE_CONVERT_PTR_CHECK ? true : false;
bool rw_cpy;
bool ro_cpy;
struct cbprintf_package_desc *in_desc = in_packaged;
in_len = in_len != 0 ? in_len : get_package_len(in_packaged);
/* Get number of RO string indexes in the package and check if copying
* includes appending those strings.
*/
ros_nbr = in_desc->ro_str_cnt;
ro_cpy = ros_nbr &&
(flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) == CBPRINTF_PACKAGE_CONVERT_RO_STR;
/* Get number of RW string indexes in the package and check if copying
* includes appending those strings.
*/
rws_nbr = in_desc->rw_str_cnt;
rw_cpy = rws_nbr > 0 &&
(flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) == CBPRINTF_PACKAGE_CONVERT_RW_STR;
/* If flags are not set or appending request without rw string indexes
* present is chosen, just do a simple copy (or length calculation).
* Assuming that it is the most common case.
*/
if (!rw_cpy && !ro_cpy) {
if (cb) {
cb(in_packaged, in_len, ctx);
}
return in_len;
}
/* If we got here, it means that coping will be more complex and will be
* done with strings appending.
* Retrieve the size of the arg list.
*/
args_size = in_desc->len * sizeof(int);
int out_len;
/* Pointer to array with string locations. Array starts with read-only
* string locations.
*/
const char *fmt = *(const char **)(buf + sizeof(void *));
uint8_t *str_pos = &buf[args_size];
size_t strl_cnt = 0;
/* If null destination, just calculate output length. */
if (cb == NULL) {
out_len = (int)in_len;
if (ro_cpy) {
for (int i = 0; i < ros_nbr; i++) {
const char *str = *(const char **)&buf32[*str_pos];
int len = append_string(cb, NULL, str, 0);
/* If possible store calculated string length. */
if (strl && strl_cnt < strl_len) {
strl[strl_cnt++] = (uint16_t)len;
}
out_len += len;
str_pos++;
}
} else {
if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
str_pos += ros_nbr;
}
}
bool drop_ro_str_pos = !(flags &
(CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR |
CBPRINTF_PACKAGE_CONVERT_RO_STR));
/* Handle RW strings. */
for (int i = 0; i < rws_nbr; i++) {
uint8_t arg_idx = *str_pos++;
uint8_t arg_pos = *str_pos++;
const char *str = *(const char **)&buf32[arg_pos];
bool is_ro = ptr_in_rodata(str);
int len;
if (fmt_present && is_ptr(fmt, arg_idx)) {
LOG_WRN("(unsigned) char * used for %%p argument. "
"It's recommended to cast it to void * because "
"it may cause misbehavior in certain "
"configurations. String:\"%s\" argument:%d", fmt, arg_idx);
/* Since location is being dropped, decrement
* output length by 2 (argument index + position)
*/
out_len -= 2;
continue;
}
if (is_ro) {
if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
goto calculate_string_length;
} else {
out_len -= drop_ro_str_pos ? 2 : 1;
}
} else if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
calculate_string_length:
len = append_string(cb, NULL, str, 0);
/* If possible store calculated string length. */
if (strl && strl_cnt < strl_len) {
strl[strl_cnt++] = (uint16_t)len;
}
/* string length decremented by 1 because argument
* index is dropped.
*/
out_len += (len - 1);
}
}
return out_len;
}
struct cbprintf_package_desc out_desc;
/* At least one is copied in. */
uint8_t cpy_str_pos[16];
/* Up to one will be kept since if both types are kept it returns earlier. */
uint8_t keep_str_pos[16];
uint8_t scpy_cnt;
uint8_t keep_cnt;
uint8_t *dst;
int rv;
/* If read-only strings shall be appended to the output package copy
* their indexes to the local array, otherwise indicate that indexes
* shall remain in the output package.
*/
if (ro_cpy) {
scpy_cnt = ros_nbr;
keep_cnt = 0;
dst = cpy_str_pos;
} else if (ros_nbr && flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
scpy_cnt = 0;
keep_cnt = ros_nbr;
dst = keep_str_pos;
} else {
scpy_cnt = 0;
keep_cnt = 0;
dst = NULL;
}
if (dst) {
memcpy(dst, str_pos, ros_nbr);
}
str_pos += ros_nbr;
/* Go through read-write strings and identify which shall be appended.
* Note that there may be read-only strings there. Use address evaluation
* to determine if strings is read-only.
*/
for (int i = 0; i < rws_nbr; i++) {
uint8_t arg_idx = *str_pos++;
uint8_t arg_pos = *str_pos++;
const char *str = *(const char **)&buf32[arg_pos];
bool is_ro = ptr_in_rodata(str);
if (fmt_present && is_ptr(fmt, arg_idx)) {
continue;
}
if (is_ro) {
if (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) {
__ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
cpy_str_pos[scpy_cnt++] = arg_pos;
} else if (flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) {
__ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
keep_str_pos[keep_cnt++] = arg_pos;
} else {
/* Drop information about ro_str location. */
}
} else {
if (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) {
__ASSERT_NO_MSG(scpy_cnt < sizeof(cpy_str_pos));
cpy_str_pos[scpy_cnt++] = arg_pos;
} else {
__ASSERT_NO_MSG(keep_cnt < sizeof(keep_str_pos));
keep_str_pos[keep_cnt++] = arg_idx;
keep_str_pos[keep_cnt++] = arg_pos;
}
}
}
/* Set amount of strings appended to the package. */
out_desc.len = in_desc->len;
out_desc.str_cnt = in_desc->str_cnt + scpy_cnt;
out_desc.rw_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RW_STR) ? 0 : (keep_cnt / 2);
out_desc.ro_str_cnt = (flags & CBPRINTF_PACKAGE_CONVERT_RO_STR) ? 0 :
((flags & CBPRINTF_PACKAGE_CONVERT_KEEP_RO_STR) ? keep_cnt : 0);
/* Temporary overwrite input descriptor to allow bulk transfer */
struct cbprintf_package_desc in_desc_backup = *in_desc;
*in_desc = out_desc;
/* Copy package header and arguments. */
rv = cb(in_packaged, args_size, ctx);
if (rv < 0) {
return rv;
}
out_len = rv;
/* Restore input descriptor. */
*in_desc = in_desc_backup;
/* Copy string positions which are kept. */
rv = cb(keep_str_pos, keep_cnt, ctx);
if (rv < 0) {
return rv;
}
out_len += rv;
/* Copy appended strings from source package to destination. */
size_t strs_len = in_len - (args_size + ros_nbr + 2 * rws_nbr);
rv = cb(str_pos, strs_len, ctx);
if (rv < 0) {
return rv;
}
out_len += rv;
/* Append strings */
for (int i = 0; i < scpy_cnt; i++) {
uint8_t loc = cpy_str_pos[i];
const char *str = *(const char **)&buf32[loc];
uint16_t str_len = strl ? strl[i] : 0;
rv = cb(&loc, 1, ctx);
if (rv < 0) {
return rv;
}
out_len += rv;
rv = append_string(cb, ctx, str, str_len);
if (rv < 0) {
return rv;
}
out_len += rv;
}
/* Empty call (can be interpreted as flushing) */
(void)cb(NULL, 0, ctx);
return out_len;
}
Reference in New Issue View Git Blame Copy Permalink