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>
diff --git a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
index 8453000..e6653bd 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/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 @@
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)
diff --git a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
index 1047bff..9524f72 100644
--- a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
+++ b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
@@ -72,4 +72,20 @@
#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 */