nios2: add early printk support

Signed-off-by: Ley Foon Tan <lftan@altera.com>

4 files changed, 174 insertions, 1 deletions

diff --git a/arch/nios2/kernel/Makefile b/arch/nios2/kernel/Makefile
index 8ae76823ff93..eaaa894c432f 100644
--- a/arch/nios2/kernel/Makefile
+++ b/arch/nios2/kernel/Makefile
@@ -20,5 +20,6 @@ obj-y	+= syscall_table.o
 obj-y   += time.o
 obj-y   += traps.o
 
+obj-$(CONFIG_EARLY_PRINTK)              += early_printk.o
 obj-$(CONFIG_MODULES)                   += module.o
 obj-$(CONFIG_NIOS2_ALIGNMENT_TRAP)      += misaligned.o
diff --git a/arch/nios2/kernel/early_printk.c b/arch/nios2/kernel/early_printk.c
new file mode 100644
index 000000000000..c08e4c1486fc
--- /dev/null
+++ b/arch/nios2/kernel/early_printk.c
@@ -0,0 +1,118 @@
+/*
+ * Early printk for Nios2.
+ *
+ * Copyright (C) 2015, Altera Corporation
+ * Copyright (C) 2010, Tobias Klauser <tklauser@distanz.ch>
+ * Copyright (C) 2009, Wind River Systems Inc
+ *   Implemented by fredrik.markstrom@gmail.com and ivarholmqvist@gmail.com
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+
+#include <asm/prom.h>
+
+static unsigned long base_addr;
+
+#if defined(CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE)
+
+#define ALTERA_JTAGUART_DATA_REG                0
+#define ALTERA_JTAGUART_CONTROL_REG             4
+#define ALTERA_JTAGUART_CONTROL_WSPACE_MSK      0xFFFF0000
+#define ALTERA_JTAGUART_CONTROL_AC_MSK          0x00000400
+
+#define JUART_GET_CR() \
+        __builtin_ldwio((void *)(base_addr + ALTERA_JTAGUART_CONTROL_REG))
+#define JUART_SET_CR(v) \
+        __builtin_stwio((void *)(base_addr + ALTERA_JTAGUART_CONTROL_REG), v)
+#define JUART_SET_TX(v) \
+        __builtin_stwio((void *)(base_addr + ALTERA_JTAGUART_DATA_REG), v)
+
+static void early_console_write(struct console *con, const char *s, unsigned n)
+{
+        unsigned long status;
+
+        while (n-- && *s) {
+                while (((status = JUART_GET_CR())
+                                & ALTERA_JTAGUART_CONTROL_WSPACE_MSK) == 0) {
+#if defined(CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE_BYPASS)
+                        if ((status & ALTERA_JTAGUART_CONTROL_AC_MSK) == 0)
+                                return; /* no connection activity */
+#endif
+                }
+                JUART_SET_TX(*s);
+                s++;
+        }
+}
+
+#elif defined(CONFIG_SERIAL_ALTERA_UART_CONSOLE)
+
+#define ALTERA_UART_TXDATA_REG          4
+#define ALTERA_UART_STATUS_REG          8
+#define ALTERA_UART_STATUS_TRDY         0x0040
+
+#define UART_GET_SR() \
+        __builtin_ldwio((void *)(base_addr + ALTERA_UART_STATUS_REG))
+#define UART_SET_TX(v) \
+        __builtin_stwio((void *)(base_addr + ALTERA_UART_TXDATA_REG), v)
+
+static void early_console_putc(char c)
+{
+        while (!(UART_GET_SR() & ALTERA_UART_STATUS_TRDY))
+                ;
+
+        UART_SET_TX(c);
+}
+
+static void early_console_write(struct console *con, const char *s, unsigned n)
+{
+        while (n-- && *s) {
+                early_console_putc(*s);
+                if (*s == '\n')
+                        early_console_putc('\r');
+                s++;
+        }
+}
+
+#else
+# error Neither SERIAL_ALTERA_JTAGUART_CONSOLE nor SERIAL_ALTERA_UART_CONSOLE \
+selected
+#endif
+
+static struct console early_console_prom = {
+        .name   = "early",
+        .write  = early_console_write,
+        .flags  = CON_PRINTBUFFER | CON_BOOT,
+        .index  = -1
+};
+
+void __init setup_early_printk(void)
+{
+#if defined(CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE) ||   \
+        defined(CONFIG_SERIAL_ALTERA_UART_CONSOLE)
+        base_addr = of_early_console();
+#else
+        base_addr = 0;
+#endif
+
+        if (!base_addr)
+                return;
+
+#if defined(CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE_BYPASS)
+        /* Clear activity bit so BYPASS doesn't stall if we've used JTAG for
+         * downloading the kernel. This might cause early data to be lost even
+         * if the JTAG terminal is running.
+         */
+        JUART_SET_CR(JUART_GET_CR() | ALTERA_JTAGUART_CONTROL_AC_MSK);
+#endif
+
+        early_console = &early_console_prom;
+        register_console(early_console);
+        pr_info("early_console initialized at 0x%08lx\n", base_addr);
+}
diff --git a/arch/nios2/kernel/prom.c b/arch/nios2/kernel/prom.c
index 0522d3378e3f..718dd197909f 100644
--- a/arch/nios2/kernel/prom.c
+++ b/arch/nios2/kernel/prom.c
@@ -1,7 +1,7 @@
 /*
  * Device tree support
  *
- * Copyright (C) 2013 Altera Corporation
+ * Copyright (C) 2013, 2015 Altera Corporation
  * Copyright (C) 2010 Thomas Chou <thomas@wytron.com.tw>
  *
  * Based on MIPS support for CONFIG_OF device tree support
@@ -30,6 +30,7 @@
 #include <linux/of_fdt.h>
 #include <linux/io.h>
 
+#include <asm/prom.h>
 #include <asm/sections.h>
 
 void __init early_init_dt_add_memory_arch(u64 base, u64 size)
@@ -63,3 +64,52 @@ void __init early_init_devtree(void *params)
 
         early_init_dt_scan(params);
 }
+
+#ifdef CONFIG_EARLY_PRINTK
+static int __init early_init_dt_scan_serial(unsigned long node,
+                        const char *uname, int depth, void *data)
+{
+        u64 *addr64 = (u64 *) data;
+        const char *p;
+
+        /* only consider serial nodes */
+        if (strncmp(uname, "serial", 6) != 0)
+                return 0;
+
+        p = of_get_flat_dt_prop(node, "compatible", NULL);
+        if (!p)
+                return 0;
+
+        /*
+         * We found an altera_jtaguart but it wasn't configured for console, so
+         * skip it.
+         */
+#ifndef CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE
+        if (strncmp(p, "altr,juart", 10) == 0)
+                return 0;
+#endif
+
+        /*
+         * Same for altera_uart.
+         */
+#ifndef CONFIG_SERIAL_ALTERA_UART_CONSOLE
+        if (strncmp(p, "altr,uart", 9) == 0)
+                return 0;
+#endif
+
+        *addr64 = fdt_translate_address((const void *)initial_boot_params,
+                node);
+
+        return *addr64 == OF_BAD_ADDR ? 0 : 1;
+}
+
+unsigned long __init of_early_console(void)
+{
+        u64 base = 0;
+
+        if (of_scan_flat_dt(early_init_dt_scan_serial, &base))
+                return (u32)ioremap(base, 32);
+        else
+                return 0;
+}
+#endif /* CONFIG_EARLY_PRINTK */
diff --git a/arch/nios2/kernel/setup.c b/arch/nios2/kernel/setup.c
index cb3121f975d4..b101a43d3c5a 100644
--- a/arch/nios2/kernel/setup.c
+++ b/arch/nios2/kernel/setup.c
@@ -139,6 +139,10 @@ void __init setup_arch(char **cmdline_p)
 
         console_verbose();
 
+#ifdef CONFIG_EARLY_PRINTK
+        setup_early_printk();
+#endif
+
         memory_start = PAGE_ALIGN((unsigned long)__pa(_end));
         memory_end = (unsigned long) CONFIG_NIOS2_MEM_BASE + memory_size;