From fbebafa518d5cbc69d0c64023f002ff6706019f2 Mon Sep 17 00:00:00 2001
From: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
Date: Mon, 13 Jan 2025 09:33:42 +0200
Subject: [PATCH] feat(nxp-clk): add get_rate for s32cc_pll

Add the option to obtain the rate of an s32cc_pll object. The rate of
the PLL can be obtained regardless of its hardware state. The targeted
frequency is returned in case the PLL is off. Otherwise, the frequency
is determined based on settings found in its registers.

Change-Id: Id200d0eff149109a724eee69b063bf750d5cba2e
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
---
 .../nxp/clk/s32cc/include/s32cc-clk-regs.h    |  4 +-
 drivers/nxp/clk/s32cc/s32cc_clk_drv.c         | 79 +++++++++++++++++++
 2 files changed, 82 insertions(+), 1 deletion(-)

diff --git a/drivers/nxp/clk/s32cc/include/s32cc-clk-regs.h b/drivers/nxp/clk/s32cc/include/s32cc-clk-regs.h
index e54d58130..665930b2e 100644
--- a/drivers/nxp/clk/s32cc/include/s32cc-clk-regs.h
+++ b/drivers/nxp/clk/s32cc/include/s32cc-clk-regs.h
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright 2020-2021, 2023-2024 NXP
+ * Copyright 2020-2021, 2023-2025 NXP
  */
 #ifndef S32CC_CLK_REGS_H
 #define S32CC_CLK_REGS_H
@@ -48,6 +48,8 @@
 #define PLLDIG_PLLDV_RDIV_MASK		GENMASK_32(14U, PLLDIG_PLLDV_RDIV_OFFSET)
 #define PLLDIG_PLLDV_RDIV_SET(VAL)	(PLLDIG_PLLDV_RDIV_MASK & \
 					((VAL) << PLLDIG_PLLDV_RDIV_OFFSET))
+#define PLLDIG_PLLDV_RDIV(VAL)		(((VAL) & PLLDIG_PLLDV_RDIV_MASK) >> \
+					 PLLDIG_PLLDV_RDIV_OFFSET)
 #define PLLDIG_PLLDV_MFI_MASK		GENMASK_32(7U, 0U)
 #define PLLDIG_PLLDV_MFI(DIV)		(PLLDIG_PLLDV_MFI_MASK & (DIV))
 
diff --git a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
index ffbc3ac0e..95298435d 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
@@ -1194,6 +1194,82 @@ static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate,
 	return 0;
 }
 
+static int get_pll_freq(const struct s32cc_clk_obj *module,
+			const struct s32cc_clk_drv *drv,
+			unsigned long *rate, unsigned int depth)
+{
+	const struct s32cc_pll *pll = s32cc_obj2pll(module);
+	const struct s32cc_clk *source;
+	uint32_t mfi, mfn, rdiv, plldv;
+	unsigned long prate, clk_src;
+	unsigned int ldepth = depth;
+	uintptr_t pll_addr = 0UL;
+	uint64_t t1, t2;
+	uint32_t pllpd;
+	int ret;
+
+	ret = update_stack_depth(&ldepth);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = get_base_addr(pll->instance, drv, &pll_addr);
+	if (ret != 0) {
+		ERROR("Failed to detect PLL instance\n");
+		return ret;
+	}
+
+	/* Disabled PLL */
+	pllpd = mmio_read_32(PLLDIG_PLLCR(pll_addr)) & PLLDIG_PLLCR_PLLPD;
+	if (pllpd != 0U) {
+		*rate = pll->vco_freq;
+		return 0;
+	}
+
+	clk_src = mmio_read_32(PLLDIG_PLLCLKMUX(pll_addr));
+	switch (clk_src) {
+	case 0:
+		clk_src = S32CC_CLK_FIRC;
+		break;
+	case 1:
+		clk_src = S32CC_CLK_FXOSC;
+		break;
+	default:
+		ERROR("Failed to identify PLL source id %" PRIu64 "\n", clk_src);
+		return -EINVAL;
+	};
+
+	source = s32cc_get_arch_clk(clk_src);
+	if (source == NULL) {
+		ERROR("Failed to get PLL source clock\n");
+		return -EINVAL;
+	}
+
+	ret = get_module_rate(&source->desc, drv, &prate, ldepth);
+	if (ret != 0) {
+		ERROR("Failed to get PLL's parent frequency\n");
+		return ret;
+	}
+
+	plldv = mmio_read_32(PLLDIG_PLLDV(pll_addr));
+	mfi = PLLDIG_PLLDV_MFI(plldv);
+	rdiv = PLLDIG_PLLDV_RDIV(plldv);
+	if (rdiv == 0U) {
+		rdiv = 1;
+	}
+
+	/* Frac-N mode */
+	mfn = PLLDIG_PLLFD_MFN_SET(mmio_read_32(PLLDIG_PLLFD(pll_addr)));
+
+	/* PLL VCO frequency in Fractional mode when PLLDV[RDIV] is not 0 */
+	t1 = prate / rdiv;
+	t2 = (mfi * FP_PRECISION) + (mfn * FP_PRECISION / 18432U);
+
+	*rate = t1 * t2 / FP_PRECISION;
+
+	return 0;
+}
+
 static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
 			    unsigned long *orate, unsigned int *depth)
 {
@@ -1391,6 +1467,9 @@ static int get_module_rate(const struct s32cc_clk_obj *module,
 	case s32cc_clk_t:
 		ret = get_clk_freq(module, drv, rate, ldepth);
 		break;
+	case s32cc_pll_t:
+		ret = get_pll_freq(module, drv, rate, ldepth);
+		break;
 	default:
 		ret = -EINVAL;
 		break;