[PATCH 2/2] clocksource: timer-imx-sysctr: support i.MX95

[Peng Fan (OSS)]

From: Peng Fan <peng.fan@xxxxxxx>

To i.MX95 System counter module, we use Read register space to get
the counter, not the Control register space to get the counter, because
System Manager firmware not allow Linux to read Control register space.

Signed-off-by: Peng Fan <peng.fan@xxxxxxx>
---
 drivers/clocksource/timer-imx-sysctr.c | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/drivers/clocksource/timer-imx-sysctr.c b/drivers/clocksource/timer-imx-sysctr.c
index 5a7a951c4efc..3d3bc16388ed 100644
--- a/drivers/clocksource/timer-imx-sysctr.c
+++ b/drivers/clocksource/timer-imx-sysctr.c
@@ -8,9 +8,12 @@
 #include "timer-of.h"
 
 #define CMP_OFFSET	0x10000
+#define RD_OFFSET	0x20000
 
 #define CNTCV_LO	0x8
 #define CNTCV_HI	0xc
+#define CNTCV_LO_IMX95	(RD_OFFSET + 0x8)
+#define CNTCV_HI_IMX95	(RD_OFFSET + 0xc)
 #define CMPCV_LO	(CMP_OFFSET + 0x20)
 #define CMPCV_HI	(CMP_OFFSET + 0x24)
 #define CMPCR		(CMP_OFFSET + 0x2c)
@@ -22,6 +25,8 @@
 
 static void __iomem *sys_ctr_base __ro_after_init;
 static u32 cmpcr __ro_after_init;
+static u32 cntcv_hi = CNTCV_HI;
+static u32 cntcv_lo = CNTCV_LO;
 
 static void sysctr_timer_enable(bool enable)
 {
@@ -43,9 +48,9 @@ static inline u64 sysctr_read_counter(void)
 	u32 cnt_hi, tmp_hi, cnt_lo;
 
 	do {
-		cnt_hi = readl_relaxed(sys_ctr_base + CNTCV_HI);
-		cnt_lo = readl_relaxed(sys_ctr_base + CNTCV_LO);
-		tmp_hi = readl_relaxed(sys_ctr_base + CNTCV_HI);
+		cnt_hi = readl_relaxed(sys_ctr_base + cntcv_hi);
+		cnt_lo = readl_relaxed(sys_ctr_base + cntcv_lo);
+		tmp_hi = readl_relaxed(sys_ctr_base + cntcv_hi);
 	} while (tmp_hi != cnt_hi);
 
 	return  ((u64) cnt_hi << 32) | cnt_lo;
@@ -139,6 +144,11 @@ static int __init sysctr_timer_init(struct device_node *np)
 		to_sysctr.of_clk.rate /= SYS_CTR_CLK_DIV;
 	}
 
+	if (of_device_is_compatible(np, "nxp,imx95-sysctr-timer")) {
+		cntcv_hi = CNTCV_HI_IMX95;
+		cntcv_lo = CNTCV_LO_IMX95;
+	}
+
 	sys_ctr_base = timer_of_base(&to_sysctr);
 	cmpcr = readl(sys_ctr_base + CMPCR);
 	cmpcr &= ~SYS_CTR_EN;
-- 
2.37.1