From 3165ab9a970ac5e4bd80184c27011e7802581263 Mon Sep 17 00:00:00 2001
From: "Huang, Tao" <huangtao@rock-chips.com>
Date: Sat, 21 Mar 2015 16:48:49 +0800
Subject: [PATCH] rk_serial: support console write by thread

Signed-off-by: Huang, Tao <huangtao@rock-chips.com>
---
 drivers/tty/serial/rk_serial.c | 91 +++++++++++++++++++++++++++++++++-
 1 file changed, 90 insertions(+), 1 deletion(-)

diff --git a/drivers/tty/serial/rk_serial.c b/drivers/tty/serial/rk_serial.c
index ea915c6a41b4..2f9d8de00ac2 100644
--- a/drivers/tty/serial/rk_serial.c
+++ b/drivers/tty/serial/rk_serial.c
@@ -79,10 +79,13 @@
 
 #define PORT_RK		90
 #define UART_USR	0x1F	/* UART Status Register */
+#define UART_USR_TX_FIFO_EMPTY		0x04 /* Transmit FIFO empty */
+#define UART_USR_TX_FIFO_NOT_FULL	0x02 /* Transmit FIFO not full */
 #define UART_USR_BUSY (1)
 #define UART_IER_PTIME	0x80	/* Programmable THRE Interrupt Mode Enable */
 #define UART_LSR_RFE	0x80    /* receive fifo error */
 #define UART_SRR		0x22    /* software reset register */
+#define UART_SFE	0x26	/* Shadow FIFO Enable */
 #define UART_RESET		0x01
 
 
@@ -1750,6 +1753,85 @@ serial_rk_console_write(struct console *co, const char *s, unsigned int count)
 	local_irq_restore(flags);
 }
 
+#ifdef CONFIG_RK_CONSOLE_THREAD
+#include <linux/kfifo.h>
+#include <linux/kthread.h>
+static struct task_struct *console_task;
+#define FIFO_SIZE SZ_512K
+static DEFINE_KFIFO(fifo, unsigned char, FIFO_SIZE);
+static bool console_thread_stop;
+
+static void console_putc(struct uart_rk_port *up, unsigned int c)
+{
+	while (!(serial_in(up, UART_USR) & UART_USR_TX_FIFO_NOT_FULL))
+		cpu_relax();
+	serial_out(up, UART_TX, c);
+}
+
+static void console_flush(struct uart_rk_port *up)
+{
+	while (!(serial_in(up, UART_USR) & UART_USR_TX_FIFO_EMPTY))
+		cpu_relax();
+}
+
+static int console_thread(void *data)
+{
+	struct uart_rk_port *up = data;
+	unsigned char c;
+
+	while (1) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule();
+		if (kthread_should_stop())
+			break;
+		set_current_state(TASK_RUNNING);
+		while (!console_thread_stop && serial_in(up, UART_SFE) && kfifo_get(&fifo, &c)) {
+			console_putc(up, c);
+		}
+		if (!console_thread_stop)
+			console_flush(up);
+	}
+
+	return 0;
+}
+
+static void console_write(struct console *co, const char *s, unsigned int count)
+{
+	struct uart_rk_port *up = serial_rk_console_ports[co->index];
+	unsigned int fifo_count = FIFO_SIZE;
+	unsigned char c, r = '\r';
+
+	if (console_thread_stop ||
+	    oops_in_progress ||
+	    system_state == SYSTEM_HALT ||
+	    system_state == SYSTEM_POWER_OFF ||
+	    system_state == SYSTEM_RESTART) {
+		if (!console_thread_stop) {
+			console_thread_stop = true;
+			smp_wmb();
+			console_flush(up);
+			while (fifo_count-- && kfifo_get(&fifo, &c))
+				console_putc(up, c);
+		}
+		while (count--) {
+			if (*s == '\n') {
+				console_putc(up, r);
+			}
+			console_putc(up, *s++);
+		}
+		console_flush(up);
+	} else {
+		while (count--) {
+			if (*s == '\n') {
+				kfifo_put(&fifo, &r);
+			}
+			kfifo_put(&fifo, s++);
+		}
+		wake_up_process(console_task);
+	}
+}
+#endif
+
 static int __init serial_rk_console_setup(struct console *co, char *options)
 {
 	struct uart_rk_port *up;
@@ -1768,6 +1850,13 @@ static int __init serial_rk_console_setup(struct console *co, char *options)
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
 
+#ifdef CONFIG_RK_CONSOLE_THREAD
+	if (!console_task) {
+		console_task = kthread_create(console_thread, up, "kconsole");
+		if (!IS_ERR(console_task))
+			co->write = console_write;
+	}
+#endif
 	return uart_set_options(&up->port, co, baud, parity, bits, flow);
 }
 
@@ -2136,4 +2225,4 @@ module_init(serial_rk_init);
 module_exit(serial_rk_exit);
 
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("RK UART driver");
\ No newline at end of file
+MODULE_DESCRIPTION("RK UART driver");
-- 
2.34.1