feat(nxp-clk): set rate for PLL objects

Add implementation for ARM PLL rate set mechanism.

Change-Id: Ic859567bd67747f173d425158cdc581801f7446c
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 69a9d21..d4c6155 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
@@ -245,6 +245,28 @@
 	return -EINVAL;
 }
 
+static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate,
+			unsigned long *orate, unsigned int *depth)
+{
+	struct s32cc_pll *pll = s32cc_obj2pll(module);
+	int ret;
+
+	ret = update_stack_depth(depth);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((pll->vco_freq != 0UL) && (pll->vco_freq != rate)) {
+		ERROR("PLL frequency was already set\n");
+		return -EINVAL;
+	}
+
+	pll->vco_freq = rate;
+	*orate = pll->vco_freq;
+
+	return 0;
+}
+
 static int set_module_rate(const struct s32cc_clk_obj *module,
 			   unsigned long rate, unsigned long *orate,
 			   unsigned int *depth)
@@ -263,9 +285,11 @@
 	case s32cc_osc_t:
 		ret = set_osc_freq(module, rate, orate, depth);
 		break;
+	case s32cc_pll_t:
+		ret = set_pll_freq(module, rate, orate, depth);
+		break;
 	case s32cc_clkmux_t:
 	case s32cc_shared_clkmux_t:
-	case s32cc_pll_t:
 	case s32cc_pll_out_div_t:
 		ret = -ENOTSUP;
 		break;
diff --git a/drivers/nxp/clk/s32cc/s32cc_early_clks.c b/drivers/nxp/clk/s32cc/s32cc_early_clks.c
index 98f30d8..7ccfc00 100644
--- a/drivers/nxp/clk/s32cc/s32cc_early_clks.c
+++ b/drivers/nxp/clk/s32cc/s32cc_early_clks.c
@@ -9,6 +9,7 @@
 #include <s32cc-clk-utils.h>
 
 #define S32CC_FXOSC_FREQ	(40U * MHZ)
+#define S32CC_ARM_PLL_VCO_FREQ	(2U * GHZ)
 
 int s32cc_init_early_clks(void)
 {
@@ -31,6 +32,11 @@
 		return ret;
 	}
 
+	ret = clk_set_rate(S32CC_CLK_ARM_PLL_VCO, S32CC_ARM_PLL_VCO_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 6ffe321..1fe2719 100644
--- a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
+++ b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
@@ -188,4 +188,12 @@
 	return s32cc_obj2clkmux(clk->module);
 }
 
+static inline struct s32cc_pll *s32cc_obj2pll(const struct s32cc_clk_obj *mod)
+{
+	uintptr_t pll_addr;
+
+	pll_addr = ((uintptr_t)mod) - offsetof(struct s32cc_pll, desc);
+	return (struct s32cc_pll *)pll_addr;
+}
+
 #endif /* S32CC_CLK_MODULES_H */