feat(nxp-clk): implement set_rate for oscillators

The set_rate callback will now be applied to FIRC, FXOSC, and SIRC
oscillators. It is a prerequisite for the upcoming commits that will
utilize this capability.

Change-Id: I82d1545c63b3e15497c1c002ff9ec0d7bf990aa0
Signed-off-by: Ciprian Costea <ciprianmarian.costea@nxp.com>
Signed-off-by: Ghennadi Procopciuc <ghennadi.procopciuc@nxp.com>

drivers/nxp/clk/s32cc/s32cc_clk_drv.c

@@ -5,7 +5,22 @@
  */
 #include <errno.h>
 
+#include <common/debug.h>
 #include <drivers/clk.h>
+#include <s32cc-clk-modules.h>
+#include <s32cc-clk-utils.h>
+
+#define MAX_STACK_DEPTH		(15U)
+
+static int update_stack_depth(unsigned int *depth)
+{
+	if (*depth == 0U) {
+		return -ENOMEM;
+	}
+
+	(*depth)--;
+	return 0;
+}
 
 static int s32cc_clk_enable(unsigned long id)
 {
@@ -26,10 +41,107 @@ static unsigned long s32cc_clk_get_rate(unsigned long id)
 	return 0;
 }
 
+static int set_module_rate(const struct s32cc_clk_obj *module,
+			   unsigned long rate, unsigned long *orate,
+			   unsigned int *depth);
+
+static int set_osc_freq(const struct s32cc_clk_obj *module, unsigned long rate,
+			unsigned long *orate, unsigned int *depth)
+{
+	struct s32cc_osc *osc = s32cc_obj2osc(module);
+	int ret;
+
+	ret = update_stack_depth(depth);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((osc->freq != 0UL) && (rate != osc->freq)) {
+		ERROR("Already initialized oscillator. freq = %lu\n",
+		      osc->freq);
+		return -EINVAL;
+	}
+
+	osc->freq = rate;
+	*orate = osc->freq;
+
+	return 0;
+}
+
+static int set_clk_freq(const struct s32cc_clk_obj *module, unsigned long rate,
+			unsigned long *orate, unsigned int *depth)
+{
+	const struct s32cc_clk *clk = s32cc_obj2clk(module);
+	int ret;
+
+	ret = update_stack_depth(depth);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((clk->min_freq != 0UL) && (clk->max_freq != 0UL) &&
+	    ((rate < clk->min_freq) || (rate > clk->max_freq))) {
+		ERROR("%lu frequency is out of the allowed range: [%lu:%lu]\n",
+		      rate, clk->min_freq, clk->max_freq);
+		return -EINVAL;
+	}
+
+	if (clk->module != NULL) {
+		return set_module_rate(clk->module, rate, orate, depth);
+	}
+
+	if (clk->pclock != NULL) {
+		return set_clk_freq(&clk->pclock->desc, rate, orate, depth);
+	}
+
+	return -EINVAL;
+}
+
+static int set_module_rate(const struct s32cc_clk_obj *module,
+			   unsigned long rate, unsigned long *orate,
+			   unsigned int *depth)
+{
+	int ret = 0;
+
+	ret = update_stack_depth(depth);
+	if (ret != 0) {
+		return ret;
+	}
+
+	switch (module->type) {
+	case s32cc_clk_t:
+		ret = set_clk_freq(module, rate, orate, depth);
+		break;
+	case s32cc_osc_t:
+		ret = set_osc_freq(module, rate, orate, depth);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 static int s32cc_clk_set_rate(unsigned long id, unsigned long rate,
 			      unsigned long *orate)
 {
-	return -ENOTSUP;
+	unsigned int depth = MAX_STACK_DEPTH;
+	const struct s32cc_clk *clk;
+	int ret;
+
+	clk = s32cc_get_arch_clk(id);
+	if (clk == NULL) {
+		return -EINVAL;
+	}
+
+	ret = set_module_rate(&clk->desc, rate, orate, &depth);
+	if (ret != 0) {
+		ERROR("Failed to set frequency (%lu MHz) for clock %lu\n",
+		      rate, id);
+	}
+
+	return ret;
 }
 
 static int s32cc_clk_get_parent(unsigned long id)

include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h

@@ -72,4 +72,20 @@ struct s32cc_clk_array {
 #define S32CC_MODULE_CLK(PARENT_MODULE) \
 	S32CC_FREQ_MODULE_CLK(PARENT_MODULE, 0, 0)
 
+static inline struct s32cc_osc *s32cc_obj2osc(const struct s32cc_clk_obj *mod)
+{
+	uintptr_t osc_addr;
+
+	osc_addr = ((uintptr_t)mod) - offsetof(struct s32cc_osc, desc);
+	return (struct s32cc_osc *)osc_addr;
+}
+
+static inline struct s32cc_clk *s32cc_obj2clk(const struct s32cc_clk_obj *mod)
+{
+	uintptr_t clk_addr;
+
+	clk_addr = ((uintptr_t)mod) - offsetof(struct s32cc_clk, desc);
+	return (struct s32cc_clk *)clk_addr;
+}
+
 #endif /* S32CC_CLK_MODULES_H */