bsp/tms570: Support printk() early

Allow use of printk() early in the initalization and without a console
driver.

1 files changed, 57 insertions, 13 deletions

diff --git a/c/src/lib/libbsp/arm/tms570/console/printk-support.c b/c/src/lib/libbsp/arm/tms570/console/printk-support.c
index 77e4f1ada6..668f2d9959 100644
--- a/c/src/lib/libbsp/arm/tms570/console/printk-support.c
+++ b/c/src/lib/libbsp/arm/tms570/console/printk-support.c
@@ -24,10 +24,12 @@
  */
 
 #include <rtems/bspIo.h>
+#include <rtems/sysinit.h>
 #include <stdint.h>
 #include <bsp/tms570-sci.h>
 #include <bsp/tms570-sci-driver.h>
 
+#define TMS570_CONSOLE (&driver_context_table[0])
 
 /**
  * @brief Puts chars into peripheral
@@ -36,15 +38,20 @@
  *
  * @retval Void
  */
-static void tms570_putc(char ch)
+static void tms570_debug_console_putc(char ch)
 {
+  tms570_sci_context *ctx = TMS570_CONSOLE;
+  volatile tms570_sci_t *regs = ctx->regs;
   rtems_interrupt_level level;
 
   rtems_interrupt_disable(level);
-  while ( ( driver_context_table[0].regs->FLR & TMS570_SCI_FLR_TXRDY ) == 0) {
+  while ( ( regs->FLR & TMS570_SCI_FLR_TXRDY ) == 0) {
+    rtems_interrupt_flash(level);
+  }
+  regs->TD = ch;
+  while ( ( regs->FLR & TMS570_SCI_FLR_TX_EMPTY ) == 0) {
     rtems_interrupt_flash(level);
   }
-  driver_context_table[0].regs->TD = ch;
   rtems_interrupt_enable(level);
 }
 
@@ -55,13 +62,31 @@ static void tms570_putc(char ch)
  *
  * @retval Void
  */
-static void tms570_uart_output(char c)
+static void tms570_debug_console_out(char c)
 {
   if ( c == '\n' ) {
-    char r = '\r';
-    tms570_putc(r);
+    tms570_debug_console_putc('\r');
   }
-  tms570_putc(c);
+
+  tms570_debug_console_putc(c);
+}
+
+static void tms570_debug_console_init(void)
+{
+  tms570_sci_context *ctx = TMS570_CONSOLE;
+  struct termios term;
+
+  tms570_sci_initialize(ctx);
+  memset(&term, 0, sizeof(term));
+  term.c_cflag = B115200;
+  tms570_sci_set_attributes(&ctx->base, &term);
+  BSP_output_char = tms570_debug_console_out;
+}
+
+static void tms570_debug_console_early_init(char c)
+{
+  tms570_debug_console_init();
+  tms570_debug_console_out(c);
 }
 
 /**
@@ -72,14 +97,33 @@ static void tms570_uart_output(char c)
  * @retval x Read char
  * @retval -1 No input character available
  */
-static int tms570_uart_input( void )
+static int tms570_debug_console_in( void )
 {
-  if ( driver_context_table[0].regs->FLR & TMS570_SCI_FLR_RXRDY ) {
-      return driver_context_table[0].regs->RD;
+  tms570_sci_context *ctx = TMS570_CONSOLE;
+  volatile tms570_sci_t *regs = ctx->regs;
+  rtems_interrupt_level level;
+  int c;
+
+  rtems_interrupt_disable(level);
+
+  if ( regs->FLR & TMS570_SCI_FLR_RXRDY ) {
+      c = (unsigned char) regs->RD;
   } else {
-      return -1;
+      c = -1;
   }
+
+  rtems_interrupt_enable(level);
+
+  return c;
 }
 
-BSP_output_char_function_type BSP_output_char = tms570_uart_output;
-BSP_polling_getchar_function_type BSP_poll_char = tms570_uart_input;
+BSP_output_char_function_type BSP_output_char =
+  tms570_debug_console_early_init;
+
+BSP_polling_getchar_function_type BSP_poll_char = tms570_debug_console_in;
+
+RTEMS_SYSINIT_ITEM(
+  tms570_debug_console_init,
+  RTEMS_SYSINIT_BSP_START,
+  RTEMS_SYSINIT_ORDER_LAST
+);