clock: stm32mp1: add stgen clock source change support

The STGEN is the clock source for the Cortex A7 arch timer.
So after modification of its frequency, CP15 cntfreq is updated
and a new timer init is performed.

Signed-off-by: Patrick Delaunay <patrick.delaunay@st.com>

diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index 55b0f7977be384a205f3be4a983f152aa497ed41..c67aa444735ae3261dcc2cd14ca3495a4950cc41 100644 (file)
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -27,6 +27,15 @@
 #define TIMEOUT_200MS          200000
 #define TIMEOUT_1S             1000000
 
+/* STGEN registers */
+#define STGENC_CNTCR           0x00
+#define STGENC_CNTSR           0x04
+#define STGENC_CNTCVL          0x08
+#define STGENC_CNTCVU          0x0C
+#define STGENC_CNTFID0         0x20
+
+#define STGENC_CNTCR_EN                BIT(0)
+
 /* RCC registers */
 #define RCC_OCENSETR           0x0C
 #define RCC_OCENCLRR           0x10
@@ -1377,6 +1386,36 @@ static int set_clksrc(struct stm32mp1_clk_priv *priv, unsigned int clksrc)
        return ret;
 }
 
+static void stgen_config(struct stm32mp1_clk_priv *priv)
+{
+       int p;
+       u32 stgenc, cntfid0;
+       ulong rate;
+
+       stgenc = (u32)syscon_get_first_range(STM32MP_SYSCON_STGEN);
+
+       cntfid0 = readl(stgenc + STGENC_CNTFID0);
+       p = stm32mp1_clk_get_parent(priv, STGEN_K);
+       rate = stm32mp1_clk_get(priv, p);
+
+       if (cntfid0 != rate) {
+               pr_debug("System Generic Counter (STGEN) update\n");
+               clrbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
+               writel(0x0, stgenc + STGENC_CNTCVL);
+               writel(0x0, stgenc + STGENC_CNTCVU);
+               writel(rate, stgenc + STGENC_CNTFID0);
+               setbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);
+
+               __asm__ volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (rate));
+
+               /* need to update gd->arch.timer_rate_hz with new frequency */
+               timer_init();
+               pr_debug("gd->arch.timer_rate_hz = %x\n",
+                        (u32)gd->arch.timer_rate_hz);
+               pr_debug("Tick = %x\n", (u32)(get_ticks()));
+       }
+}
+
 static int set_clkdiv(unsigned int clkdiv, u32 address)
 {
        u32 val;
@@ -1544,8 +1583,10 @@ static int stm32mp1_clktree(struct udevice *dev)
 
        /* configure HSIDIV */
        debug("configure HSIDIV\n");
-       if (priv->osc[_HSI])
+       if (priv->osc[_HSI]) {
                stm32mp1_hsidiv(rcc, priv->osc[_HSI]);
+               stgen_config(priv);
+       }
 
        /* select DIV */
        debug("select DIV\n");
@@ -1634,6 +1675,9 @@ static int stm32mp1_clktree(struct udevice *dev)
                        pkcs_config(priv, CLK_CKPER_DISABLED);
        }
 
+       /* STGEN clock source can change with CLK_STGEN_XXX */
+       stgen_config(priv);
+
        debug("oscillator off\n");
        /* switch OFF HSI if not found in device-tree */
        if (!priv->osc[_HSI])