rk_serial: support console write by thread

Signed-off-by: Huang, Tao <huangtao@rock-chips.com>

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