Dynamically adjust BCM2835 AUX UART to VPU clock changes (currently untested)

0001-tty-bcm2835aux-Dynamically-adjust-to-VPU-clock-chang.patch

From 2a183ad2bdcfb773a8ef9009b49a14ede10f2c53 Mon Sep 17 00:00:00 2001
From: Stefan Wahren <stefan.wahren@i2se.com>
Date: Sat, 23 Mar 2019 12:13:08 +0100
Subject: [PATCH] tty: bcm2835aux: Dynamically adjust to VPU clock changes

The BCM2835 AUX UART clock is derived from the VPU clock, so in case
this clock changes (triggered by cpufreq) we need to reprogram the
clock dividers.

Signed-off-by: Stefan Wahren <stefan.wahren@i2se.com>
---
 drivers/tty/serial/8250/8250_bcm2835aux.c | 40 +++++++++++++++++++++++++++++--
 drivers/tty/serial/8250/8250_port.c       |  1 +
 include/linux/serial_8250.h               |  1 +
 3 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/drivers/tty/serial/8250/8250_bcm2835aux.c b/drivers/tty/serial/8250/8250_bcm2835aux.c
index bd53661..1df822f 100644
--- a/drivers/tty/serial/8250/8250_bcm2835aux.c
+++ b/drivers/tty/serial/8250/8250_bcm2835aux.c
@@ -20,7 +20,35 @@ struct bcm2835aux_data {
 	struct uart_8250_port uart;
 	struct clk *clk;
 	int line;
+	struct notifier_block clk_rate_change_nb;
 };
+#define to_priv(_nb) container_of(_nb, struct bcm2835aux_data, clk_rate_change_nb)
+
+static int bcm2835aux_clk_notifier_cb(struct notifier_block *nb,
+				      unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct bcm2835aux_data *priv = to_priv(nb);
+	struct uart_port *up;
+	unsigned long flags = 0;
+	unsigned int quot;
+
+	up = &priv->uart.port;
+	if (up->suspended)
+		return NOTIFY_OK;
+
+	if (event == POST_RATE_CHANGE) {
+		serial8250_rpm_get(&priv->uart);
+		spin_lock_irqsave(&up->lock, flags);
+		up->uartclk = ndata->new_rate * 2;
+		quot = uart_get_divisor(up, priv->uart.baud);
+		serial8250_do_set_divisor(up, priv->uart.baud, quot, 0);
+		spin_unlock_irqrestore(&up->lock, flags);
+		serial8250_rpm_put(&priv->uart);
+	}
+
+	return NOTIFY_OK;
+}
 
 static int bcm2835aux_serial_probe(struct platform_device *pdev)
 {
@@ -92,13 +120,19 @@ static int bcm2835aux_serial_probe(struct platform_device *pdev)
 	 * to get identical baudrates.
 	 */
 	data->uart.port.uartclk = clk_get_rate(data->clk) * 2;
+	data->clk_rate_change_nb.notifier_call = bcm2835aux_clk_notifier_cb;
+	ret = clk_notifier_register(data->clk, &data->clk_rate_change_nb);
+	if (ret) {
+		dev_warn(&pdev->dev, "unable to register clock notifier - %d\n",
+			 ret);
+	}
 
 	/* register the port */
 	ret = serial8250_register_8250_port(&data->uart);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "unable to register 8250 port - %d\n",
 			ret);
-		goto dis_clk;
+		goto unreg_notif;
 	}
 	data->line = ret;
 
@@ -106,7 +140,8 @@ static int bcm2835aux_serial_probe(struct platform_device *pdev)
 
 	return 0;
 
-dis_clk:
+unreg_notif:
+	clk_notifier_unregister(data->clk, &data->clk_rate_change_nb);
 	clk_disable_unprepare(data->clk);
 	return ret;
 }
@@ -116,6 +151,7 @@ static int bcm2835aux_serial_remove(struct platform_device *pdev)
 	struct bcm2835aux_data *data = platform_get_drvdata(pdev);
 
 	serial8250_unregister_port(data->uart.port.line);
+	clk_notifier_unregister(data->clk, &data->clk_rate_change_nb);
 	clk_disable_unprepare(data->clk);
 
 	return 0;
diff --git a/drivers/tty/serial/8250/8250_port.c b/drivers/tty/serial/8250/8250_port.c
index d2f3310..5a22c7b 100644
--- a/drivers/tty/serial/8250/8250_port.c
+++ b/drivers/tty/serial/8250/8250_port.c
@@ -2748,6 +2748,7 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	}
 
 	serial8250_set_divisor(port, baud, quot, frac);
+	up->baud = baud;
 
 	/*
 	 * LCR DLAB must be set to enable 64-byte FIFO mode. If the FCR
diff --git a/include/linux/serial_8250.h b/include/linux/serial_8250.h
index 5a655ba..10a39fd 100644
--- a/include/linux/serial_8250.h
+++ b/include/linux/serial_8250.h
@@ -114,6 +114,7 @@ struct uart_8250_port {
 						 *   if no_console_suspend
 						 */
 	unsigned char		probe;
+	unsigned int		baud;
 #define UART_PROBE_RSA	(1 << 0)
 
 	/*
-- 
2.7.4