From e0a660ec5bdb11820600388d084f461f0bef8ed7 Mon Sep 17 00:00:00 2001
From: Atish Patra <atish.patra@wdc.com>
Date: Mon, 17 Dec 2018 15:31:49 -0800
Subject: [PATCH] Fix baud rate divisor computation for U540.

Signed-off-by: Atish Patra <atish.patra@wdc.com>
---
 plat/common/serial/sifive-uart.c   | 26 +++++++++++++++++++++++++-
 plat/sifive/hifive_u540/platform.c | 25 +++++++++++++++++++++----
 2 files changed, 46 insertions(+), 5 deletions(-)

diff --git a/plat/common/serial/sifive-uart.c b/plat/common/serial/sifive-uart.c
index 50c7cad..60eade3 100644
--- a/plat/common/serial/sifive-uart.c
+++ b/plat/common/serial/sifive-uart.c
@@ -8,6 +8,7 @@
  */
 
 #include <sbi/riscv_io.h>
+#include <sbi/sbi_console.h>
 #include <plat/serial/sifive-uart.h>
 
 #define UART_REG_TXFIFO		0
@@ -28,6 +29,29 @@ static volatile void *uart_base;
 static u32 uart_in_freq;
 static u32 uart_baudrate;
 
+/**
+ * Find minimum divisor divides in_freq to max_target_hz;
+ * Based on uart driver n SiFive FSBL.
+ *
+ * f_baud = f_in / (div + 1) => div = (f_in / f_baud) - 1
+ * The nearest integer solution requires rounding up as to not exceed max_target_hz.
+ * div  = ceil(f_in / f_baud) - 1
+ *	= floor((f_in - 1 + f_baud) / f_baud) - 1
+ * This should not overflow as long as (f_in - 1 + f_baud) does not exceed
+ * 2^32 - 1, which is unlikely since we represent frequencies in kHz.
+ */
+static inline unsigned int uart_min_clk_divisor(uint64_t in_freq,
+						uint64_t max_target_hz)
+{
+	uint64_t quotient = (in_freq + max_target_hz - 1) / (max_target_hz);
+	// Avoid underflow
+	if (quotient == 0) {
+		return 0;
+	} else {
+		return quotient - 1;
+	}
+}
+
 static u32 get_reg(u32 num)
 {
 	return readl(uart_base + (num * 0x4));
@@ -61,7 +85,7 @@ int sifive_uart_init(unsigned long base,
 	uart_baudrate = baudrate;
 
 	/* Configure baudrate */
-	set_reg(UART_REG_DIV, (in_freq / baudrate) - 1);
+	set_reg(UART_REG_DIV, uart_min_clk_divisor(in_freq, baudrate));
 	/* Disable interrupts */
 	set_reg(UART_REG_IE, 0);
 	/* Enable TX */
diff --git a/plat/sifive/hifive_u540/platform.c b/plat/sifive/hifive_u540/platform.c
index a4a401e..5b1c989 100644
--- a/plat/sifive/hifive_u540/platform.c
+++ b/plat/sifive/hifive_u540/platform.c
@@ -10,12 +10,12 @@
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_const.h>
 #include <sbi/sbi_platform.h>
+#include <sbi/riscv_io.h>
 #include <plat/irqchip/plic.h>
 #include <plat/serial/sifive-uart.h>
 #include <plat/sys/clint.h>
 
 #define SIFIVE_U_SYS_CLK			1000000000
-#define SIFIVE_U_PERIPH_CLK			(SIFIVE_U_SYS_CLK / 2)
 
 #define SIFIVE_U_CLINT_ADDR			0x2000000
 
@@ -23,8 +23,15 @@
 #define SIFIVE_U_PLIC_NUM_SOURCES		0x35
 #define SIFIVE_U_PLIC_NUM_PRIORITIES		7
 
-#define SIFIVE_U_UART0_ADDR			0x10013000
-#define SIFIVE_U_UART1_ADDR			0x10023000
+#define SIFIVE_U_UART0_ADDR			0x10010000
+#define SIFIVE_U_UART1_ADDR			0x10011000
+#define SIFIVE_UART_BAUDRATE			115200
+
+/* PRCI clock related macros */
+//TODO: Do we need a separate driver for this ?
+#define SIFIVE_PRCI_BASE_ADDR			0x10000000
+#define SIFIVE_PRCI_CLKMUXSTATUSREG    		0x002C
+#define SIFIVE_PRCI_CLKMUX_STATUS_TLCLKSEL      (0x1 << 1)
 
 static int sifive_u_cold_final_init(void)
 {
@@ -57,8 +64,18 @@ static int sifive_u_pmp_region_info(u32 target_hart, u32 index,
 
 static int sifive_u_console_init(void)
 {
+	unsigned long peri_in_freq;
+
+	if (readl((volatile void *)SIFIVE_PRCI_BASE_ADDR +
+		  SIFIVE_PRCI_CLKMUXSTATUSREG) &
+		  SIFIVE_PRCI_CLKMUX_STATUS_TLCLKSEL){
+		peri_in_freq = SIFIVE_U_SYS_CLK;
+	} else {
+		peri_in_freq = SIFIVE_U_SYS_CLK / 2;
+	}
+
 	return sifive_uart_init(SIFIVE_U_UART0_ADDR,
-				SIFIVE_U_PERIPH_CLK, 115200);
+				peri_in_freq, SIFIVE_UART_BAUDRATE);
 }
 
 static int sifive_u_cold_irqchip_init(void)
-- 
2.7.4