1 files changed, 45 insertions, 14 deletions

diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 5a7503986d7d..b5d779cd3c2b 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -769,32 +769,63 @@ static inline struct uart_port *get_port_from_line(unsigned int line)
 }
 
 #ifdef CONFIG_SERIAL_MSM_CONSOLE
-
-static void msm_console_putchar(struct uart_port *port, int c)
-{
-        struct msm_port *msm_port = UART_TO_MSM(port);
-
-        if (msm_port->is_uartdm)
-                reset_dm_count(port, 1);
-
-        while (!(msm_read(port, UART_SR) & UART_SR_TX_READY))
-                ;
-        msm_write(port, c, msm_port->is_uartdm ? UARTDM_TF : UART_TF);
-}
-
 static void msm_console_write(struct console *co, const char *s,
                               unsigned int count)
 {
+        int i;
         struct uart_port *port;
         struct msm_port *msm_port;
+        int num_newlines = 0;
+        bool replaced = false;
 
         BUG_ON(co->index < 0 || co->index >= UART_NR);
 
         port = get_port_from_line(co->index);
         msm_port = UART_TO_MSM(port);
 
+        /* Account for newlines that will get a carriage return added */
+        for (i = 0; i < count; i++)
+                if (s[i] == '\n')
+                        num_newlines++;
+        count += num_newlines;
+
         spin_lock(&port->lock);
-        uart_console_write(port, s, count, msm_console_putchar);
+        if (msm_port->is_uartdm)
+                reset_dm_count(port, count);
+
+        i = 0;
+        while (i < count) {
+                int j;
+                unsigned int num_chars;
+                char buf[4] = { 0 };
+                unsigned int *bf = (unsigned int *)&buf;
+
+                if (msm_port->is_uartdm)
+                        num_chars = min(count - i, (unsigned int)sizeof(buf));
+                else
+                        num_chars = 1;
+
+                for (j = 0; j < num_chars; j++) {
+                        char c = *s;
+
+                        if (c == '\n' && !replaced) {
+                                buf[j] = '\r';
+                                j++;
+                                replaced = true;
+                        }
+                        if (j < num_chars) {
+                                buf[j] = c;
+                                s++;
+                                replaced = false;
+                        }
+                }
+
+                while (!(msm_read(port, UART_SR) & UART_SR_TX_READY))
+                        cpu_relax();
+
+                msm_write(port, *bf, msm_port->is_uartdm ? UARTDM_TF : UART_TF);
+                i += num_chars;
+        }
         spin_unlock(&port->lock);
 }