From: Patrick Delaunay <patrick.delaunay@st.com>
Date: Wed, 30 Jan 2019 12:07:03 +0000 (+0100)
Subject: clk: stm32mp1: recalculate counter when switching freq
X-Git-Url: http://git.dujemihanovic.xyz/?a=commitdiff_plain;h=f3a23c26097144de9ad08644a4bfc9c238df873a;p=u-boot.git

clk: stm32mp1: recalculate counter when switching freq

Because stgen is initialized with HSI clock, we need to
recalculate the counter when changing frequency.

Signed-off-by: Lionel Debieve <lionel.debieve@st.com>
Signed-off-by: Patrick Delaunay <patrick.delaunay@st.com>
---

diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index 39eaf64cdd..916f9f9e8c 100644
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -1469,10 +1469,15 @@ static void stgen_config(struct stm32mp1_clk_priv *priv)
 	rate = stm32mp1_clk_get(priv, p);
 
 	if (cntfid0 != rate) {
+		u64 counter;
+
 		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);
+		counter = (u64)readl(stgenc + STGENC_CNTCVL);
+		counter |= ((u64)(readl(stgenc + STGENC_CNTCVU))) << 32;
+		counter = lldiv(counter * (u64)rate, cntfid0);
+		writel((u32)counter, stgenc + STGENC_CNTCVL);
+		writel((u32)(counter >> 32), stgenc + STGENC_CNTCVU);
 		writel(rate, stgenc + STGENC_CNTFID0);
 		setbits_le32(stgenc + STGENC_CNTCR, STGENC_CNTCR_EN);