Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit c4b0510c authored by Soren Brinkmann's avatar Soren Brinkmann Committed by Greg Kroah-Hartman
Browse files

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: default avatarSoren Brinkmann <soren.brinkmann@xilinx.com>
Signed-off-by: default avatarGreg Kroah-Hartman <gregkh@linuxfoundation.org>
parent e6b39bfd

drivers/tty/serial/xilinx_uartps.c

+119 −4
Original line number Diff line number Diff line
@@ -163,13 +163,20 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255"); 


/**

 * struct xuartps - device data

 * @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);