diff --git a/drivers/soc/rockchip/rk_fiq_debugger.c b/drivers/soc/rockchip/rk_fiq_debugger.c
index 1986e6373c50..f47fb56afb0c 100755
--- a/drivers/soc/rockchip/rk_fiq_debugger.c
+++ b/drivers/soc/rockchip/rk_fiq_debugger.c
@@ -207,26 +207,95 @@ static void debug_flush(struct platform_device *pdev)
 
 #ifdef CONFIG_RK_CONSOLE_THREAD
 #define FIFO_SIZE SZ_64K
+#define LINE_MAX 1024
 static DEFINE_KFIFO(fifo, unsigned char, FIFO_SIZE);
+static char console_buf[LINE_MAX]; /* avoid FRAME WARN */
 static bool console_thread_stop;
+static unsigned int console_dropped_messages;
+
+static void console_putc(struct platform_device *pdev, unsigned int c)
+{
+	struct rk_fiq_debugger *t;
+	unsigned int count = 500;
+
+	t = container_of(dev_get_platdata(&pdev->dev), typeof(*t), pdata);
+
+	while (!(rk_fiq_read(t, UART_USR) & UART_USR_TX_FIFO_NOT_FULL) &&
+	       count--)
+		usleep_range(200, 210);
+	/* If uart is always busy, maybe it is abnormal, reinit it */
+	if ((count == 0) && (rk_fiq_read(t, UART_USR) & UART_USR_BUSY))
+		debug_port_init(pdev);
+
+	rk_fiq_write(t, c, UART_TX);
+}
+
+static void console_flush(struct platform_device *pdev)
+{
+	struct rk_fiq_debugger *t;
+	unsigned int count = 500;
+
+	t = container_of(dev_get_platdata(&pdev->dev), typeof(*t), pdata);
+
+	while (!(rk_fiq_read_lsr(t) & UART_LSR_TEMT) && count--)
+		usleep_range(200, 210);
+	/* If uart is always busy, maybe it is abnormal, reinit it */
+	if ((count == 0) && (rk_fiq_read(t, UART_USR) & UART_USR_BUSY))
+		debug_port_init(pdev);
+}
+
+static void console_put(struct platform_device *pdev,
+			const char *s, unsigned int count)
+{
+	while (count--) {
+		if (*s == '\n')
+			console_putc(pdev, '\r');
+		console_putc(pdev, *s++);
+	}
+}
+
+static void debug_put(struct platform_device *pdev,
+		      const char *s, unsigned int count)
+{
+	while (count--) {
+		if (*s == '\n')
+			debug_putc(pdev, '\r');
+		debug_putc(pdev, *s++);
+	}
+}
 
 static int console_thread(void *data)
 {
 	struct platform_device *pdev = data;
-	struct rk_fiq_debugger *t;
-	unsigned char c;
-	t = container_of(dev_get_platdata(&pdev->dev), typeof(*t), pdata);
+	char *buf = console_buf;
+	unsigned int len;
 
 	while (1) {
+		unsigned int dropped;
+
 		set_current_state(TASK_INTERRUPTIBLE);
-		schedule();
+		if (kfifo_is_empty(&fifo))
+			schedule();
 		if (kthread_should_stop())
 			break;
 		set_current_state(TASK_RUNNING);
-		while (!console_thread_stop && kfifo_get(&fifo, &c))
-			debug_putc(pdev, c);
+		while (!console_thread_stop) {
+			len = kfifo_out(&fifo, buf, LINE_MAX);
+			if (!len)
+				break;
+			console_put(pdev, buf, len);
+		}
+		dropped = console_dropped_messages;
+		if (dropped && !console_thread_stop) {
+			console_dropped_messages = 0;
+			smp_wmb();
+			len = snprintf(buf, LINE_MAX,
+				       "** %u console messages dropped **\n",
+				       dropped);
+			console_put(pdev, buf, len);
+		}
 		if (!console_thread_stop)
-			debug_flush(pdev);
+			console_flush(pdev);
 	}
 
 	return 0;
@@ -235,8 +304,9 @@ static int console_thread(void *data)
 static void console_write(struct platform_device *pdev, const char *s, unsigned int count)
 {
 	unsigned int fifo_count = FIFO_SIZE;
-	unsigned char c, r = '\r';
+	unsigned char c;
 	struct rk_fiq_debugger *t;
+
 	t = container_of(dev_get_platdata(&pdev->dev), typeof(*t), pdata);
 
 	if (console_thread_stop ||
@@ -249,23 +319,21 @@ static void console_write(struct platform_device *pdev, const char *s, unsigned
 			smp_wmb();
 			debug_flush(pdev);
 			while (fifo_count-- && kfifo_get(&fifo, &c))
-				debug_putc(pdev, c);
-		}
-		while (count--) {
-			if (*s == '\n') {
-				debug_putc(pdev, r);
-			}
-			debug_putc(pdev, *s++);
+				debug_put(pdev, &c, 1);
 		}
+		debug_put(pdev, s, count);
 		debug_flush(pdev);
-	} else {
-		while (count--) {
-			if (*s == '\n') {
-				kfifo_put(&fifo, r);
-			}
-			kfifo_put(&fifo, *s++);
+	} else if (count) {
+		unsigned int ret = 0;
+
+		if (kfifo_len(&fifo) + count < FIFO_SIZE)
+			ret = kfifo_in(&fifo, s, count);
+		if (!ret) {
+			console_dropped_messages++;
+			smp_wmb();
+		} else {
+			wake_up_process(t->console_task);
 		}
-		wake_up_process(t->console_task);
 	}
 }
 #endif