From c4b0510cc1571ff44e1d6024d92683d49a8bcfde Mon Sep 17 00:00:00 2001 From: Soren Brinkmann Date: Thu, 17 Oct 2013 14:08:11 -0700 Subject: [PATCH] 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 Signed-off-by: Greg Kroah-Hartman --- drivers/tty/serial/xilinx_uartps.c | 123 ++++++++++++++++++++++++++++- 1 file changed, 119 insertions(+), 4 deletions(-) diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c index 95e12c216984..82195040e906 100644 --- 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); -- 2.34.1