diff --git a/src/arch/armv8/aarch32/aborts.c b/src/arch/armv8/aarch32/aborts.c index 9e310d836..da9269810 100644 --- a/src/arch/armv8/aarch32/aborts.c +++ b/src/arch/armv8/aarch32/aborts.c @@ -10,10 +10,10 @@ void internal_abort_handler(unsigned long gprs[]) { for(ssize_t i = 14; i >= 0; i--) { - printk("x%d:\t\t0x%0lx\n", i, gprs[14 - i]); + console_printk("x%d:\t\t0x%0lx\n", i, gprs[14 - i]); } - printk("ESR:\t0x%0lx\n", sysreg_esr_el2_read()); - printk("ELR:\t0x%0lx\n", sysreg_elr_el2_read()); - printk("FAR:\t0x%0lx\n", sysreg_far_el2_read()); + console_printk("ESR:\t0x%0lx\n", sysreg_esr_el2_read()); + console_printk("ELR:\t0x%0lx\n", sysreg_elr_el2_read()); + console_printk("FAR:\t0x%0lx\n", sysreg_far_el2_read()); ERROR("cpu%d internal hypervisor abort - PANIC\n", cpu()->id); } diff --git a/src/arch/armv8/aarch64/aborts.c b/src/arch/armv8/aarch64/aborts.c index c8888c5b9..0f71eb3c3 100644 --- a/src/arch/armv8/aarch64/aborts.c +++ b/src/arch/armv8/aarch64/aborts.c @@ -10,11 +10,11 @@ void internal_abort_handler(unsigned long gprs[]) { for(size_t i = 0; i < 31; i++) { - printk("x%d:\t\t0x%0lx\n", i, gprs[i]); + console_printk("x%d:\t\t0x%0lx\n", i, gprs[i]); } - printk("SP:\t\t0x%0lx\n", gprs[31]); - printk("ESR:\t0x%0lx\n", sysreg_esr_el2_read()); - printk("ELR:\t0x%0lx\n", sysreg_elr_el2_read()); - printk("FAR:\t0x%0lx\n", sysreg_far_el2_read()); + console_printk("SP:\t\t0x%0lx\n", gprs[31]); + console_printk("ESR:\t0x%0lx\n", sysreg_esr_el2_read()); + console_printk("ELR:\t0x%0lx\n", sysreg_elr_el2_read()); + console_printk("FAR:\t0x%0lx\n", sysreg_far_el2_read()); ERROR("cpu%d internal hypervisor abort - PANIC\n", cpu()->id); } diff --git a/src/arch/riscv/sync_exceptions.c b/src/arch/riscv/sync_exceptions.c index 9bcd1d1c1..6f6056b86 100644 --- a/src/arch/riscv/sync_exceptions.c +++ b/src/arch/riscv/sync_exceptions.c @@ -13,11 +13,11 @@ void internal_exception_handler(unsigned long gprs[]) { for(int i = 0; i < 31; i++) { - printk("x%d:\t\t0x%0lx\n", i, gprs[i]); + console_printk("x%d:\t\t0x%0lx\n", i, gprs[i]); } - printk("sstatus:\t0x%0lx\n", CSRR(sstatus)); - printk("stval:\t\t0x%0lx\n", CSRR(stval)); - printk("sepc:\t\t0x%0lx\n", CSRR(sepc)); + console_printk("sstatus:\t0x%0lx\n", CSRR(sstatus)); + console_printk("stval:\t\t0x%0lx\n", CSRR(stval)); + console_printk("sepc:\t\t0x%0lx\n", CSRR(sepc)); ERROR("cpu%d internal hypervisor abort - PANIC\n", cpu()->id); } diff --git a/src/core/console.c b/src/core/console.c index 7a0c25366..e9c30d7af 100644 --- a/src/core/console.c +++ b/src/core/console.c @@ -11,10 +11,12 @@ #include #include #include +#include +#include -volatile bao_uart_t *uart; -bool ready = false; -static spinlock_t print_lock = SPINLOCK_INITVAL; +static volatile bao_uart_t *uart; +static bool console_ready = false; +static spinlock_t console_lock = SPINLOCK_INITVAL; void console_init() { @@ -31,16 +33,40 @@ void console_init() uart_init(uart); uart_enable(uart); - ready = true; + console_ready = true; } cpu_sync_and_clear_msgs(&cpu_glb_sync); } -void console_write(char const* const str) + +void console_write(const char* buf, size_t n) { - if (!ready) return; - spin_lock(&print_lock); - uart_puts(uart, str); - spin_unlock(&print_lock); + while (!console_ready); + for (size_t i = 0; i < n; i++) { + if (buf[i] == '\n') { + uart_putc(uart, '\r'); + } + uart_putc(uart, buf[i]); + } +} + +#define PRINTF_BUFFER_LEN (256) +static char console_bufffer[PRINTF_BUFFER_LEN]; + +__attribute__((format(printf, 1, 2))) void console_printk(const char* fmt, ...) +{ + va_list args; + size_t chars_writen; + const char* fmt_it = fmt; + + va_start(args, fmt); + spin_lock(&console_lock); + while (*fmt_it != '\0') { + chars_writen = + vsnprintk(console_bufffer, PRINTF_BUFFER_LEN, &fmt_it, &args); + console_write(console_bufffer, min(PRINTF_BUFFER_LEN, chars_writen)); + } + spin_unlock(&console_lock); + va_end(args); } diff --git a/src/core/inc/bao.h b/src/core/inc/bao.h index 500240af2..388e8b2a7 100644 --- a/src/core/inc/bao.h +++ b/src/core/inc/bao.h @@ -11,20 +11,20 @@ #ifndef __ASSEMBLER__ #include -#include +#include #include #define INFO(args, ...) \ - printk("BAO INFO: " args "\n" __VA_OPT__(, ) __VA_ARGS__); + console_printk("BAO INFO: " args "\n" __VA_OPT__(, ) __VA_ARGS__); #define WARNING(args, ...) \ - printk("BAO WARNING: " args "\n" __VA_OPT__(, ) __VA_ARGS__); + console_printk("BAO WARNING: " args "\n" __VA_OPT__(, ) __VA_ARGS__); -#define ERROR(args, ...) \ - { \ - printk("BAO ERROR: " args "\n" __VA_OPT__(, ) __VA_ARGS__); \ - while (1) \ - ; \ +#define ERROR(args, ...) \ + { \ + console_printk("BAO ERROR: " args "\n" __VA_OPT__(, ) __VA_ARGS__); \ + while (1) \ + ; \ } #endif /* __ASSEMBLER__ */ diff --git a/src/core/inc/console.h b/src/core/inc/console.h index 03808a10e..c38e1511f 100644 --- a/src/core/inc/console.h +++ b/src/core/inc/console.h @@ -9,6 +9,7 @@ #include void console_init(); -void console_write(char const* const str); +void console_write(const char* buf, size_t n); +void console_printk(const char* fmt, ...); #endif /* __CONSOLE_H__ */ diff --git a/src/core/init.c b/src/core/init.c index 00dca238b..56a960144 100644 --- a/src/core/init.c +++ b/src/core/init.c @@ -27,7 +27,7 @@ void init(cpuid_t cpu_id, paddr_t load_addr) console_init(); if (cpu()->id == CPU_MASTER) { - printk("Bao Hypervisor\n\r"); + console_printk("Bao Hypervisor\n\r"); } interrupts_init(); diff --git a/src/lib/inc/printk.h b/src/lib/inc/printk.h index 930f36eb9..0d6252ee7 100644 --- a/src/lib/inc/printk.h +++ b/src/lib/inc/printk.h @@ -7,7 +7,9 @@ #define __PRINTK_H #include +#include -size_t printk(const char *fmt, ...); +size_t vsnprintk(char* buf, size_t buf_size, const char** fmt, + va_list* args); #endif /* __PRINTK_H */ diff --git a/src/lib/printk.c b/src/lib/printk.c index af6066402..67138e06f 100644 --- a/src/lib/printk.c +++ b/src/lib/printk.c @@ -5,166 +5,178 @@ #include -#include -#include -#include +#define F_LONG (1U << 0U) +#define F_UNSIGNED (1U << 1U) +#define F_BASE16 (1U << 2U) -#define PRINT_TEXT_LEN 0x100 +static inline char digit_to_char(unsigned long i, unsigned int base) +{ + unsigned long c; + unsigned long digit = i % base; + if (i < 10U) { + c = ((unsigned long)'0') + digit; + } else { + c = ((unsigned long)'a') + + (digit - 10U); + } + return (char)c; +} + +static inline void printc(char** buf, char c) +{ + if (buf != NULL) { + **buf = c; + (*buf)++; + } +} -size_t vsprintk(char *buf, const char *fmt, va_list args) +static size_t prints(char** buf, const char* str) { - char *str; - str = buf; - size_t len = strnlen(fmt, PRINT_TEXT_LEN); - for (; *fmt; ++fmt) { - if ((*fmt != '%') && (*fmt != '\n') && (*fmt != '\t')) { - *str++ = *fmt; - continue; + const char* str_it = str; + size_t char_count = 0; + while (*str_it != '\0') { + printc(buf, *str_it); + char_count++; + str_it++; + } + return char_count; +} + +static size_t vprintd(char** buf, unsigned int flags, va_list* args) +{ + unsigned long u; + size_t base = ((flags & F_BASE16) != 0U) ? (unsigned int)16U : 10U; + bool is_long = ((flags & F_LONG) != 0U); + bool is_unsigned = ((flags & F_UNSIGNED) != 0U) || (base != 10U); + size_t divisor; + unsigned long tmp; + size_t char_count = 0; + + if (is_unsigned) { + u = is_long ? va_arg(*args, unsigned long) : + va_arg(*args, unsigned int); + } else { + signed long s = + is_long ? va_arg(*args, signed long) : va_arg(*args, signed int); + if (s < 0) { + printc(buf, '-'); + char_count++; + s = -s; } + u = (unsigned long)s; + } - if (*fmt == '%') { - ++fmt; - bool is_unsigned = false; - bool zero_padding = false; - bool is_long = false; + divisor = 1; + tmp = u; + while (tmp >= base) { + divisor *= base; + tmp /= base; + } + + while (divisor > 0U) { + unsigned long digit = u / divisor; + u -= digit * divisor; + divisor /= base; + printc(buf, digit_to_char(digit, base)); + char_count++; + } - if (*fmt == '0') { - ++fmt; - zero_padding = true; + return char_count; +} + +/** + * This is a limited printf implementation. The format string only supports + * integer, string and char arguments. That is, 'd', 'u' or 'x', 's' and 'c' + * specifiers, respectively. For integers, it only supports the none and 'l' + * lengths. It does not support any flags, width or precision fields. If + * present, this fields are ignored. + * + * Note this does not follow the C lib vsnprintf specification. It returns the + * numbers of characters written to the buffer, and changes fmt to point to the + * first character that was not printed. + */ +size_t vsnprintk(char* buf, size_t buf_size, const char** fmt, + va_list* args) +{ + char* buf_it = buf; + size_t buf_left = buf_size; + const char* fmt_it = *fmt; + va_list args_tmp; + + while ((*fmt_it != '\0') && (buf_left > 0U)) { + if ((*fmt_it) != '%') { + printc(&buf_it, *fmt_it); + buf_left--; + } else { + unsigned int flags; + bool ignore_char; + size_t arg_char_count = 0; + + fmt_it++; + flags = 0; + if (*fmt_it == 'l') { + fmt_it++; + flags = flags | F_LONG; + if (*fmt_it == 'l') { + fmt_it++; + } // ignore long long } - if (*fmt == 'l') { - ++fmt; - is_long = true; - } - - switch (*fmt) { - case 'x': { - unsigned long number = is_long ? va_arg(args, unsigned long) : va_arg(args, unsigned); - size_t length = is_long ? 16 : 8; - size_t length_in_bits = is_long ? 64 : 32; - uint8_t byte = 0; - size_t i = 0; - bool keep_zeros = false; - - for (i = 0; i < length; i++) { - byte = number >> (length_in_bits - ((i + 1) * 4)); - byte = byte & 0xF; - if (byte != 0 || i == length-1) { - keep_zeros = true; - } - if (keep_zeros || zero_padding) { - if ((byte >= 0) && (byte <= 9)) { - byte += 0x30; - } else { - byte += (0x61-0xa); - } - *str++ = byte; + do { + ignore_char = false; + switch (*fmt_it) { + case 'x': + case 'X': + flags = flags | F_BASE16; + /* fallthrough */ + case 'u': + flags = flags | F_UNSIGNED; + /* fallthrough */ + case 'd': + case 'i': + va_copy(args_tmp, *args); + arg_char_count = vprintd(NULL, flags, &args_tmp); + if (arg_char_count <= buf_left) { + (void)vprintd(&buf_it, flags, args); } - } - break; - } - case 'u': - is_unsigned = true; - case 'i': - case 'd': { - size_t i, j, max_num_zeros, num_of_digits_uint64_t, - number, divisor_value_uint64_t, - new_div_val = 1, sw_quotient_value = 0; - bool keep_zeros = false; - - if (!is_unsigned) { - long number_signed = is_long ? va_arg(args, long) : va_arg(args, int); - if (number_signed < 0) { - *str++ = 0x2d; - number_signed = -(number_signed); + break; + case 's': + va_copy(args_tmp, *args); + arg_char_count = prints(NULL, va_arg(args_tmp, char*)); + if (arg_char_count <= buf_left) { + (void)prints(&buf_it, va_arg(*args, char*)); } - number = number_signed; - } else { - number = is_long ? va_arg(args, unsigned long) : va_arg(args, unsigned); - } - - divisor_value_uint64_t = 1000000000; - num_of_digits_uint64_t = 10; - max_num_zeros = num_of_digits_uint64_t - 1; - - for (i = 0; i < max_num_zeros; i++) { - while (number >= divisor_value_uint64_t) { - number -= divisor_value_uint64_t; - ++sw_quotient_value; + break; + case 'c': + arg_char_count = 1; + if (arg_char_count <= buf_left) { + printc(&buf_it, (char)va_arg(args_tmp, int)); } - if (sw_quotient_value != 0) keep_zeros = true; - if (keep_zeros || zero_padding) { - sw_quotient_value += 0x30; - *str++ = sw_quotient_value; + break; + case '%': + arg_char_count = 1; + if (arg_char_count <= buf_left) { + printc(&buf_it, *fmt_it); } - j = i; - while (j < (max_num_zeros - 1)) { - new_div_val *= 10; - j++; - } - sw_quotient_value = 0; - divisor_value_uint64_t = new_div_val; - new_div_val = 1; - } - *str++ = (number + 0x30); - break; - } - case 's': { - char *arg_string = va_arg(args, char *); - while (((*str = *arg_string++) && (len < PRINT_TEXT_LEN))) { - ++str; - len++; - } - break; - } - case 'c': { - char character = va_arg(args, int); - *str++ = character; - break; - } - case '%': { - *str++ = *fmt; - break; - } - case '\t': { - *str++ = '%'; - *str++ = *fmt; - break; - } - case '\n': { - *str++ = '%'; - *str++ = '\r'; - *str++ = '\n'; - break; + break; + default: + ignore_char = true; + break; } - default: { + } while (ignore_char); + + if (arg_char_count <= buf_left) { + buf_left -= arg_char_count; + } else { + while (*fmt_it != '%') { + fmt_it--; } + break; } } - - if (*fmt == '\n') { - *str++ = '\r'; - *str++ = '\n'; - } - if (*fmt == '\t') *str++ = *fmt; + fmt_it++; } - *str = '\0'; - return strnlen(fmt, PRINT_TEXT_LEN); -} - -size_t printk(const char *fmt, ...) -{ - va_list args; - size_t i; - - char print_buffer[PRINT_TEXT_LEN]; - - va_start(args, fmt); - - i = vsprintk(print_buffer, fmt, args); - va_end(args); - console_write(print_buffer); - return i; + *fmt = fmt_it; + return buf_size - buf_left; } diff --git a/src/platform/drivers/8250_uart/8250_uart.c b/src/platform/drivers/8250_uart/8250_uart.c index 2b9838951..0a63e775f 100644 --- a/src/platform/drivers/8250_uart/8250_uart.c +++ b/src/platform/drivers/8250_uart/8250_uart.c @@ -39,9 +39,3 @@ void uart_putc(volatile struct uart8250_hw *uart, int8_t c){ while(!(uart->lsr & UART8250_LSR_THRE)); uart->thr = c; } - -void uart_puts(volatile struct uart8250_hw *uart, char const* str){ - while (*str) { - uart_putc(uart, *str++); - } -} diff --git a/src/platform/drivers/8250_uart/inc/drivers/8250_uart.h b/src/platform/drivers/8250_uart/inc/drivers/8250_uart.h index 566957ec2..f93f2da31 100644 --- a/src/platform/drivers/8250_uart/inc/drivers/8250_uart.h +++ b/src/platform/drivers/8250_uart/inc/drivers/8250_uart.h @@ -65,6 +65,5 @@ typedef struct uart8250_hw bao_uart_t; void uart_enable(volatile struct uart8250_hw *uart); void uart_init(volatile struct uart8250_hw *uart); -void uart_puts(volatile struct uart8250_hw *uart, const char* str); #endif /* UART8250_H */ diff --git a/src/platform/drivers/nxp_uart/inc/drivers/nxp_uart.h b/src/platform/drivers/nxp_uart/inc/drivers/nxp_uart.h index b3b24eb71..f2a9d81ad 100644 --- a/src/platform/drivers/nxp_uart/inc/drivers/nxp_uart.h +++ b/src/platform/drivers/nxp_uart/inc/drivers/nxp_uart.h @@ -33,6 +33,4 @@ typedef struct lpuart bao_uart_t; void uart_enable(volatile struct lpuart *uart); void uart_init(volatile struct lpuart *uart); -void uart_puts(volatile struct lpuart * uart, const char* str); - #endif /* __UART_NXP_H */ diff --git a/src/platform/drivers/nxp_uart/nxp_uart.c b/src/platform/drivers/nxp_uart/nxp_uart.c index 8c96bcb27..3345ae082 100644 --- a/src/platform/drivers/nxp_uart/nxp_uart.c +++ b/src/platform/drivers/nxp_uart/nxp_uart.c @@ -20,10 +20,3 @@ void uart_putc(volatile struct lpuart *uart, char c){ while(!(uart->stat & LPUART_STAT_TDRE_BIT)); uart->data = c; } - -void uart_puts(volatile struct lpuart *uart, char const* str){ - while (*str) { - uart_putc(uart, *str++); - } -} - diff --git a/src/platform/drivers/pl011_uart/inc/drivers/pl011_uart.h b/src/platform/drivers/pl011_uart/inc/drivers/pl011_uart.h index 96ddfb334..d04fffaf1 100644 --- a/src/platform/drivers/pl011_uart/inc/drivers/pl011_uart.h +++ b/src/platform/drivers/pl011_uart/inc/drivers/pl011_uart.h @@ -206,6 +206,5 @@ void uart_set_baud_rate(volatile struct Pl011_Uart_hw * ptr_uart, uint32_t baud_ void uart_init(volatile struct Pl011_Uart_hw * ptr_uart); uint32_t uart_getc(volatile struct Pl011_Uart_hw * ptr_uart); void uart_putc(volatile struct Pl011_Uart_hw * ptr_uart,int8_t c); -void uart_puts(volatile struct Pl011_Uart_hw * ptr_uart,const char *s); #endif /* __PL011_UART_H_ */ diff --git a/src/platform/drivers/pl011_uart/pl011_uart.c b/src/platform/drivers/pl011_uart/pl011_uart.c index e65bc777b..a9b35a397 100644 --- a/src/platform/drivers/pl011_uart/pl011_uart.c +++ b/src/platform/drivers/pl011_uart/pl011_uart.c @@ -112,12 +112,3 @@ void uart_putc(volatile struct Pl011_Uart_hw * ptr_uart,int8_t c){ } - -void uart_puts(volatile struct Pl011_Uart_hw * ptr_uart,const char *s){ - - while (*s) - { - uart_putc(ptr_uart,*s++); - } - -} diff --git a/src/platform/drivers/sbi_uart/inc/drivers/sbi_uart.h b/src/platform/drivers/sbi_uart/inc/drivers/sbi_uart.h index 334b2d188..c93fb4304 100644 --- a/src/platform/drivers/sbi_uart/inc/drivers/sbi_uart.h +++ b/src/platform/drivers/sbi_uart/inc/drivers/sbi_uart.h @@ -12,6 +12,6 @@ typedef volatile uint8_t bao_uart_t; bool uart_init(bao_uart_t* uart); void uart_enable(bao_uart_t* uart); -void uart_puts(bao_uart_t* uart,const char *s); +void uart_putc(bao_uart_t* uart, const char c); #endif /* __SBI_UART_H__ */ diff --git a/src/platform/drivers/sbi_uart/sbi_uart.c b/src/platform/drivers/sbi_uart/sbi_uart.c index 1965ea2e4..9b9ae4444 100644 --- a/src/platform/drivers/sbi_uart/sbi_uart.c +++ b/src/platform/drivers/sbi_uart/sbi_uart.c @@ -12,8 +12,6 @@ bool uart_init(bao_uart_t* uart) } void uart_enable(bao_uart_t* uart) {} -void uart_puts(bao_uart_t* uart, char const* const str) -{ - char const* ptr = str; - while (*ptr) sbi_console_putchar(*ptr++); +void uart_putc(bao_uart_t* uart, const char c) { + sbi_console_putchar(c); } diff --git a/src/platform/drivers/zynq_uart/inc/drivers/zynq_uart.h b/src/platform/drivers/zynq_uart/inc/drivers/zynq_uart.h index 49e40586d..8dbbfad23 100644 --- a/src/platform/drivers/zynq_uart/inc/drivers/zynq_uart.h +++ b/src/platform/drivers/zynq_uart/inc/drivers/zynq_uart.h @@ -280,6 +280,5 @@ void uart_disable(volatile struct Uart_Zynq_hw* uart); bool uart_set_baud_rate(volatile struct Uart_Zynq_hw* uart, uint32_t baud_rate); uint32_t uart_getc(volatile struct Uart_Zynq_hw* uart); void uart_putc(volatile struct Uart_Zynq_hw* uart,int8_t c); -void uart_puts(volatile struct Uart_Zynq_hw* uart,const char *s); #endif /* __UART_ZYNQ_H */ diff --git a/src/platform/drivers/zynq_uart/zynq_uart.c b/src/platform/drivers/zynq_uart/zynq_uart.c index 4aff84800..1ae5dd96d 100644 --- a/src/platform/drivers/zynq_uart/zynq_uart.c +++ b/src/platform/drivers/zynq_uart/zynq_uart.c @@ -108,10 +108,3 @@ void uart_putc(volatile struct Uart_Zynq_hw* uart, int8_t c) uart->tx_rx_fifo = c; } - -void uart_puts(volatile struct Uart_Zynq_hw* uart, const char* s) -{ - while (*s) { - uart_putc(uart, *s++); - } -}