tty: xuartps: Dynamically adjust to input frequency changes

Add a clock notifier to dynamically handle frequency changes of the
input clock by reprogramming the UART in order to keep the baud rate
constant.

Signed-off-by: Soren Brinkmann <soren.brinkmann@xilinx.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index 95e12c2..8219504 100644 (file)
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -163,13 +163,20 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
 
 /**
  * struct xuartps - device data
- * @refclk     Reference clock
- * @aperclk    APB clock
+ * @port               Pointer to the UART port
+ * @refclk             Reference clock
+ * @aperclk            APB clock
+ * @baud               Current baud rate
+ * @clk_rate_change_nb Notifier block for clock changes
  */
 struct xuartps {
+       struct uart_port        *port;
        struct clk              *refclk;
        struct clk              *aperclk;
+       unsigned int            baud;
+       struct notifier_block   clk_rate_change_nb;
 };
+#define to_xuartps(_nb) container_of(_nb, struct xuartps, clk_rate_change_nb);
 
 /**
  * xuartps_isr - Interrupt handler
@@ -385,6 +392,7 @@ static unsigned int xuartps_set_baud_rate(struct uart_port *port,
        u32 cd, bdiv;
        u32 mreg;
        int div8;
+       struct xuartps *xuartps = port->private_data;
 
        calc_baud = xuartps_calc_baud_divs(port->uartclk, baud, &bdiv, &cd,
                        &div8);
@@ -398,10 +406,105 @@ static unsigned int xuartps_set_baud_rate(struct uart_port *port,
        xuartps_writel(mreg, XUARTPS_MR_OFFSET);
        xuartps_writel(cd, XUARTPS_BAUDGEN_OFFSET);
        xuartps_writel(bdiv, XUARTPS_BAUDDIV_OFFSET);
+       xuartps->baud = baud;
 
        return calc_baud;
 }
 
+/**
+ * xuartps_clk_notitifer_cb - Clock notifier callback
+ * @nb:                Notifier block
+ * @event:     Notify event
+ * @data:      Notifier data
+ * Returns NOTIFY_OK on success, NOTIFY_BAD on error.
+ */
+static int xuartps_clk_notifier_cb(struct notifier_block *nb,
+               unsigned long event, void *data)
+{
+       u32 ctrl_reg;
+       struct uart_port *port;
+       int locked = 0;
+       struct clk_notifier_data *ndata = data;
+       unsigned long flags = 0;
+       struct xuartps *xuartps = to_xuartps(nb);
+
+       port = xuartps->port;
+       if (port->suspended)
+               return NOTIFY_OK;
+
+       switch (event) {
+       case PRE_RATE_CHANGE:
+       {
+               u32 bdiv;
+               u32 cd;
+               int div8;
+
+               /*
+                * Find out if current baud-rate can be achieved with new clock
+                * frequency.
+                */
+               if (!xuartps_calc_baud_divs(ndata->new_rate, xuartps->baud,
+                                       &bdiv, &cd, &div8))
+                       return NOTIFY_BAD;
+
+               spin_lock_irqsave(&xuartps->port->lock, flags);
+
+               /* Disable the TX and RX to set baud rate */
+               xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
+                               (XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS),
+                               XUARTPS_CR_OFFSET);
+
+               spin_unlock_irqrestore(&xuartps->port->lock, flags);
+
+               return NOTIFY_OK;
+       }
+       case POST_RATE_CHANGE:
+               /*
+                * Set clk dividers to generate correct baud with new clock
+                * frequency.
+                */
+
+               spin_lock_irqsave(&xuartps->port->lock, flags);
+
+               locked = 1;
+               port->uartclk = ndata->new_rate;
+
+               xuartps->baud = xuartps_set_baud_rate(xuartps->port,
+                               xuartps->baud);
+               /* fall through */
+       case ABORT_RATE_CHANGE:
+               if (!locked)
+                       spin_lock_irqsave(&xuartps->port->lock, flags);
+
+               /* Set TX/RX Reset */
+               xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
+                               (XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
+                               XUARTPS_CR_OFFSET);
+
+               while (xuartps_readl(XUARTPS_CR_OFFSET) &
+                               (XUARTPS_CR_TXRST | XUARTPS_CR_RXRST))
+                       cpu_relax();
+
+               /*
+                * Clear the RX disable and TX disable bits and then set the TX
+                * enable bit and RX enable bit to enable the transmitter and
+                * receiver.
+                */
+               xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+               ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
+               xuartps_writel(
+                       (ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS)) |
+                       (XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
+                       XUARTPS_CR_OFFSET);
+
+               spin_unlock_irqrestore(&xuartps->port->lock, flags);
+
+               return NOTIFY_OK;
+       default:
+               return NOTIFY_DONE;
+       }
+}
+
 /*----------------------Uart Operations---------------------------*/
 
 /**
@@ -1164,13 +1267,19 @@ static int xuartps_probe(struct platform_device *pdev)
                goto err_out_clk_disable;
        }
 
+       xuartps_data->clk_rate_change_nb.notifier_call =
+                       xuartps_clk_notifier_cb;
+       if (clk_notifier_register(xuartps_data->refclk,
+                               &xuartps_data->clk_rate_change_nb))
+               dev_warn(&pdev->dev, "Unable to register clock notifier.\n");
+
        /* Initialize the port structure */
        port = xuartps_get_port();
 
        if (!port) {
                dev_err(&pdev->dev, "Cannot get uart_port structure\n");
                rc = -ENODEV;
-               goto err_out_clk_disable;
+               goto err_out_notif_unreg;
        } else {
                /* Register the port.
                 * This function also registers this device with the tty layer
@@ -1181,16 +1290,20 @@ static int xuartps_probe(struct platform_device *pdev)
                port->dev = &pdev->dev;
                port->uartclk = clk_get_rate(xuartps_data->refclk);
                port->private_data = xuartps_data;
+               xuartps_data->port = port;
                platform_set_drvdata(pdev, port);
                rc = uart_add_one_port(&xuartps_uart_driver, port);
                if (rc) {
                        dev_err(&pdev->dev,
                                "uart_add_one_port() failed; err=%i\n", rc);
-                       goto err_out_clk_disable;
+                       goto err_out_notif_unreg;
                }
                return 0;
        }
 
+err_out_notif_unreg:
+       clk_notifier_unregister(xuartps_data->refclk,
+                       &xuartps_data->clk_rate_change_nb);
 err_out_clk_disable:
        clk_disable_unprepare(xuartps_data->refclk);
 err_out_clk_dis_aper:
@@ -1212,6 +1325,8 @@ static int xuartps_remove(struct platform_device *pdev)
        int rc;
 
        /* Remove the xuartps port from the serial core */
+       clk_notifier_unregister(xuartps_data->refclk,
+                       &xuartps_data->clk_rate_change_nb);
        rc = uart_remove_one_port(&xuartps_uart_driver, port);
        port->mapbase = 0;
        clk_disable_unprepare(xuartps_data->refclk);