356 lines
6.6 KiB
C
356 lines
6.6 KiB
C
/*
|
|
* Copyright (c) 2010, 2013-2014 Wind River Systems, Inc.
|
|
* Copyright (c) 2020 Nordic Semiconductor ASA
|
|
* Copyright (c) 2021 BayLibre, SAS
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <stdarg.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <zephyr/sys/cbprintf.h>
|
|
#include <sys/types.h>
|
|
#include <zephyr/sys/util.h>
|
|
|
|
#ifdef CONFIG_CBPRINTF_FULL_INTEGRAL
|
|
typedef intmax_t int_value_type;
|
|
typedef uintmax_t uint_value_type;
|
|
#define DIGITS_BUFLEN 21
|
|
BUILD_ASSERT(sizeof(uint_value_type) <= 8U,
|
|
"DIGITS_BUFLEN may be incorrect");
|
|
#else
|
|
typedef int32_t int_value_type;
|
|
typedef uint32_t uint_value_type;
|
|
#define DIGITS_BUFLEN 10
|
|
#endif
|
|
|
|
#define ALPHA(fmt) (((fmt) & 0x60) - '0' - 10 + 1)
|
|
|
|
/* Convert value to string, storing characters downwards */
|
|
static inline int convert_value(uint_value_type num, unsigned int base,
|
|
unsigned int alpha, char *buftop)
|
|
{
|
|
int i = 0;
|
|
|
|
do {
|
|
unsigned int c = num % base;
|
|
if (c >= 10) {
|
|
c += alpha;
|
|
}
|
|
buftop[i--] = c + '0';
|
|
num /= base;
|
|
} while (num);
|
|
|
|
return -i;
|
|
}
|
|
|
|
#define OUTC(_c) do { \
|
|
out((int)(_c), ctx); \
|
|
if (IS_ENABLED(CONFIG_CBPRINTF_LIBC_SUBSTS)) { \
|
|
++count; \
|
|
} \
|
|
} while (false)
|
|
|
|
#define PAD_ZERO BIT(0)
|
|
#define PAD_TAIL BIT(1)
|
|
|
|
/* Skip over the argument tag if needed as it is not being used here. */
|
|
#define SKIP_TAG_IF_NEEDED(ap, tagged_ap) \
|
|
do { \
|
|
if (IS_ENABLED(CONFIG_CBPRINTF_PACKAGE_SUPPORT_TAGGED_ARGUMENTS) \
|
|
&& tagged_ap) { \
|
|
(void)va_arg(ap, int); \
|
|
} \
|
|
} while (0)
|
|
|
|
/**
|
|
* @brief Printk internals
|
|
*
|
|
* See printk() for description.
|
|
* @param fmt Format string
|
|
* @param ap Variable parameters
|
|
*
|
|
* @return printed byte count if CONFIG_CBPRINTF_LIBC_SUBSTS is set
|
|
*/
|
|
int z_cbvprintf_impl(cbprintf_cb out, void *ctx, const char *fmt,
|
|
va_list ap, uint32_t flags)
|
|
{
|
|
size_t count = 0;
|
|
char buf[DIGITS_BUFLEN];
|
|
char *prefix, *data;
|
|
int min_width, precision, data_len;
|
|
char padding_mode, length_mod, special;
|
|
|
|
const bool tagged_ap = (flags & Z_CBVPRINTF_PROCESS_FLAG_TAGGED_ARGS)
|
|
== Z_CBVPRINTF_PROCESS_FLAG_TAGGED_ARGS;
|
|
|
|
/* we pre-increment in the loop afterwards */
|
|
fmt--;
|
|
|
|
start:
|
|
while (*++fmt != '%') {
|
|
if (*fmt == '\0') {
|
|
return count;
|
|
}
|
|
OUTC(*fmt);
|
|
}
|
|
|
|
min_width = -1;
|
|
precision = -1;
|
|
prefix = "";
|
|
padding_mode = 0;
|
|
length_mod = 0;
|
|
special = 0;
|
|
|
|
for (fmt++ ; ; fmt++) {
|
|
switch (*fmt) {
|
|
case 0:
|
|
return count;
|
|
|
|
case '%':
|
|
OUTC('%');
|
|
goto start;
|
|
|
|
case '-':
|
|
padding_mode = PAD_TAIL;
|
|
continue;
|
|
|
|
case '.':
|
|
precision = 0;
|
|
padding_mode &= (char)~PAD_ZERO;
|
|
continue;
|
|
|
|
case '0':
|
|
if (min_width < 0 && precision < 0 && !padding_mode) {
|
|
padding_mode = PAD_ZERO;
|
|
continue;
|
|
}
|
|
__fallthrough;
|
|
|
|
case '1':
|
|
case '2':
|
|
case '3':
|
|
case '4':
|
|
case '5':
|
|
case '6':
|
|
case '7':
|
|
case '8':
|
|
case '9':
|
|
if (precision >= 0) {
|
|
precision = 10 * precision + *fmt - '0';
|
|
} else {
|
|
if (min_width < 0) {
|
|
min_width = 0;
|
|
}
|
|
min_width = 10 * min_width + *fmt - '0';
|
|
}
|
|
continue;
|
|
|
|
case '*':
|
|
SKIP_TAG_IF_NEEDED(ap, tagged_ap);
|
|
|
|
if (precision >= 0) {
|
|
precision = va_arg(ap, int);
|
|
} else {
|
|
min_width = va_arg(ap, int);
|
|
if (min_width < 0) {
|
|
min_width = -min_width;
|
|
padding_mode = PAD_TAIL;
|
|
}
|
|
}
|
|
continue;
|
|
|
|
case '+':
|
|
case ' ':
|
|
case '#':
|
|
special = *fmt;
|
|
continue;
|
|
|
|
case 'h':
|
|
case 'l':
|
|
case 'z':
|
|
if (*fmt == 'h' && length_mod == 'h') {
|
|
length_mod = 'H';
|
|
} else if (*fmt == 'l' && length_mod == 'l') {
|
|
length_mod = 'L';
|
|
} else if (length_mod == '\0') {
|
|
length_mod = *fmt;
|
|
} else {
|
|
OUTC('%');
|
|
OUTC(*fmt);
|
|
goto start;
|
|
}
|
|
continue;
|
|
|
|
case 'd':
|
|
case 'i':
|
|
case 'u': {
|
|
uint_value_type d;
|
|
|
|
SKIP_TAG_IF_NEEDED(ap, tagged_ap);
|
|
|
|
if (length_mod == 'z') {
|
|
if (*fmt == 'u') {
|
|
d = va_arg(ap, size_t);
|
|
} else {
|
|
d = va_arg(ap, ssize_t);
|
|
}
|
|
} else if (length_mod == 'l') {
|
|
if (*fmt == 'u') {
|
|
d = va_arg(ap, unsigned long);
|
|
} else {
|
|
d = va_arg(ap, long);
|
|
}
|
|
} else if (length_mod == 'L') {
|
|
if (*fmt == 'u') {
|
|
unsigned long long llu =
|
|
va_arg(ap, unsigned long long);
|
|
|
|
if (llu != (uint_value_type) llu) {
|
|
data = "ERR";
|
|
data_len = 3;
|
|
precision = 0;
|
|
break;
|
|
}
|
|
d = (uint_value_type) llu;
|
|
} else {
|
|
long long lld = va_arg(ap, long long);
|
|
|
|
if (lld != (int_value_type) lld) {
|
|
data = "ERR";
|
|
data_len = 3;
|
|
precision = 0;
|
|
break;
|
|
}
|
|
d = (int_value_type) lld;
|
|
}
|
|
} else if (*fmt == 'u') {
|
|
d = va_arg(ap, unsigned int);
|
|
} else {
|
|
d = va_arg(ap, int);
|
|
}
|
|
|
|
if (*fmt != 'u' && (int_value_type)d < 0) {
|
|
d = -d;
|
|
prefix = "-";
|
|
min_width--;
|
|
} else if (special == ' ') {
|
|
prefix = " ";
|
|
min_width--;
|
|
} else if (special == '+') {
|
|
prefix = "+";
|
|
min_width--;
|
|
} else {
|
|
;
|
|
}
|
|
data_len = convert_value(d, 10, 0, buf + sizeof(buf) - 1);
|
|
data = buf + sizeof(buf) - data_len;
|
|
break;
|
|
}
|
|
|
|
case 'p':
|
|
case 'x':
|
|
case 'X': {
|
|
uint_value_type x;
|
|
|
|
SKIP_TAG_IF_NEEDED(ap, tagged_ap);
|
|
|
|
if (*fmt == 'p') {
|
|
x = (uintptr_t)va_arg(ap, void *);
|
|
if (x == (uint_value_type)0) {
|
|
data = "(nil)";
|
|
data_len = 5;
|
|
precision = 0;
|
|
break;
|
|
}
|
|
special = '#';
|
|
} else if (length_mod == 'l') {
|
|
x = va_arg(ap, unsigned long);
|
|
} else if (length_mod == 'L') {
|
|
unsigned long long llx =
|
|
va_arg(ap, unsigned long long);
|
|
|
|
if (llx != (uint_value_type) llx) {
|
|
data = "ERR";
|
|
data_len = 3;
|
|
precision = 0;
|
|
break;
|
|
}
|
|
x = (uint_value_type) llx;
|
|
} else {
|
|
x = va_arg(ap, unsigned int);
|
|
}
|
|
if (special == '#') {
|
|
prefix = (*fmt & 0x20) ? "0x" : "0X";
|
|
min_width -= 2;
|
|
}
|
|
data_len = convert_value(x, 16, ALPHA(*fmt),
|
|
buf + sizeof(buf) - 1);
|
|
data = buf + sizeof(buf) - data_len;
|
|
break;
|
|
}
|
|
|
|
case 's': {
|
|
SKIP_TAG_IF_NEEDED(ap, tagged_ap);
|
|
|
|
data = va_arg(ap, char *);
|
|
data_len = strlen(data);
|
|
if (precision >= 0 && data_len > precision) {
|
|
data_len = precision;
|
|
}
|
|
precision = 0;
|
|
break;
|
|
}
|
|
|
|
case 'c': {
|
|
int c;
|
|
|
|
SKIP_TAG_IF_NEEDED(ap, tagged_ap);
|
|
|
|
c = va_arg(ap, int);
|
|
|
|
buf[0] = c;
|
|
data = buf;
|
|
data_len = 1;
|
|
precision = 0;
|
|
break;
|
|
}
|
|
|
|
default:
|
|
OUTC('%');
|
|
OUTC(*fmt);
|
|
goto start;
|
|
}
|
|
|
|
if (precision < 0 && (padding_mode & PAD_ZERO)) {
|
|
precision = min_width;
|
|
}
|
|
min_width -= data_len;
|
|
precision -= data_len;
|
|
if (precision > 0) {
|
|
min_width -= precision;
|
|
}
|
|
|
|
if (!(padding_mode & PAD_TAIL)) {
|
|
while (--min_width >= 0) {
|
|
OUTC(' ');
|
|
}
|
|
}
|
|
while (*prefix) {
|
|
OUTC(*prefix++);
|
|
}
|
|
while (--precision >= 0) {
|
|
OUTC('0');
|
|
}
|
|
while (--data_len >= 0) {
|
|
OUTC(*data++);
|
|
}
|
|
while (--min_width >= 0) {
|
|
OUTC(' ');
|
|
}
|
|
|
|
goto start;
|
|
}
|
|
}
|