From c23dde6c193d26fae9b2a8e18140b90faeba3661 Mon Sep 17 00:00:00 2001
From: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
Date: Wed, 15 Jan 2025 14:59:22 +0200
Subject: [PATCH] feat(nxp-clk): restore pll output dividers rate
Reconfiguration of the PLL may be requested while some output dividers
are already enabled. To prevent setting a different frequency for these
enabled dividers, the driver will attempt to adjust the division factor
to achieve the initially requested rate.
Change-Id: I7800c05b2f21bbdeda243db865942b647983687d
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
---
drivers/nxp/clk/s32cc/s32cc_clk_drv.c | 144 ++++++++++++++++++++++++--
1 file changed, 136 insertions(+), 8 deletions(-)
diff --git a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
index a7320486d..c235e046f 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
@@ -281,6 +281,70 @@ static void enable_odiv(uintptr_t pll_addr, uint32_t div_index)
mmio_setbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE);
}
+static void enable_odivs(uintptr_t pll_addr, uint32_t ndivs, uint32_t mask)
+{
+ uint32_t i;
+
+ for (i = 0; i < ndivs; i++) {
+ if ((mask & BIT_32(i)) != 0U) {
+ enable_odiv(pll_addr, i);
+ }
+ }
+}
+
+static int adjust_odiv_settings(const struct s32cc_pll *pll, uintptr_t pll_addr,
+ uint32_t odivs_mask, unsigned long old_vco)
+{
+ uint64_t old_odiv_freq, odiv_freq;
+ uint32_t i, pllodiv, pdiv;
+ int ret = 0;
+
+ if (old_vco == 0UL) {
+ return 0;
+ }
+
+ for (i = 0; i < pll->ndividers; i++) {
+ if ((odivs_mask & BIT_32(i)) == 0U) {
+ continue;
+ }
+
+ pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, i));
+
+ pdiv = PLLDIG_PLLODIV_DIV(pllodiv);
+
+ old_odiv_freq = ((old_vco * FP_PRECISION) / (pdiv + 1U)) / FP_PRECISION;
+ pdiv = (uint32_t)(pll->vco_freq * FP_PRECISION / old_odiv_freq / FP_PRECISION);
+
+ odiv_freq = pll->vco_freq * FP_PRECISION / pdiv / FP_PRECISION;
+
+ if (old_odiv_freq != odiv_freq) {
+ ERROR("Failed to adjust ODIV %" PRIu32 " to match previous frequency\n",
+ i);
+ }
+
+ pllodiv = PLLDIG_PLLODIV_DIV_SET(pdiv - 1U);
+ mmio_write_32(PLLDIG_PLLODIV(pll_addr, i), pllodiv);
+ }
+
+ return ret;
+}
+
+static uint32_t get_enabled_odivs(uintptr_t pll_addr, uint32_t ndivs)
+{
+ uint32_t mask = 0;
+ uint32_t pllodiv;
+ uint32_t i;
+
+ for (i = 0; i < ndivs; i++) {
+ pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, i));
+ if ((pllodiv & PLLDIG_PLLODIV_DE) != 0U) {
+ mask |= BIT_32(i);
+ }
+ }
+
+ return mask;
+}
+
static void disable_odivs(uintptr_t pll_addr, uint32_t ndivs)
{
uint32_t i;
@@ -305,18 +369,54 @@ static void disable_pll_hw(uintptr_t pll_addr)
mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD);
}
+static bool is_pll_enabled(uintptr_t pll_base)
+{
+ uint32_t pllcr, pllsr;
+
+ pllcr = mmio_read_32(PLLDIG_PLLCR(pll_base));
+ pllsr = mmio_read_32(PLLDIG_PLLSR(pll_base));
+
+ /* Enabled and locked PLL */
+ if ((pllcr & PLLDIG_PLLCR_PLLPD) != 0U) {
+ return false;
+ }
+
+ if ((pllsr & PLLDIG_PLLSR_LOCK) == 0U) {
+ return false;
+ }
+
+ return true;
+}
+
static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr,
const struct s32cc_clk_drv *drv, uint32_t sclk_id,
- unsigned long sclk_freq)
+ unsigned long sclk_freq, unsigned int depth)
{
uint32_t rdiv = 1, mfi, mfn;
+ unsigned long old_vco = 0UL;
+ unsigned int ldepth = depth;
+ uint32_t odivs_mask;
int ret;
+ ret = update_stack_depth(&ldepth);
+ if (ret != 0) {
+ return ret;
+ }
+
ret = get_pll_mfi_mfn(pll->vco_freq, sclk_freq, &mfi, &mfn);
if (ret != 0) {
return -EINVAL;
}
+ odivs_mask = get_enabled_odivs(pll_addr, pll->ndividers);
+
+ if (is_pll_enabled(pll_addr)) {
+ ret = get_module_rate(&pll->desc, drv, &old_vco, ldepth);
+ if (ret != 0) {
+ return ret;
+ }
+ }
+
/* Disable ODIVs*/
disable_odivs(pll_addr, pll->ndividers);
@@ -334,8 +434,16 @@ static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr,
mmio_write_32(PLLDIG_PLLFD(pll_addr),
PLLDIG_PLLFD_MFN_SET(mfn) | PLLDIG_PLLFD_SMDEN);
+ ret = adjust_odiv_settings(pll, pll_addr, odivs_mask, old_vco);
+ if (ret != 0) {
+ return ret;
+ }
+
enable_pll_hw(pll_addr);
+ /* Enable out dividers */
+ enable_odivs(pll_addr, pll->ndividers, odivs_mask);
+
return ret;
}
@@ -344,10 +452,11 @@ static int enable_pll(struct s32cc_clk_obj *module,
unsigned int depth)
{
const struct s32cc_pll *pll = s32cc_obj2pll(module);
+ unsigned int clk_src, ldepth = depth;
+ unsigned long sclk_freq, pll_vco;
const struct s32cc_clkmux *mux;
uintptr_t pll_addr = UL(0x0);
- unsigned int ldepth = depth;
- unsigned long sclk_freq;
+ bool pll_enabled;
uint32_t sclk_id;
int ret;
@@ -387,7 +496,20 @@ static int enable_pll(struct s32cc_clk_obj *module,
return -EINVAL;
};
- return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq);
+ ret = get_module_rate(&pll->desc, drv, &pll_vco, depth);
+ if (ret != 0) {
+ return ret;
+ }
+
+ pll_enabled = is_pll_enabled(pll_addr);
+ clk_src = mmio_read_32(PLLDIG_PLLCLKMUX(pll_addr));
+
+ if ((clk_src == sclk_id) && pll_enabled &&
+ (pll_vco == pll->vco_freq)) {
+ return 0;
+ }
+
+ return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq, ldepth);
}
static inline struct s32cc_pll *get_div_pll(const struct s32cc_pll_out_div *pdiv)
@@ -449,6 +571,7 @@ static int enable_pll_div(struct s32cc_clk_obj *module,
uintptr_t pll_addr = 0x0ULL;
unsigned int ldepth = depth;
const struct s32cc_pll *pll;
+ unsigned long pll_vco;
uint32_t dc;
int ret;
@@ -469,7 +592,14 @@ static int enable_pll_div(struct s32cc_clk_obj *module,
return -EINVAL;
}
- dc = (uint32_t)(pll->vco_freq / pdiv->freq);
+ ret = get_module_rate(&pll->desc, drv, &pll_vco, ldepth);
+ if (ret != 0) {
+ ERROR("Failed to enable the PLL due to unknown rate for 0x%" PRIxPTR "\n",
+ pll_addr);
+ return ret;
+ }
+
+ dc = (uint32_t)(pll_vco / pdiv->freq);
config_pll_out_div(pll_addr, pdiv->index, dc);
@@ -1225,7 +1355,6 @@ static int get_pll_freq(const struct s32cc_clk_obj *module,
unsigned int ldepth = depth;
uintptr_t pll_addr = 0UL;
uint64_t t1, t2;
- uint32_t pllpd;
int ret;
ret = update_stack_depth(&ldepth);
@@ -1240,8 +1369,7 @@ static int get_pll_freq(const struct s32cc_clk_obj *module,
}
/* Disabled PLL */
- pllpd = mmio_read_32(PLLDIG_PLLCR(pll_addr)) & PLLDIG_PLLCR_PLLPD;
- if (pllpd != 0U) {
+ if (!is_pll_enabled(pll_addr)) {
*rate = pll->vco_freq;
return 0;
}