123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485 |
- /*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2019 Western Digital Corporation or its affiliates.
- *
- * Authors:
- * Anup Patel <anup.patel@wdc.com>
- */
- #include <sbi/riscv_locks.h>
- #include <sbi/sbi_console.h>
- #include <sbi/sbi_hart.h>
- #include <sbi/sbi_platform.h>
- #include <sbi/sbi_scratch.h>
- #include <sbi/sbi_string.h>
- #define CONSOLE_TBUF_MAX 256
- static const struct sbi_console_device *console_dev = NULL;
- static char console_tbuf[CONSOLE_TBUF_MAX];
- static u32 console_tbuf_len;
- static spinlock_t console_out_lock = SPIN_LOCK_INITIALIZER;
- bool sbi_isprintable(char c)
- {
- if (((31 < c) && (c < 127)) || (c == '\f') || (c == '\r') ||
- (c == '\n') || (c == '\t')) {
- return true;
- }
- return false;
- }
- int sbi_getc(void)
- {
- if (console_dev && console_dev->console_getc)
- return console_dev->console_getc();
- return -1;
- }
- void sbi_putc(char ch)
- {
- if (console_dev && console_dev->console_putc) {
- if (ch == '\n')
- console_dev->console_putc('\r');
- console_dev->console_putc(ch);
- }
- }
- static unsigned long nputs(const char *str, unsigned long len)
- {
- unsigned long i, ret;
- if (console_dev && console_dev->console_puts) {
- ret = console_dev->console_puts(str, len);
- } else {
- for (i = 0; i < len; i++)
- sbi_putc(str[i]);
- ret = len;
- }
- return ret;
- }
- static void nputs_all(const char *str, unsigned long len)
- {
- unsigned long p = 0;
- while (p < len)
- p += nputs(&str[p], len - p);
- }
- void sbi_puts(const char *str)
- {
- unsigned long len = sbi_strlen(str);
- spin_lock(&console_out_lock);
- nputs_all(str, len);
- spin_unlock(&console_out_lock);
- }
- unsigned long sbi_nputs(const char *str, unsigned long len)
- {
- unsigned long ret;
- spin_lock(&console_out_lock);
- ret = nputs(str, len);
- spin_unlock(&console_out_lock);
- return ret;
- }
- void sbi_gets(char *s, int maxwidth, char endchar)
- {
- int ch;
- char *retval = s;
- while ((ch = sbi_getc()) != endchar && ch >= 0 && maxwidth > 1) {
- *retval = (char)ch;
- retval++;
- maxwidth--;
- }
- *retval = '\0';
- }
- unsigned long sbi_ngets(char *str, unsigned long len)
- {
- int ch;
- unsigned long i;
- for (i = 0; i < len; i++) {
- ch = sbi_getc();
- if (ch < 0)
- break;
- str[i] = ch;
- }
- return i;
- }
- #define PAD_RIGHT 1
- #define PAD_ZERO 2
- #define PAD_ALTERNATE 4
- #define PRINT_BUF_LEN 64
- #define va_start(v, l) __builtin_va_start((v), l)
- #define va_end __builtin_va_end
- #define va_arg __builtin_va_arg
- typedef __builtin_va_list va_list;
- static void printc(char **out, u32 *out_len, char ch)
- {
- if (!out) {
- sbi_putc(ch);
- return;
- }
- /*
- * The *printf entry point functions have enforced that (*out) can
- * only be null when out_len is non-null and its value is zero.
- */
- if (!out_len || *out_len > 1) {
- *(*out)++ = ch;
- **out = '\0';
- }
- if (out_len && *out_len > 0)
- --(*out_len);
- }
- static int prints(char **out, u32 *out_len, const char *string, int width,
- int flags)
- {
- int pc = 0;
- char padchar = ' ';
- if (width > 0) {
- int len = 0;
- const char *ptr;
- for (ptr = string; *ptr; ++ptr)
- ++len;
- if (len >= width)
- width = 0;
- else
- width -= len;
- if (flags & PAD_ZERO)
- padchar = '0';
- }
- if (!(flags & PAD_RIGHT)) {
- for (; width > 0; --width) {
- printc(out, out_len, padchar);
- ++pc;
- }
- }
- for (; *string; ++string) {
- printc(out, out_len, *string);
- ++pc;
- }
- for (; width > 0; --width) {
- printc(out, out_len, padchar);
- ++pc;
- }
- return pc;
- }
- static int printi(char **out, u32 *out_len, long long i, int b, int sg,
- int width, int flags, int letbase)
- {
- char print_buf[PRINT_BUF_LEN];
- char *s;
- int neg = 0, pc = 0;
- u64 t;
- unsigned long long u = i;
- if (sg && b == 10 && i < 0) {
- neg = 1;
- u = -i;
- }
- s = print_buf + PRINT_BUF_LEN - 1;
- *s = '\0';
- if (!u) {
- *--s = '0';
- } else {
- while (u) {
- t = u % b;
- u = u / b;
- if (t >= 10)
- t += letbase - '0' - 10;
- *--s = t + '0';
- }
- }
- if (flags & PAD_ALTERNATE) {
- if ((b == 16) && (letbase == 'A')) {
- *--s = 'X';
- } else if ((b == 16) && (letbase == 'a')) {
- *--s = 'x';
- }
- *--s = '0';
- }
- if (neg) {
- if (width && (flags & PAD_ZERO)) {
- printc(out, out_len, '-');
- ++pc;
- --width;
- } else {
- *--s = '-';
- }
- }
- return pc + prints(out, out_len, s, width, flags);
- }
- static int print(char **out, u32 *out_len, const char *format, va_list args)
- {
- int width, flags, pc = 0;
- char scr[2], *tout;
- bool use_tbuf = (!out) ? true : false;
- unsigned long long tmp;
- /*
- * The console_tbuf is protected by console_out_lock and
- * print() is always called with console_out_lock held
- * when out == NULL.
- */
- if (use_tbuf) {
- console_tbuf_len = CONSOLE_TBUF_MAX;
- tout = console_tbuf;
- out = &tout;
- out_len = &console_tbuf_len;
- }
- for (; *format != 0; ++format) {
- if (use_tbuf && !console_tbuf_len) {
- nputs_all(console_tbuf, CONSOLE_TBUF_MAX);
- console_tbuf_len = CONSOLE_TBUF_MAX;
- tout = console_tbuf;
- }
- if (*format == '%') {
- ++format;
- width = flags = 0;
- if (*format == '\0')
- break;
- if (*format == '%')
- goto literal;
- /* Get flags */
- if (*format == '-') {
- ++format;
- flags = PAD_RIGHT;
- }
- if (*format == '#') {
- ++format;
- flags |= PAD_ALTERNATE;
- }
- while (*format == '0') {
- ++format;
- flags |= PAD_ZERO;
- }
- /* Get width */
- for (; *format >= '0' && *format <= '9'; ++format) {
- width *= 10;
- width += *format - '0';
- }
- if (*format == 's') {
- char *s = va_arg(args, char *);
- pc += prints(out, out_len, s ? s : "(null)",
- width, flags);
- continue;
- }
- if ((*format == 'd') || (*format == 'i')) {
- pc += printi(out, out_len, va_arg(args, int),
- 10, 1, width, flags, '0');
- continue;
- }
- if (*format == 'x') {
- pc += printi(out, out_len,
- va_arg(args, unsigned int), 16, 0,
- width, flags, 'a');
- continue;
- }
- if (*format == 'X') {
- pc += printi(out, out_len,
- va_arg(args, unsigned int), 16, 0,
- width, flags, 'A');
- continue;
- }
- if (*format == 'u') {
- pc += printi(out, out_len,
- va_arg(args, unsigned int), 10, 0,
- width, flags, 'a');
- continue;
- }
- if (*format == 'p') {
- pc += printi(out, out_len,
- va_arg(args, unsigned long), 16, 0,
- width, flags, 'a');
- continue;
- }
- if (*format == 'P') {
- pc += printi(out, out_len,
- va_arg(args, unsigned long), 16, 0,
- width, flags, 'A');
- continue;
- }
- if (*format == 'l' && *(format + 1) == 'l') {
- tmp = va_arg(args, unsigned long long);
- if (*(format + 2) == 'u') {
- format += 2;
- pc += printi(out, out_len, tmp, 10, 0,
- width, flags, 'a');
- } else if (*(format + 2) == 'x') {
- format += 2;
- pc += printi(out, out_len, tmp, 16, 0,
- width, flags, 'a');
- } else if (*(format + 2) == 'X') {
- format += 2;
- pc += printi(out, out_len, tmp, 16, 0,
- width, flags, 'A');
- } else {
- format += 1;
- pc += printi(out, out_len, tmp, 10, 1,
- width, flags, '0');
- }
- continue;
- } else if (*format == 'l') {
- if (*(format + 1) == 'u') {
- format += 1;
- pc += printi(
- out, out_len,
- va_arg(args, unsigned long), 10,
- 0, width, flags, 'a');
- } else if (*(format + 1) == 'x') {
- format += 1;
- pc += printi(
- out, out_len,
- va_arg(args, unsigned long), 16,
- 0, width, flags, 'a');
- } else if (*(format + 1) == 'X') {
- format += 1;
- pc += printi(
- out, out_len,
- va_arg(args, unsigned long), 16,
- 0, width, flags, 'A');
- } else {
- pc += printi(out, out_len,
- va_arg(args, long), 10, 1,
- width, flags, '0');
- }
- }
- if (*format == 'c') {
- /* char are converted to int then pushed on the stack */
- scr[0] = va_arg(args, int);
- scr[1] = '\0';
- pc += prints(out, out_len, scr, width, flags);
- continue;
- }
- } else {
- literal:
- printc(out, out_len, *format);
- ++pc;
- }
- }
- if (use_tbuf && console_tbuf_len < CONSOLE_TBUF_MAX)
- nputs_all(console_tbuf, CONSOLE_TBUF_MAX - console_tbuf_len);
- return pc;
- }
- int sbi_sprintf(char *out, const char *format, ...)
- {
- va_list args;
- int retval;
- if (unlikely(!out))
- sbi_panic("sbi_sprintf called with NULL output string\n");
- va_start(args, format);
- retval = print(&out, NULL, format, args);
- va_end(args);
- return retval;
- }
- int sbi_snprintf(char *out, u32 out_sz, const char *format, ...)
- {
- va_list args;
- int retval;
- if (unlikely(!out && out_sz != 0))
- sbi_panic("sbi_snprintf called with NULL output string and "
- "output size is not zero\n");
- va_start(args, format);
- retval = print(&out, &out_sz, format, args);
- va_end(args);
- return retval;
- }
- int sbi_printf(const char *format, ...)
- {
- va_list args;
- int retval;
- spin_lock(&console_out_lock);
- va_start(args, format);
- retval = print(NULL, NULL, format, args);
- va_end(args);
- spin_unlock(&console_out_lock);
- return retval;
- }
- int sbi_dprintf(const char *format, ...)
- {
- va_list args;
- int retval = 0;
- struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
- va_start(args, format);
- if (scratch->options & SBI_SCRATCH_DEBUG_PRINTS) {
- spin_lock(&console_out_lock);
- retval = print(NULL, NULL, format, args);
- spin_unlock(&console_out_lock);
- }
- va_end(args);
- return retval;
- }
- void sbi_panic(const char *format, ...)
- {
- va_list args;
- spin_lock(&console_out_lock);
- va_start(args, format);
- print(NULL, NULL, format, args);
- va_end(args);
- spin_unlock(&console_out_lock);
- sbi_hart_hang();
- }
- const struct sbi_console_device *sbi_console_get_device(void)
- {
- return console_dev;
- }
- void sbi_console_set_device(const struct sbi_console_device *dev)
- {
- if (!dev || console_dev)
- return;
- console_dev = dev;
- }
- int sbi_console_init(struct sbi_scratch *scratch)
- {
- return sbi_platform_console_init(sbi_platform_ptr(scratch));
- }
|