diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c index 3efb80f511db..4dde02f4317f 100644 --- a/drivers/tty/serial/msm_serial.c +++ b/drivers/tty/serial/msm_serial.c @@ -1197,15 +1197,21 @@ static int msm_poll_get_char_single(struct uart_port *port) return msm_read(port, rf_reg) & 0xff; } +static void msm_poll_put_char(struct uart_port *port, unsigned char c); static int msm_poll_get_char_dm(struct uart_port *port) { int c; static u32 slop; static int count; unsigned char *sp = (unsigned char *)&slop; + static const char x[] = { + '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', + 'c', 'd', 'e', 'f', + }; /* Check if a previous read had more than one char */ if (count) { + msm_poll_put_char(port, 'Q'); c = sp[sizeof(slop) - count]; count--; /* Or if FIFO is empty */ @@ -1217,10 +1223,13 @@ static int msm_poll_get_char_dm(struct uart_port *port) count = msm_read(port, UARTDM_RXFS); count = (count >> UARTDM_RXFS_BUF_SHIFT) & UARTDM_RXFS_BUF_MASK; if (count) { + msm_poll_put_char(port, 'R'); + msm_poll_put_char(port, '0' + count); msm_write(port, UART_CR_CMD_FORCE_STALE, UART_CR); slop = msm_read(port, UARTDM_RF); c = sp[0]; count--; + msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR); msm_write(port, 0xFFFFFF, UARTDM_DMRX); msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, @@ -1230,12 +1239,21 @@ static int msm_poll_get_char_dm(struct uart_port *port) } /* FIFO has a word */ } else { + msm_poll_put_char(port, 'F'); slop = msm_read(port, UARTDM_RF); c = sp[0]; count = sizeof(slop) - 1; } - return c; + if (c != NO_POLL_CHAR) { + msm_poll_put_char(port, ':'); + msm_poll_put_char(port, x[c >> 4]); + msm_poll_put_char(port, x[c & 0xf]); + msm_poll_put_char(port, '\r'); + msm_poll_put_char(port, '\n'); + } + + return NO_POLL_CHAR; } static int msm_poll_get_char(struct uart_port *port)