[PATCH v7 0/6] serial: mvebu-uart: Support for higher baudrates

@@ -300,6 +300,19 @@ u64 mul_u64_u64_div_u64(u64 a, u64 mul, u64 div);
 #define DIV64_U64_ROUND_CLOSEST(dividend, divisor)	\
 	({ u64 _tmp = (divisor); div64_u64((dividend) + _tmp / 2, _tmp); })
 
+/*
+ * DIV_U64_ROUND_CLOSEST - unsigned 64bit divide with 32bit divisor rounded to nearest integer
+ * @dividend: unsigned 64bit dividend
+ * @divisor: unsigned 32bit divisor
+ *
+ * Divide unsigned 64bit dividend by unsigned 32bit divisor
+ * and round to closest integer.
+ *
+ * Return: dividend / divisor rounded to nearest integer
+ */
+#define DIV_U64_ROUND_CLOSEST(dividend, divisor)	\
+	({ u32 _tmp = (divisor); div_u64((u64)(dividend) + _tmp / 2, _tmp); })
+
 /*
  * DIV_S64_ROUND_CLOSEST - signed 64bit divide with 32bit divisor rounded to nearest integer
  * @dividend: signed 64bit dividend

@@ -0,0 +1,59 @@
+# SPDX-License-Identifier: GPL-2.0-only OR BSD-2-Clause
+%YAML 1.2
+---
+$id: http://devicetree.org/schemas/clock/marvell,armada-3700-uart-clock.yaml#
+$schema: http://devicetree.org/meta-schemas/core.yaml#
+title: Marvell Armada 3720 UART clocks
+
+maintainers:
+  - Pali Rohár <pali@kernel.org>
+
+properties:
+  compatible:
+    const: marvell,armada-3700-uart-clock
+
+  reg:
+    items:
+      - description: UART Clock Control Register
+      - description: UART 2 Baud Rate Divisor Register
+
+  clocks:
+    description: |
+      List of parent clocks suitable for UART from following set:
+        "TBG-A-P", "TBG-B-P", "TBG-A-S", "TBG-B-S", "xtal"
+      UART clock can use one from this set and when more are provided
+      then kernel would choose and configure the most suitable one.
+      It is suggest to specify at least one TBG clock to achieve
+      baudrates above 230400 and also to specify clock which bootloader
+      used for UART (most probably xtal) for smooth boot log on UART.
+
+  clock-names:
+    items:
+      - const: TBG-A-P
+      - const: TBG-B-P
+      - const: TBG-A-S
+      - const: TBG-B-S
+      - const: xtal
+    minItems: 1
+
+  '#clock-cells':
+    const: 1
+
+required:
+  - compatible
+  - reg
+  - clocks
+  - clock-names
+  - '#clock-cells'
+
+additionalProperties: false
+
+examples:
+  - |
+    uartclk: clock-controller@12010 {
+      compatible = "marvell,armada-3700-uart-clock";
+      reg = <0x12010 0x4>, <0x12210 0x4>;
+      clocks = <&tbg 0>, <&tbg 1>, <&tbg 2>, <&tbg 3>, <&xtalclk>;
+      clock-names = "TBG-A-P", "TBG-B-P", "TBG-A-S", "TBG-B-S", "xtal";
+      #clock-cells = <1>;
+    };

@@ -1444,6 +1444,7 @@ config SERIAL_STM32_CONSOLE
 config SERIAL_MVEBU_UART
 	bool "Marvell EBU serial port support"
 	depends on ARCH_MVEBU || COMPILE_TEST
+	depends on COMMON_CLK
 	select SERIAL_CORE
 	help
 	  This driver is for Marvell EBU SoC's UART. If you have a machine

@@ -8,12 +8,14 @@
 */
 
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
 #include <linux/console.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
+#include <linux/math64.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>

@@ -68,8 +70,31 @@
 #define  STAT_BRK_ERR		(STAT_BRK_DET | STAT_FRM_ERR \
 				 | STAT_PAR_ERR | STAT_OVR_ERR)
 
+/*
+ * Marvell Armada 3700 Functional Specifications describes that bit 21 of UART
+ * Clock Control register controls UART1 and bit 20 controls UART2. But in
+ * reality bit 21 controls UART2 and bit 20 controls UART1. This seems to be a
+ * bug in Marvell documentation. Hence following CLK_DIS macros are swapped.
+ */
+
 #define UART_BRDV		0x10
+/* These bits are located in UART1 address space and control UART2 */
+#define  UART2_CLK_DIS		BIT(21)
+/* These bits are located in UART1 address space and control UART1 */
+#define  UART1_CLK_DIS		BIT(20)
+/* These bits are located in UART1 address space and control both UARTs */
+#define  CLK_NO_XTAL		BIT(19)
+#define  CLK_TBG_DIV1_SHIFT	15
+#define  CLK_TBG_DIV1_MASK	0x7
+#define  CLK_TBG_DIV1_MAX	6
+#define  CLK_TBG_DIV2_SHIFT	12
+#define  CLK_TBG_DIV2_MASK	0x7
+#define  CLK_TBG_DIV2_MAX	6
+#define  CLK_TBG_SEL_SHIFT	10
+#define  CLK_TBG_SEL_MASK	0x3
+/* These bits are located in both UARTs address space */
 #define  BRDV_BAUD_MASK         0x3FF
+#define  BRDV_BAUD_MAX		BRDV_BAUD_MASK
 
 #define UART_OSAMP		0x14
 #define  OSAMP_DEFAULT_DIVISOR	16

@@ -153,6 +178,8 @@ static struct mvebu_uart *to_mvuart(struct uart_port *port)
 
 static struct uart_port mvebu_uart_ports[MVEBU_NR_UARTS];
 
+static DEFINE_SPINLOCK(mvebu_uart_lock);
+
 /* Core UART Driver Operations */
 static unsigned int mvebu_uart_tx_empty(struct uart_port *port)
 {

@@ -445,6 +472,7 @@ static void mvebu_uart_shutdown(struct uart_port *port)
 static int mvebu_uart_baud_rate_set(struct uart_port *port, unsigned int baud)
 {
 	unsigned int d_divisor, m_divisor;
+	unsigned long flags;
 	u32 brdv, osamp;
 
 	if (!port->uartclk)

@@ -463,10 +491,12 @@ static int mvebu_uart_baud_rate_set(struct uart_port *port, unsigned int baud)
 	m_divisor = OSAMP_DEFAULT_DIVISOR;
 	d_divisor = DIV_ROUND_CLOSEST(port->uartclk, baud * m_divisor);
 
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
 	brdv = readl(port->membase + UART_BRDV);
 	brdv &= ~BRDV_BAUD_MASK;
 	brdv |= d_divisor;
 	writel(brdv, port->membase + UART_BRDV);
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
 
 	osamp = readl(port->membase + UART_OSAMP);
 	osamp &= ~OSAMP_DIVISORS_MASK;

@@ -762,6 +792,7 @@ static int mvebu_uart_suspend(struct device *dev)
 {
 	struct mvebu_uart *mvuart = dev_get_drvdata(dev);
 	struct uart_port *port = mvuart->port;
+	unsigned long flags;
 
 	uart_suspend_port(&mvebu_uart_driver, port);
 

@@ -770,7 +801,9 @@ static int mvebu_uart_suspend(struct device *dev)
 	mvuart->pm_regs.ctrl = readl(port->membase + UART_CTRL(port));
 	mvuart->pm_regs.intr = readl(port->membase + UART_INTR(port));
 	mvuart->pm_regs.stat = readl(port->membase + UART_STAT);
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
 	mvuart->pm_regs.brdv = readl(port->membase + UART_BRDV);
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
 	mvuart->pm_regs.osamp = readl(port->membase + UART_OSAMP);
 
 	device_set_wakeup_enable(dev, true);

@@ -782,13 +815,16 @@ static int mvebu_uart_resume(struct device *dev)
 {
 	struct mvebu_uart *mvuart = dev_get_drvdata(dev);
 	struct uart_port *port = mvuart->port;
+	unsigned long flags;
 
 	writel(mvuart->pm_regs.rbr, port->membase + UART_RBR(port));
 	writel(mvuart->pm_regs.tsh, port->membase + UART_TSH(port));
 	writel(mvuart->pm_regs.ctrl, port->membase + UART_CTRL(port));
 	writel(mvuart->pm_regs.intr, port->membase + UART_INTR(port));
 	writel(mvuart->pm_regs.stat, port->membase + UART_STAT);
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
 	writel(mvuart->pm_regs.brdv, port->membase + UART_BRDV);
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
 	writel(mvuart->pm_regs.osamp, port->membase + UART_OSAMP);
 
 	uart_resume_port(&mvebu_uart_driver, port);

@@ -972,6 +1008,476 @@ static struct platform_driver mvebu_uart_platform_driver = {
 	},
 };
 
+/* This code is based on clk-fixed-factor.c driver and modified. */
+
+struct mvebu_uart_clock {
+	struct clk_hw clk_hw;
+	int clock_idx;
+	u32 pm_context_reg1;
+	u32 pm_context_reg2;
+};
+
+struct mvebu_uart_clock_base {
+	struct mvebu_uart_clock clocks[2];
+	unsigned int parent_rates[5];
+	int parent_idx;
+	unsigned int div;
+	void __iomem *reg1;
+	void __iomem *reg2;
+	bool configured;
+};
+
+#define PARENT_CLOCK_XTAL 4
+
+#define to_uart_clock(hw) container_of(hw, struct mvebu_uart_clock, clk_hw)
+#define to_uart_clock_base(uart_clock) container_of(uart_clock, \
+	struct mvebu_uart_clock_base, clocks[uart_clock->clock_idx])
+
+static int mvebu_uart_clock_prepare(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	unsigned int prev_clock_idx, prev_clock_rate, prev_d1d2;
+	unsigned int parent_clock_idx, parent_clock_rate;
+	unsigned long flags;
+	unsigned int d1, d2;
+	u64 divisor;
+	u32 val;
+
+	/*
+	 * This function just reconfigures UART Clock Control register (located
+	 * in UART1 address space which controls both UART1 and UART2) to
+	 * selected UART base clock and recalculate current UART1/UART2 divisors
+	 * in their address spaces, so final baudrate will not be changed by
+	 * switching UART base clock. This is required otherwise kernel boot log
+	 * stops working. It is needed to ensure that UART baudrate does not
+	 * change during this setup. It is one time operation, so based on
+	 * "configured" member this function is skipped on second call. Because
+	 * this UART Clock Control register (UART_BRDV) is shared between UART1
+	 * baudrate function, UART1 clock selector and UART2 clock selector,
+	 * every access to UART_BRDV (reg1) needs to be protected by lock.
+	 */
+
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
+
+	if (uart_clock_base->configured) {
+		spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+		return 0;
+	}
+
+	parent_clock_idx = uart_clock_base->parent_idx;
+	parent_clock_rate = uart_clock_base->parent_rates[parent_clock_idx];
+
+	val = readl(uart_clock_base->reg1);
+
+	if (uart_clock_base->div > CLK_TBG_DIV1_MAX) {
+		d1 = CLK_TBG_DIV1_MAX;
+		d2 = uart_clock_base->div / CLK_TBG_DIV1_MAX;
+	} else {
+		d1 = uart_clock_base->div;
+		d2 = 1;
+	}
+
+	if (val & CLK_NO_XTAL) {
+		prev_clock_idx = (val >> CLK_TBG_SEL_SHIFT) & CLK_TBG_SEL_MASK;
+		prev_d1d2 = ((val >> CLK_TBG_DIV1_SHIFT) & CLK_TBG_DIV1_MASK)
+			  * ((val >> CLK_TBG_DIV2_SHIFT) & CLK_TBG_DIV2_MASK);
+	} else {
+		prev_clock_idx = PARENT_CLOCK_XTAL;
+		prev_d1d2 = 1;
+	}
+
+	/* Note that uart_clock_base->parent_rates[i] may not be available */
+	prev_clock_rate = uart_clock_base->parent_rates[prev_clock_idx];
+
+	/* Recalculate UART1 divisor so UART1 baudrate does not change */
+	if (prev_clock_rate) {
+		divisor = DIV_U64_ROUND_CLOSEST((u64)(val & BRDV_BAUD_MASK) *
+						parent_clock_rate * prev_d1d2,
+						prev_clock_rate * d1 * d2);
+		if (divisor < 1)
+			divisor = 1;
+		else if (divisor > BRDV_BAUD_MAX)
+			divisor = BRDV_BAUD_MAX;
+		val = (val & ~BRDV_BAUD_MASK) | divisor;
+	}
+
+	if (parent_clock_idx != PARENT_CLOCK_XTAL) {
+		/* Do not use XTAL, select TBG clock and TBG d1 * d2 divisors */
+		val |= CLK_NO_XTAL;
+		val &= ~(CLK_TBG_DIV1_MASK << CLK_TBG_DIV1_SHIFT);
+		val |= d1 << CLK_TBG_DIV1_SHIFT;
+		val &= ~(CLK_TBG_DIV2_MASK << CLK_TBG_DIV2_SHIFT);
+		val |= d2 << CLK_TBG_DIV2_SHIFT;
+		val &= ~(CLK_TBG_SEL_MASK << CLK_TBG_SEL_SHIFT);
+		val |= parent_clock_idx << CLK_TBG_SEL_SHIFT;
+	} else {
+		/* Use XTAL, TBG bits are then ignored */
+		val &= ~CLK_NO_XTAL;
+	}
+
+	writel(val, uart_clock_base->reg1);
+
+	/* Recalculate UART2 divisor so UART2 baudrate does not change */
+	if (prev_clock_rate) {
+		val = readl(uart_clock_base->reg2);
+		divisor = DIV_U64_ROUND_CLOSEST((u64)(val & BRDV_BAUD_MASK) *
+						parent_clock_rate * prev_d1d2,
+						prev_clock_rate * d1 * d2);
+		if (divisor < 1)
+			divisor = 1;
+		else if (divisor > BRDV_BAUD_MAX)
+			divisor = BRDV_BAUD_MAX;
+		val = (val & ~BRDV_BAUD_MASK) | divisor;
+		writel(val, uart_clock_base->reg2);
+	}
+
+	uart_clock_base->configured = true;
+
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+
+	return 0;
+}
+
+static int mvebu_uart_clock_enable(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
+
+	val = readl(uart_clock_base->reg1);
+
+	if (uart_clock->clock_idx == 0)
+		val &= ~UART1_CLK_DIS;
+	else
+		val &= ~UART2_CLK_DIS;
+
+	writel(val, uart_clock_base->reg1);
+
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+
+	return 0;
+}
+
+static void mvebu_uart_clock_disable(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
+
+	val = readl(uart_clock_base->reg1);
+
+	if (uart_clock->clock_idx == 0)
+		val |= UART1_CLK_DIS;
+	else
+		val |= UART2_CLK_DIS;
+
+	writel(val, uart_clock_base->reg1);
+
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+}
+
+static int mvebu_uart_clock_is_enabled(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	u32 val;
+
+	val = readl(uart_clock_base->reg1);
+
+	if (uart_clock->clock_idx == 0)
+		return !(val & UART1_CLK_DIS);
+	else
+		return !(val & UART2_CLK_DIS);
+}
+
+static int mvebu_uart_clock_save_context(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
+	uart_clock->pm_context_reg1 = readl(uart_clock_base->reg1);
+	uart_clock->pm_context_reg2 = readl(uart_clock_base->reg2);
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+
+	return 0;
+}
+
+static void mvebu_uart_clock_restore_context(struct clk_hw *hw)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mvebu_uart_lock, flags);
+	writel(uart_clock->pm_context_reg1, uart_clock_base->reg1);
+	writel(uart_clock->pm_context_reg2, uart_clock_base->reg2);
+	spin_unlock_irqrestore(&mvebu_uart_lock, flags);
+}
+
+static unsigned long mvebu_uart_clock_recalc_rate(struct clk_hw *hw,
+						  unsigned long parent_rate)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+
+	return parent_rate / uart_clock_base->div;
+}
+
+static long mvebu_uart_clock_round_rate(struct clk_hw *hw, unsigned long rate,
+					unsigned long *parent_rate)
+{
+	struct mvebu_uart_clock *uart_clock = to_uart_clock(hw);
+	struct mvebu_uart_clock_base *uart_clock_base =
+						to_uart_clock_base(uart_clock);
+
+	return *parent_rate / uart_clock_base->div;
+}
+
+static int mvebu_uart_clock_set_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long parent_rate)
+{
+	/*
+	 * We must report success but we can do so unconditionally because
+	 * mvebu_uart_clock_round_rate returns values that ensure this call is a
+	 * nop.
+	 */
+
+	return 0;
+}
+
+static const struct clk_ops mvebu_uart_clock_ops = {
+	.prepare = mvebu_uart_clock_prepare,
+	.enable = mvebu_uart_clock_enable,
+	.disable = mvebu_uart_clock_disable,
+	.is_enabled = mvebu_uart_clock_is_enabled,
+	.save_context = mvebu_uart_clock_save_context,
+	.restore_context = mvebu_uart_clock_restore_context,
+	.round_rate = mvebu_uart_clock_round_rate,
+	.set_rate = mvebu_uart_clock_set_rate,
+	.recalc_rate = mvebu_uart_clock_recalc_rate,
+};
+
+static int mvebu_uart_clock_register(struct device *dev,
+				     struct mvebu_uart_clock *uart_clock,
+				     const char *name,
+				     const char *parent_name)
+{
+	struct clk_init_data init = { };
+
+	uart_clock->clk_hw.init = &init;
+
+	init.name = name;
+	init.ops = &mvebu_uart_clock_ops;
+	init.flags = 0;
+	init.num_parents = 1;
+	init.parent_names = &parent_name;
+
+	return devm_clk_hw_register(dev, &uart_clock->clk_hw);
+}
+
+static int mvebu_uart_clock_probe(struct platform_device *pdev)
+{
+	static const char *const uart_clk_names[] = { "uart_1", "uart_2" };
+	static const char *const parent_clk_names[] = { "TBG-A-P", "TBG-B-P",
+							"TBG-A-S", "TBG-B-S",
+							"xtal" };
+	struct clk *parent_clks[ARRAY_SIZE(parent_clk_names)];
+	struct mvebu_uart_clock_base *uart_clock_base;
+	struct clk_hw_onecell_data *hw_clk_data;
+	struct device *dev = &pdev->dev;
+	int i, parent_clk_idx, ret;
+	unsigned long div, rate;
+	struct resource *res;
+	unsigned int d1, d2;
+
+	BUILD_BUG_ON(ARRAY_SIZE(uart_clk_names) !=
+		     ARRAY_SIZE(uart_clock_base->clocks));
+	BUILD_BUG_ON(ARRAY_SIZE(parent_clk_names) !=
+		     ARRAY_SIZE(uart_clock_base->parent_rates));
+
+	uart_clock_base = devm_kzalloc(dev,
+				       sizeof(*uart_clock_base),
+				       GFP_KERNEL);
+	if (!uart_clock_base)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "Couldn't get first register\n");
+		return -ENOENT;
+	}
+
+	/*
+	 * UART Clock Control register (reg1 / UART_BRDV) is in address range
+	 * of UART1 (standard UART variant), controls clock source and dividers
+	 * for both UART1 and UART2 and is supplied via DT as first resource.
+	 * Therefore use ioremap() function rather than ioremap_resource() to
+	 * avoid conflicts with UART1 driver. Access to UART_BRDV is protected
+	 * by lock shared between clock and UART driver.
+	 */
+	uart_clock_base->reg1 = devm_ioremap(dev, res->start,
+					     resource_size(res));
+	if (IS_ERR(uart_clock_base->reg1))
+		return PTR_ERR(uart_clock_base->reg1);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res) {
+		dev_err(dev, "Couldn't get second register\n");
+		return -ENOENT;
+	}
+
+	/*
+	 * UART 2 Baud Rate Divisor register (reg2 / UART_BRDV) is in address
+	 * range of UART2 (extended UART variant), controls only one UART2
+	 * specific divider and is supplied via DT as second resource.
+	 * Therefore use ioremap() function rather than ioremap_resource() to
+	 * avoid conflicts with UART2 driver. Access to UART_BRDV is protected
+	 * by lock shared between clock and UART driver.
+	 */
+	uart_clock_base->reg2 = devm_ioremap(dev, res->start,
+					     resource_size(res));
+	if (IS_ERR(uart_clock_base->reg2))
+		return PTR_ERR(uart_clock_base->reg2);
+
+	hw_clk_data = devm_kzalloc(dev,
+				   struct_size(hw_clk_data, hws,
+					       ARRAY_SIZE(uart_clk_names)),
+				   GFP_KERNEL);
+	if (!hw_clk_data)
+		return -ENOMEM;
+
+	hw_clk_data->num = ARRAY_SIZE(uart_clk_names);
+	for (i = 0; i < ARRAY_SIZE(uart_clk_names); i++) {
+		hw_clk_data->hws[i] = &uart_clock_base->clocks[i].clk_hw;
+		uart_clock_base->clocks[i].clock_idx = i;
+	}
+
+	parent_clk_idx = -1;
+
+	for (i = 0; i < ARRAY_SIZE(parent_clk_names); i++) {
+		parent_clks[i] = devm_clk_get(dev, parent_clk_names[i]);
+		if (IS_ERR(parent_clks[i])) {
+			if (PTR_ERR(parent_clks[i]) == -EPROBE_DEFER)
+				return -EPROBE_DEFER;
+			dev_warn(dev, "Couldn't get the parent clock %s: %ld\n",
+				parent_clk_names[i], PTR_ERR(parent_clks[i]));
+			continue;
+		}
+
+		ret = clk_prepare_enable(parent_clks[i]);
+		if (ret) {
+			dev_warn(dev, "Couldn't enable parent clock %s: %d\n",
+				parent_clk_names[i], ret);
+			continue;
+		}
+		rate = clk_get_rate(parent_clks[i]);
+		uart_clock_base->parent_rates[i] = rate;
+
+		if (i != PARENT_CLOCK_XTAL) {
+			/*
+			 * Calculate the smallest TBG d1 and d2 divisors that
+			 * still can provide 9600 baudrate.
+			 */
+			d1 = DIV_ROUND_UP(rate, 9600 * OSAMP_DEFAULT_DIVISOR *
+						BRDV_BAUD_MAX);
+			if (d1 < 1)
+				d1 = 1;
+			else if (d1 > CLK_TBG_DIV1_MAX)
+				d1 = CLK_TBG_DIV1_MAX;
+
+			d2 = DIV_ROUND_UP(rate, 9600 * OSAMP_DEFAULT_DIVISOR *
+						BRDV_BAUD_MAX * d1);
+			if (d2 < 1)
+				d2 = 1;
+			else if (d2 > CLK_TBG_DIV2_MAX)
+				d2 = CLK_TBG_DIV2_MAX;
+		} else {
+			/*
+			 * When UART clock uses XTAL clock as a source then it
+			 * is not possible to use d1 and d2 divisors.
+			 */
+			d1 = d2 = 1;
+		}
+
+		/* Skip clock source which cannot provide 9600 baudrate */
+		if (rate > 9600 * OSAMP_DEFAULT_DIVISOR * BRDV_BAUD_MAX * d1 * d2)
+			continue;
+
+		/*
+		 * Choose TBG clock source with the smallest divisors. Use XTAL
+		 * clock source only in case TBG is not available as XTAL cannot
+		 * be used for baudrates higher than 230400.
+		 */
+		if (parent_clk_idx == -1 ||
+		    (i != PARENT_CLOCK_XTAL && div > d1 * d2)) {
+			parent_clk_idx = i;
+			div = d1 * d2;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(parent_clk_names); i++) {
+		if (i == parent_clk_idx || IS_ERR(parent_clks[i]))
+			continue;
+		clk_disable_unprepare(parent_clks[i]);
+		devm_clk_put(dev, parent_clks[i]);
+	}
+
+	if (parent_clk_idx == -1) {
+		dev_err(dev, "No usable parent clock\n");
+		return -ENOENT;
+	}
+
+	uart_clock_base->parent_idx = parent_clk_idx;
+	uart_clock_base->div = div;
+
+	dev_notice(dev, "Using parent clock %s as base UART clock\n",
+		   __clk_get_name(parent_clks[parent_clk_idx]));
+
+	for (i = 0; i < ARRAY_SIZE(uart_clk_names); i++) {
+		ret = mvebu_uart_clock_register(dev,
+				&uart_clock_base->clocks[i],
+				uart_clk_names[i],
+				__clk_get_name(parent_clks[parent_clk_idx]));
+		if (ret) {
+			dev_err(dev, "Can't register UART clock %d: %d\n",
+				i, ret);
+			return ret;
+		}
+	}
+
+	return devm_of_clk_add_hw_provider(dev, of_clk_hw_onecell_get,
+					   hw_clk_data);
+}
+
+static const struct of_device_id mvebu_uart_clock_of_match[] = {
+	{ .compatible = "marvell,armada-3700-uart-clock", },
+	{ }
+};
+
+static struct platform_driver mvebu_uart_clock_platform_driver = {
+	.probe = mvebu_uart_clock_probe,
+	.driver		= {
+		.name	= "mvebu-uart-clock",
+		.of_match_table = mvebu_uart_clock_of_match,
+	},
+};
+
 static int __init mvebu_uart_init(void)
 {
 	int ret;

@@ -980,10 +1486,19 @@ static int __init mvebu_uart_init(void)
 	if (ret)
 		return ret;
 
+	ret = platform_driver_register(&mvebu_uart_clock_platform_driver);
+	if (ret) {
+		uart_unregister_driver(&mvebu_uart_driver);
+		return ret;
+	}
+
 	ret = platform_driver_register(&mvebu_uart_platform_driver);
-	if (ret)
+	if (ret) {
+		platform_driver_unregister(&mvebu_uart_clock_platform_driver);
 		uart_unregister_driver(&mvebu_uart_driver);
+		return ret;
+	}
 
-	return ret;
+	return 0;
 }
 arch_initcall(mvebu_uart_init);

@@ -99,6 +99,7 @@
 #define UART_OSAMP		0x14
 #define  OSAMP_DEFAULT_DIVISOR	16
 #define  OSAMP_DIVISORS_MASK	0x3F3F3F3F
+#define  OSAMP_MAX_DIVISOR	63
 
 #define MVEBU_NR_UARTS		2
 

@@ -479,18 +480,59 @@ static int mvebu_uart_baud_rate_set(struct uart_port *port, unsigned int baud)
 		return -EOPNOTSUPP;
 
 	/*
-	 * The baudrate is derived from the UART clock thanks to two divisors:
-	 *   > D ("baud generator"): can divide the clock from 2 to 2^10 - 1.
-	 *   > M ("fractional divisor"): allows a better accuracy for
-	 *     baudrates higher than 230400.
+	 * The baudrate is derived from the UART clock thanks to divisors:
+	 *   > d1 * d2 ("TBG divisors"): can divide only TBG clock from 1 to 6
+	 *   > D ("baud generator"): can divide the clock from 1 to 1023
+	 *   > M ("fractional divisor"): allows a better accuracy (from 1 to 63)
 	 *
-	 * As the derivation of M is rather complicated, the code sticks to its
-	 * default value (x16) when all the prescalers are zeroed, and only
-	 * makes use of D to configure the desired baudrate.
+	 * Exact formulas for calculating baudrate:
+	 *
+	 * with default x16 scheme:
+	 *   baudrate = xtal / (d * 16)
+	 *   baudrate = tbg / (d1 * d2 * d * 16)
+	 *
+	 * with fractional divisor:
+	 *   baudrate = 10 * xtal / (d * (3 * (m1 + m2) + 2 * (m3 + m4)))
+	 *   baudrate = 10 * tbg / (d1*d2 * d * (3 * (m1 + m2) + 2 * (m3 + m4)))
+	 *
+	 * Oversampling value:
+	 *   osamp = (m1 << 0) | (m2 << 8) | (m3 << 16) | (m4 << 24);
+	 *
+	 * Where m1 controls number of clock cycles per bit for bits 1,2,3;
+	 * m2 for bits 4,5,6; m3 for bits 7,8 and m4 for bits 9,10.
+	 *
+	 * To simplify baudrate setup set all the M prescalers to same value.
+	 * For 9600 baudrate and higher it is enough to use just default (x16)
+	 * divisor or fractional divisor with M = 63, so there is no need to
+	 * use real fractional support (when the M prescalers are not equal).
+	 *
+	 * When all the M prescalers are zeroed then default (x16) divisor is
+	 * used. Default x16 scheme is more stable than M (fractional divisor),
+	 * so use M only when D divisor is not enough to derivate baudrate.
+	 *
+	 * Member port->uartclk is either xtal clock rate or TBG clock rate
+	 * divided by (d1 * d2). So UART clock driver already sets d1 and d2
+	 * divisors and UART driver cannot change them. Moreover they are
+	 * shared with both UARTs.
 	 */
+
 	m_divisor = OSAMP_DEFAULT_DIVISOR;
 	d_divisor = DIV_ROUND_CLOSEST(port->uartclk, baud * m_divisor);
 
+	if (d_divisor > BRDV_BAUD_MAX) {
+		/*
+		 * Experiments showed that small M divisors are unstable.
+		 * So use maximal possible M = 63 and calculate D divisor.
+		 */
+		m_divisor = OSAMP_MAX_DIVISOR;
+		d_divisor = DIV_ROUND_CLOSEST(port->uartclk, baud * m_divisor);
+	}
+
+	if (d_divisor < 1)
+		d_divisor = 1;
+	else if (d_divisor > BRDV_BAUD_MAX)
+		d_divisor = BRDV_BAUD_MAX;
+
 	spin_lock_irqsave(&mvebu_uart_lock, flags);
 	brdv = readl(port->membase + UART_BRDV);
 	brdv &= ~BRDV_BAUD_MASK;

@@ -500,6 +542,9 @@ static int mvebu_uart_baud_rate_set(struct uart_port *port, unsigned int baud)
 
 	osamp = readl(port->membase + UART_OSAMP);
 	osamp &= ~OSAMP_DIVISORS_MASK;
+	if (m_divisor != OSAMP_DEFAULT_DIVISOR)
+		osamp |= (m_divisor << 0) | (m_divisor << 8) |
+			(m_divisor << 16) | (m_divisor << 24);
 	writel(osamp, port->membase + UART_OSAMP);
 
 	return 0;

@@ -529,14 +574,14 @@ static void mvebu_uart_set_termios(struct uart_port *port,
 		port->ignore_status_mask |= STAT_RX_RDY(port) | STAT_BRK_ERR;
 
 	/*
-	 * Maximal divisor is 1023 * 16 when using default (x16) scheme.
-	 * Maximum achievable frequency with simple baudrate divisor is 230400.
-	 * Since the error per bit frame would be of more than 15%, achieving
-	 * higher frequencies would require to implement the fractional divisor
-	 * feature.
+	 * Maximal divisor is 1023 and maximal fractional divisor is 63. And
+	 * experiments showed that baudrates above 1/80 of base clock are not
+	 * stable and usable. So disallow baudrate above 1/80 of the base clock.
+	 * When port->uartclk is not available then mvebu_uart_baud_rate_set()
+	 * fails so values min_baud and max_baud in this case does not matter.
 	 */
-	min_baud = DIV_ROUND_UP(port->uartclk, 1023 * 16);
-	max_baud = 230400;
+	min_baud = DIV_ROUND_UP(port->uartclk, BRDV_BAUD_MAX*OSAMP_MAX_DIVISOR);
+	max_baud = port->uartclk / 80;
 
 	baud = uart_get_baud_rate(port, termios, old, min_baud, max_baud);
 	if (mvebu_uart_baud_rate_set(port, baud)) {