1 files changed, 30 insertions, 91 deletions

diff --git a/src/soc/qualcomm/ipq40xx/clock.c b/src/soc/qualcomm/ipq40xx/clock.c
index 4d8d8e9adc..fab0c9e07a 100644
--- a/src/soc/qualcomm/ipq40xx/clock.c
+++ b/src/soc/qualcomm/ipq40xx/clock.c
@@ -31,104 +31,43 @@
 #include <soc/clock.h>
 #include <types.h>
 
-/**
- * uart_pll_vote_clk_enable - enables PLL8
- */
-void uart_pll_vote_clk_enable(unsigned int clk_dummy)
-{
-	setbits_le32(BB_PLL_ENA_SC0_REG, BIT(8));
-
-	if (!clk_dummy)
-		while ((read32(PLL_LOCK_DET_STATUS_REG) & BIT(8)) == 0)
-			;
-}
-
-/**
- * uart_set_rate_mnd - configures divider M and D values
- *
- * Sets the M, D parameters of the divider to generate the GSBI UART
- * apps clock.
- */
-static void uart_set_rate_mnd(unsigned int gsbi_port, unsigned int m,
-		unsigned int n)
-{
-	/* Assert MND reset. */
-	setbits_le32(GSBIn_UART_APPS_NS_REG(gsbi_port), BIT(7));
-	/* Program M and D values. */
-	write32(GSBIn_UART_APPS_MD_REG(gsbi_port), MD16(m, n));
-	/* Deassert MND reset. */
-	clrbits_le32(GSBIn_UART_APPS_NS_REG(gsbi_port), BIT(7));
-}
-
-/**
- * uart_branch_clk_enable_reg - enables branch clock
- *
- * Enables branch clock for GSBI UART port.
- */
-static void uart_branch_clk_enable_reg(unsigned int gsbi_port)
-{
-	setbits_le32(GSBIn_UART_APPS_NS_REG(gsbi_port), BIT(9));
-}
-
-/**
- * uart_local_clock_enable - configures N value and enables root clocks
- *
- * Sets the N parameter of the divider and enables root clock and
- * branch clocks for GSBI UART port.
- */
-static void uart_local_clock_enable(unsigned int gsbi_port, unsigned int n,
-					unsigned int m)
-{
-	unsigned int reg_val, uart_ns_val;
-	void *const reg = (void *)GSBIn_UART_APPS_NS_REG(gsbi_port);
-
-	/*
-	* Program the NS register, if applicable. NS registers are not
-	* set in the set_rate path because power can be saved by deferring
-	* the selection of a clocked source until the clock is enabled.
-	*/
-	reg_val = read32(reg); // REG(0x29D4+(0x20*((n)-1)))
-	reg_val &= ~(Uart_clk_ns_mask);
-	uart_ns_val =  NS(BIT_POS_31, BIT_POS_16, n, m, 5, 4, 3, 1, 2, 0, 3);
-	reg_val |= (uart_ns_val & Uart_clk_ns_mask);
-	write32(reg, reg_val);
-
-	/* enable MNCNTR_EN */
-	reg_val = read32(reg);
-	reg_val |= BIT(8);
-	write32(reg, reg_val);
-
-	/* set source to PLL8 running @384MHz */
-	reg_val = read32(reg);
-	reg_val |= 0x3;
-	write32(reg, reg_val);
-
-	/* Enable root. */
-	reg_val |= Uart_en_mask;
-	write32(reg, reg_val);
-	uart_branch_clk_enable_reg(gsbi_port);
-}
-
-/**
- * uart_set_gsbi_clk - enables HCLK for UART GSBI port
- */
-static void uart_set_gsbi_clk(unsigned int gsbi_port)
-{
-	setbits_le32(GSBIn_HCLK_CTL_REG(gsbi_port), BIT(4));
-}
+#define CLOCK_UPDATE_DELAY		1000
 
 /**
  * uart_clock_config - configures UART clocks
  *
  * Configures GSBI UART dividers, enable root and branch clocks.
  */
-void uart_clock_config(unsigned int gsbi_port, unsigned int m,
-		unsigned int n, unsigned int d, unsigned int clk_dummy)
+void uart_clock_config(unsigned int blsp_uart, unsigned int m,
+		unsigned int n, unsigned int d)
 {
-	uart_set_rate_mnd(gsbi_port, m, d);
-	uart_pll_vote_clk_enable(clk_dummy);
-	uart_local_clock_enable(gsbi_port, n, m);
-	uart_set_gsbi_clk(gsbi_port);
+	int i;
+
+	/* Setup M, N & D */
+	write32(GCC_BLSP1_UART_APPS_M(blsp_uart), m);
+	write32(GCC_BLSP1_UART_APPS_N(blsp_uart), ~(n - m));
+	write32(GCC_BLSP1_UART_APPS_D(blsp_uart), ~d);
+	write32(GCC_BLSP1_UART_MISC(blsp_uart), 0);
+
+	/* Setup source sel etc. */
+	write32(GCC_BLSP1_UART_APPS_CFG_RCGR(blsp_uart),
+			0 |		/*  0: 4 SRC_DIV = Bypass */
+			0 << 8 |	/*  8:10 SRC_SEL = CxO */
+			2 << 12);	/* 13:12 Mode = Dual Edge */
+
+	/* Trigger update */
+	setbits_le32(GCC_BLSP1_UART_APPS_CMD_RCGR(blsp_uart), 1);
+
+	/* Wait for update */
+	for (i = 0; i < CLOCK_UPDATE_DELAY; i++) {
+		if (!(read32(GCC_BLSP1_UART_APPS_CMD_RCGR(blsp_uart)) & 1)) {
+			/* Updated */
+			break;
+		}
+		udelay(1);
+	}
+
+	setbits_le32(GCC_CLK_BRANCH_ENA, BLSP1_AHB | BLSP1_SLEEP);
 }
 
 /**