1 files changed, 52 insertions, 0 deletions

diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index 5d5557af4d22..62259f37ddda 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -775,6 +775,54 @@ static void xuartps_enable_ms(struct uart_port *port)
         /* N/A */
 }
 
+#ifdef CONFIG_CONSOLE_POLL
+static int xuartps_poll_get_char(struct uart_port *port)
+{
+        u32 imr;
+        int c;
+
+        /* Disable all interrupts */
+        imr = xuartps_readl(XUARTPS_IMR_OFFSET);
+        xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+
+        /* Check if FIFO is empty */
+        if (xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_RXEMPTY)
+                c = NO_POLL_CHAR;
+        else /* Read a character */
+                c = (unsigned char) xuartps_readl(XUARTPS_FIFO_OFFSET);
+
+        /* Enable interrupts */
+        xuartps_writel(imr, XUARTPS_IER_OFFSET);
+
+        return c;
+}
+
+static void xuartps_poll_put_char(struct uart_port *port, unsigned char c)
+{
+        u32 imr;
+
+        /* Disable all interrupts */
+        imr = xuartps_readl(XUARTPS_IMR_OFFSET);
+        xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+
+        /* Wait until FIFO is empty */
+        while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+                cpu_relax();
+
+        /* Write a character */
+        xuartps_writel(c, XUARTPS_FIFO_OFFSET);
+
+        /* Wait until FIFO is empty */
+        while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+                cpu_relax();
+
+        /* Enable interrupts */
+        xuartps_writel(imr, XUARTPS_IER_OFFSET);
+
+        return;
+}
+#endif
+
 /** The UART operations structure
  */
 static struct uart_ops xuartps_ops = {
@@ -807,6 +855,10 @@ static struct uart_ops xuartps_ops = {
         .config_port    = xuartps_config_port,  /* Configure when driver
                                                  * adds a xuartps port
                                                  */
+#ifdef CONFIG_CONSOLE_POLL
+        .poll_get_char  = xuartps_poll_get_char,
+        .poll_put_char  = xuartps_poll_put_char,
+#endif
 };
 
 static struct uart_port xuartps_port[2];