diff --git a/src/include/mtprintf.h b/src/include/mtprintf.h new file mode 100644 index 000000000000..72831240c54c --- /dev/null +++ b/src/include/mtprintf.h @@ -0,0 +1,194 @@ +#ifndef MTPRINTF_H +#define MTPRINTF_H + +/* + * Copyright (c) 2020 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include + +/* (Cribbed from Zephyr: arch/x86/zefi/printf.h) */ + +/* Tiny, but not-as-primitive-as-it-looks implementation of something + * like s/n/printf(). Handles %d, %x, %p, %c and %s only, allows a + * "l" qualifier on %d and %x (and silently ignores one %s/%c/%p). + * Accepts, but ignores, field width and precision values that match: + * the regex: [0-9]*\.?[0-9]* + */ + +/* Use this 1M region for the log data (see notes in mt8195-load.py). + * Remember that the firmware loader doesn't (!!!) zero this RAM, and + * that it starts out populated with seeming garbage from firmware + * startup (there are some bootloader-looking strings in there). So + * something very early on needs to set MTPRINTF_LEN=0. + * + * Obviously this will cause overruns the second log data enters the + * last four bytes. It's a debug tool. NOT RELIABLE FOR PRODUCTION + * USE. + */ +#define MTPRINTF_BUF ((volatile char *)0x60700000) +#define MTPRINTF_LEN (*(volatile int*)0x607ffffc) + +static inline void z_putchar(char c) +{ + volatile char *p = &MTPRINTF_BUF[MTPRINTF_LEN]; + p[1] = 0; + p[0] = c; + MTPRINTF_LEN++; + __asm__ volatile("dhwb %0, 0; dhwb %0, 4" :: "r"(p)); +} + +struct _pfr { + char *buf; + int len; + int idx; +}; + +static inline void pc(struct _pfr *r, int c) +{ + if (r->buf != NULL) { + if (r->idx <= r->len) { + r->buf[r->idx] = c; + } + } else { + z_putchar(c); + } + r->idx++; +} + +static inline void prdec(struct _pfr *r, long v) +{ + if (v < 0) { + pc(r, '-'); + v = -v; + } + + char digs[11 * sizeof(long)/4]; + int i = sizeof(digs) - 1; + + digs[i--] = 0; + while (v || i == 9) { + digs[i--] = '0' + (v % 10); + v /= 10; + } + + while (digs[++i] != '\0') { + pc(r, digs[i]); + } +} + +static inline void endrec(struct _pfr *r) +{ + if (r->buf && r->idx < r->len) { + r->buf[r->idx] = 0; + } +} + +static inline int vpf(struct _pfr *r, const char *f, va_list ap) +{ + for (/**/; *f != '\0'; f++) { + bool islong = false; + + if (*f != '%') { + pc(r, *f); + continue; + } + + if (f[1] == 'l') { + islong = sizeof(long) > 4; + f++; + } + + /* Ignore (but accept) field width and precision values */ + while (f[1] >= '0' && f[1] <= '9') { + f++; + } + if (f[1] == '.') { + f++; + } + while (f[1] >= '0' && f[1] <= '9') { + f++; + } + + switch (*(++f)) { + case 0: + return r->idx; + case '%': + pc(r, '%'); + break; + case 'c': + pc(r, va_arg(ap, int)); + break; + case 's': { + char *s = va_arg(ap, char *); + + while (*s != '\0') + pc(r, *s++); + break; + } + case 'p': + pc(r, '0'); + pc(r, 'x'); /* fall through... */ + islong = sizeof(long) > 4; + case 'x': { + int i, sig = 0; + unsigned long v = islong ? va_arg(ap, unsigned long) + : va_arg(ap, unsigned int); + for (i = 2*sizeof(long) - 1; i >= 0; i--) { + int d = (v >> (i*4)) & 0xf; + + sig += !!d; + if (sig || i == 0) + pc(r, "0123456789abcdef"[d]); + } + break; + } + case 'd': + prdec(r, va_arg(ap, int)); + break; + default: + pc(r, '%'); + pc(r, *f); + } + } + endrec(r); + return r->idx; +} + +#define CALL_VPF(rec) \ + va_list ap; \ + va_start(ap, f); \ + ret = vpf(&r, f, ap); \ + va_end(ap); + +static inline int mtsnprintf(char *buf, unsigned long len, const char *f, ...) +{ + int ret = 0; + struct _pfr r = { .buf = buf, .len = len }; + + CALL_VPF(&r); + return ret; +} + +static inline int mtsprintf(char *buf, const char *f, ...) +{ + int ret = 0; + struct _pfr r = { .buf = buf, .len = 0x7fffffff }; + + CALL_VPF(&r); + return ret; +} + +static inline int mtprintf(const char *f, ...) +{ + int ret = 0; + struct _pfr r = {0}; + + CALL_VPF(&r); + return ret; +} + +#endif // MTPRINTF_H diff --git a/src/init/init.c b/src/init/init.c index cdfa2b6ef794..4942bf21b693 100644 --- a/src/init/init.c +++ b/src/init/init.c @@ -1,3 +1,4 @@ +#include // SPDX-License-Identifier: BSD-3-Clause // // Copyright(c) 2016 Intel Corporation. All rights reserved. @@ -314,6 +315,9 @@ static int primary_core_init(int argc, char *argv[], struct sof *sof) #ifndef __ZEPHYR__ int main(int argc, char *argv[]) { + MTPRINTF_LEN = 0; + mtprintf("Hello, SOF world!\n"); + int err = 0; trace_point(TRACE_BOOT_START);