feat(nxp-clk): set rate for PLL divider objects
Add implementation for ARM PLL divider rate set mechanism.
Change-Id: I78f4418bcbb5ea0a6ef64675e44bd074d2230ea3
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>
diff --git a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
index d4c6155..6d8e3d1 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
@@ -267,6 +267,54 @@
return 0;
}
+static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
+ unsigned long *orate, unsigned int *depth)
+{
+ struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
+ const struct s32cc_pll *pll;
+ unsigned long prate, dc;
+ int ret;
+
+ ret = update_stack_depth(depth);
+ if (ret != 0) {
+ return ret;
+ }
+
+ if (pdiv->parent == NULL) {
+ ERROR("Failed to identify PLL divider's parent\n");
+ return -EINVAL;
+ }
+
+ pll = s32cc_obj2pll(pdiv->parent);
+ if (pll == NULL) {
+ ERROR("The parent of the PLL DIV is invalid\n");
+ return -EINVAL;
+ }
+
+ prate = pll->vco_freq;
+
+ /**
+ * The PLL is not initialized yet, so let's take a risk
+ * and accept the proposed rate.
+ */
+ if (prate == 0UL) {
+ pdiv->freq = rate;
+ *orate = rate;
+ return 0;
+ }
+
+ /* Decline in case the rate cannot fit PLL's requirements. */
+ dc = prate / rate;
+ if ((prate / dc) != rate) {
+ return -EINVAL;
+ }
+
+ pdiv->freq = rate;
+ *orate = pdiv->freq;
+
+ return 0;
+}
+
static int set_module_rate(const struct s32cc_clk_obj *module,
unsigned long rate, unsigned long *orate,
unsigned int *depth)
@@ -288,9 +336,11 @@
case s32cc_pll_t:
ret = set_pll_freq(module, rate, orate, depth);
break;
+ case s32cc_pll_out_div_t:
+ ret = set_pll_div_freq(module, rate, orate, depth);
+ break;
case s32cc_clkmux_t:
case s32cc_shared_clkmux_t:
- case s32cc_pll_out_div_t:
ret = -ENOTSUP;
break;
default:
diff --git a/drivers/nxp/clk/s32cc/s32cc_early_clks.c b/drivers/nxp/clk/s32cc/s32cc_early_clks.c
index 7ccfc00..ac1f7d0 100644
--- a/drivers/nxp/clk/s32cc/s32cc_early_clks.c
+++ b/drivers/nxp/clk/s32cc/s32cc_early_clks.c
@@ -10,6 +10,7 @@
#define S32CC_FXOSC_FREQ (40U * MHZ)
#define S32CC_ARM_PLL_VCO_FREQ (2U * GHZ)
+#define S32CC_ARM_PLL_PHI0_FREQ (1U * GHZ)
int s32cc_init_early_clks(void)
{
@@ -37,6 +38,11 @@
return ret;
}
+ ret = clk_set_rate(S32CC_CLK_ARM_PLL_PHI0, S32CC_ARM_PLL_PHI0_FREQ, NULL);
+ if (ret != 0) {
+ return ret;
+ }
+
ret = clk_enable(S32CC_CLK_FXOSC);
if (ret != 0) {
return ret;
diff --git a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
index 1fe2719..87b1ad4 100644
--- a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
+++ b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
@@ -196,4 +196,12 @@
return (struct s32cc_pll *)pll_addr;
}
+static inline struct s32cc_pll_out_div *s32cc_obj2plldiv(const struct s32cc_clk_obj *mod)
+{
+ uintptr_t plldiv_addr;
+
+ plldiv_addr = ((uintptr_t)mod) - offsetof(struct s32cc_pll_out_div, desc);
+ return (struct s32cc_pll_out_div *)plldiv_addr;
+}
+
#endif /* S32CC_CLK_MODULES_H */