blackfin: Set correct early debug serial baudrate.

Calculate the early uart clock from the system clock registers set by
the bootrom other than the predefine uboot clock macros.

Split the early baudrate setting function and the normal baudrate
setting one.

Signed-off-by: Sonic Zhang <sonic.zhang@analog.com>

6 files changed, 123 insertions, 88 deletions

diff --git a/arch/blackfin/cpu/initcode.c b/arch/blackfin/cpu/initcode.c
index e8ea0ba66a..5c12726d77 100644
--- a/arch/blackfin/cpu/initcode.c
+++ b/arch/blackfin/cpu/initcode.c
@@ -194,15 +194,8 @@ static inline void serial_init(void)
 #endif
 
 	if (BFIN_DEBUG_EARLY_SERIAL) {
-		int enabled = serial_early_enabled(uart_base);
-
 		serial_early_init(uart_base);
-
-		/* If the UART is off, that means we need to program
-		 * the baud rate ourselves initially.
-		 */
-		if (!enabled)
-			serial_early_set_baud(uart_base, CONFIG_BAUDRATE);
+		serial_early_set_baud(uart_base, CONFIG_BAUDRATE);
 	}
 }
 
@@ -714,37 +707,29 @@ program_clocks(ADI_BOOT_DATA *bs, bool put_into_srfs)
 __attribute__((always_inline)) static inline void
 update_serial_clocks(ADI_BOOT_DATA *bs, uint sdivB, uint divB, uint vcoB)
 {
-	serial_putc('a');
-
 	/* Since we've changed the SCLK above, we may need to update
 	 * the UART divisors (UART baud rates are based on SCLK).
 	 * Do the division by hand as there are no native instructions
 	 * for dividing which means we'd generate a libgcc reference.
 	 */
-	if (CONFIG_BFIN_BOOT_MODE == BFIN_BOOT_UART) {
-		unsigned int sdivR, vcoR;
-		int dividend = sdivB * divB * vcoR;
-		int divisor = vcoB * sdivR;
-		unsigned int quotient;
+	unsigned int sdivR, vcoR;
+	unsigned int dividend = sdivB * divB * vcoR;
+	unsigned int divisor = vcoB * sdivR;
+	unsigned int quotient;
 
-		serial_putc('b');
+	serial_putc('a');
 
 #ifdef __ADSPBF60x__
-		sdivR = bfin_read_CGU_DIV();
-		sdivR = ((sdivR >> 8) & 0x1f) * ((sdivR >> 5) & 0x7);
-		vcoR = (bfin_read_CGU_CTL() >> 8) & 0x7f;
+	sdivR = bfin_read_CGU_DIV();
+	sdivR = ((sdivR >> 8) & 0x1f) * ((sdivR >> 5) & 0x7);
+	vcoR = (bfin_read_CGU_CTL() >> 8) & 0x7f;
 #else
-		sdivR = bfin_read_PLL_DIV() & 0xf;
-		vcoR = (bfin_read_PLL_CTL() >> 9) & 0x3f;
+	sdivR = bfin_read_PLL_DIV() & 0xf;
+	vcoR = (bfin_read_PLL_CTL() >> 9) & 0x3f;
 #endif
-
-		for (quotient = 0; dividend > 0; ++quotient)
-			dividend -= divisor;
-		serial_early_put_div(quotient - ANOMALY_05000230);
-		serial_putc('c');
-	}
-
-	serial_putc('d');
+	quotient = early_division(dividend, divisor);
+	serial_early_put_div(quotient - ANOMALY_05000230);
+	serial_putc('c');
 }
 
 __attribute__((always_inline)) static inline void
diff --git a/arch/blackfin/cpu/serial.c b/arch/blackfin/cpu/serial.c
index 9847e9f2c5..36d2a5c8e4 100644
--- a/arch/blackfin/cpu/serial.c
+++ b/arch/blackfin/cpu/serial.c
@@ -195,6 +195,14 @@ static void uart_loop(uint32_t uart_base, int state)
 
 #endif
 
+static inline void __serial_set_baud(uint32_t uart_base, uint32_t baud)
+{
+	uint16_t divisor = (get_uart_clk() + (baud * 8)) / (baud * 16)
+			- ANOMALY_05000230;
+
+	/* Program the divisor to get the baud rate we want */
+	serial_set_divisor(uart_base, divisor);
+}
 #ifdef CONFIG_SYS_BFIN_UART
 
 static void uart_puts(uint32_t uart_base, const char *s)
@@ -209,7 +217,7 @@ static int uart##n##_init(void) \
 	const unsigned short pins[] = { _P_UART(n, RX), _P_UART(n, TX), 0, }; \
 	peripheral_request_list(pins, "bfin-uart"); \
 	uart_init(MMR_UART(n)); \
-	serial_early_set_baud(MMR_UART(n), gd->baudrate); \
+	__serial_set_baud(MMR_UART(n), gd->baudrate); \
 	uart_lsr_clear(MMR_UART(n)); \
 	return 0; \
 } \
@@ -221,7 +229,7 @@ static int uart##n##_uninit(void) \
 \
 static void uart##n##_setbrg(void) \
 { \
-	serial_early_set_baud(MMR_UART(n), gd->baudrate); \
+	__serial_set_baud(MMR_UART(n), gd->baudrate); \
 } \
 \
 static int uart##n##_getc(void) \
diff --git a/arch/blackfin/cpu/serial1.h b/arch/blackfin/cpu/serial1.h
index a20175bc7f..52f1c62eb9 100644
--- a/arch/blackfin/cpu/serial1.h
+++ b/arch/blackfin/cpu/serial1.h
@@ -15,6 +15,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include <asm/clock.h>
+
 #define MMR_UART(n) _PASTE_UART(n, UART, DLL)
 #ifdef UART_DLL
 # define UART0_DLL UART_DLL
@@ -230,19 +232,6 @@ static inline void serial_early_do_portmux(void)
 }
 
 __attribute__((always_inline))
-static inline uint32_t uart_sclk(void)
-{
-#if defined(BFIN_IN_INITCODE) || defined(CONFIG_DEBUG_EARLY_SERIAL)
-	/* We cannot use get_sclk() early on as it uses
-	 * caches in external memory
-	 */
-	return CONFIG_CLKIN_HZ * CONFIG_VCO_MULT / CONFIG_SCLK_DIV;
-#else
-	return get_sclk();
-#endif
-}
-
-__attribute__((always_inline))
 static inline int uart_init(uint32_t uart_base)
 {
 	/* always enable UART -- avoids anomalies 05000309 and 05000350 */
@@ -275,21 +264,8 @@ static inline int serial_early_uninit(uint32_t uart_base)
 }
 
 __attribute__((always_inline))
-static inline int serial_early_enabled(uint32_t uart_base)
+static inline void serial_set_divisor(uint32_t uart_base, uint16_t divisor)
 {
-	return bfin_read(&pUART->gctl) & UCEN;
-}
-
-__attribute__((always_inline))
-static inline void serial_early_set_baud(uint32_t uart_base, uint32_t baud)
-{
-	/* Translate from baud into divisor in terms of SCLK.  The
-	 * weird multiplication is to make sure we over sample just
-	 * a little rather than under sample the incoming signals.
-	 */
-	uint16_t divisor = (uart_sclk() + (baud * 8)) / (baud * 16) -
-			ANOMALY_05000230;
-
 	/* Set DLAB in LCR to Access DLL and DLH */
 	ACCESS_LATCH();
 	SSYNC();
@@ -305,6 +281,19 @@ static inline void serial_early_set_baud(uint32_t uart_base, uint32_t baud)
 }
 
 __attribute__((always_inline))
+static inline void serial_early_set_baud(uint32_t uart_base, uint32_t baud)
+{
+	/* Translate from baud into divisor in terms of SCLK.  The
+	 * weird multiplication is to make sure we over sample just
+	 * a little rather than under sample the incoming signals.
+	 */
+	uint16_t divisor = early_division(early_get_uart_clk() + (baud * 8),
+			baud * 16) - ANOMALY_05000230;
+
+	serial_set_divisor(uart_base, divisor);
+}
+
+__attribute__((always_inline))
 static inline void serial_early_put_div(uint16_t divisor)
 {
 	uint32_t uart_base = UART_BASE;
diff --git a/arch/blackfin/cpu/serial4.h b/arch/blackfin/cpu/serial4.h
index 887845c186..65483960b9 100644
--- a/arch/blackfin/cpu/serial4.h
+++ b/arch/blackfin/cpu/serial4.h
@@ -15,6 +15,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include <asm/clock.h>
+
 #define MMR_UART(n) _PASTE_UART(n, UART, REVID)
 #define UART_BASE MMR_UART(CONFIG_UART_CONSOLE)
 
@@ -84,20 +86,6 @@ static inline void serial_early_do_portmux(void)
 }
 
 __attribute__((always_inline))
-static inline uint32_t uart_sclk(void)
-{
-#if defined(BFIN_IN_INITCODE) || defined(CONFIG_DEBUG_EARLY_SERIAL)
-	/* We cannot use get_sclk() early on as it uses caches in
-	 * external memory
-	 */
-	return CONFIG_CLKIN_HZ * CONFIG_VCO_MULT / CONFIG_SCLK_DIV /
-		CONFIG_SCLK0_DIV;
-#else
-	return get_sclk0();
-#endif
-}
-
-__attribute__((always_inline))
 static inline int uart_init(uint32_t uart_base)
 {
 	/* always enable UART to 8-bit mode */
@@ -127,19 +115,20 @@ static inline int serial_early_uninit(uint32_t uart_base)
 }
 
 __attribute__((always_inline))
-static inline int serial_early_enabled(uint32_t uart_base)
+static inline void serial_set_divisor(uint32_t uart_base, uint16_t divisor)
 {
-	return bfin_read(&pUART->control) & UEN;
+	/* Program the divisor to get the baud rate we want */
+	bfin_write(&pUART->clock, divisor);
+	SSYNC();
 }
 
 __attribute__((always_inline))
 static inline void serial_early_set_baud(uint32_t uart_base, uint32_t baud)
 {
-	uint32_t divisor = uart_sclk() / (baud * 16);
+	uint16_t divisor = early_division(early_get_uart_clk(), baud * 16);
 
 	/* Program the divisor to get the baud rate we want */
-	bfin_write(&pUART->clock, divisor);
-	SSYNC();
+	serial_set_divisor(uart_base, divisor);
 }
 
 __attribute__((always_inline))
diff --git a/arch/blackfin/include/asm/clock.h b/arch/blackfin/include/asm/clock.h
new file mode 100644
index 0000000000..df6cd68171
--- /dev/null
+++ b/arch/blackfin/include/asm/clock.h
@@ -0,0 +1,74 @@
+
+/*
+ * Copyright (C) 2012 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+#ifndef __CLOCK_H__
+#define __CLOCK_H__
+
+#include <asm/blackfin.h>
+#ifdef PLL_CTL
+#include <asm/mach-common/bits/pll.h>
+# define pll_is_bypassed() (bfin_read_PLL_STAT() & DF)
+#else
+#include <asm/mach-common/bits/cgu.h>
+# define pll_is_bypassed() (bfin_read_CGU_STAT() & PLLBP)
+# define bfin_read_PLL_CTL() bfin_read_CGU_CTL()
+# define bfin_read_PLL_DIV() bfin_read_CGU_DIV()
+# define SSEL SYSSEL
+# define SSEL_P SYSSEL_P
+#endif
+
+__attribute__((always_inline))
+static inline uint32_t early_division(uint32_t dividend, uint32_t divisor)
+{
+	uint32_t quotient;
+	uint32_t i, j;
+
+	for (quotient = 1, i = 1; dividend > divisor; ++i) {
+		j = divisor << i;
+		if (j > dividend || (j & 0x80000000)) {
+			--i;
+			quotient += (1 << i);
+			dividend -= (divisor << i);
+			i = 0;
+		}
+	}
+
+	return quotient;
+}
+
+__attribute__((always_inline))
+static inline uint32_t early_get_uart_clk(void)
+{
+	uint32_t msel, pll_ctl, vco;
+	uint32_t div, ssel, sclk, uclk;
+
+	pll_ctl = bfin_read_PLL_CTL();
+	msel = (pll_ctl & MSEL) >> MSEL_P;
+	if (msel == 0)
+		msel = (MSEL >> MSEL_P) + 1;
+
+	vco = (CONFIG_CLKIN_HZ >> (pll_ctl & DF)) * msel;
+	sclk = vco;
+	if (!pll_is_bypassed()) {
+		div = bfin_read_PLL_DIV();
+		ssel = (div & SSEL) >> SSEL_P;
+		sclk = early_division(vco, ssel);
+	}
+	uclk = sclk;
+#ifdef CGU_DIV
+	ssel = (div & S0SEL) >> S0SEL_P;
+	uclk = early_division(sclk, ssel);
+#endif
+	return uclk;
+}
+
+#ifdef CGU_DIV
+# define get_uart_clk get_sclk0
+#else
+# define get_uart_clk get_sclk
+#endif
+
+#endif
diff --git a/arch/blackfin/lib/clocks.c b/arch/blackfin/lib/clocks.c
index d852f5ebed..97795e11ac 100644
--- a/arch/blackfin/lib/clocks.c
+++ b/arch/blackfin/lib/clocks.c
@@ -7,17 +7,7 @@
  */
 
 #include <common.h>
-#include <asm/blackfin.h>
-
-#ifdef PLL_CTL
-# include <asm/mach-common/bits/pll.h>
-# define pll_is_bypassed() (bfin_read_PLL_STAT() & DF)
-#else
-# include <asm/mach-common/bits/cgu.h>
-# define pll_is_bypassed() (bfin_read_CGU_STAT() & PLLBP)
-# define bfin_read_PLL_CTL() bfin_read_CGU_CTL()
-# define bfin_read_PLL_DIV() bfin_read_CGU_DIV()
-#endif
+#include <asm/clock.h>
 
 /* Get the voltage input multiplier */
 u_long get_vco(void)