feat(nxp-clk): add set_parent callback
On S32CC SoCs, the set_parent operation will be used on clock modules
that are mux instances in order to establish the clock source. This will
be used for PLLs and MC_CGM muxes.
Change-Id: I7228d379500ea790459b858da8fc0bdcbed4fd62
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 c15b033..dd81d66 100644
--- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
+++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c
@@ -301,7 +301,49 @@
static int s32cc_clk_set_parent(unsigned long id, unsigned long parent_id)
{
- return -ENOTSUP;
+ const struct s32cc_clk *parent;
+ const struct s32cc_clk *clk;
+ bool valid_source = false;
+ struct s32cc_clkmux *mux;
+ uint8_t i;
+
+ clk = s32cc_get_arch_clk(id);
+ if (clk == NULL) {
+ return -EINVAL;
+ }
+
+ parent = s32cc_get_arch_clk(parent_id);
+ if (parent == NULL) {
+ return -EINVAL;
+ }
+
+ if (!is_s32cc_clk_mux(clk)) {
+ ERROR("Clock %lu is not a mux\n", id);
+ return -EINVAL;
+ }
+
+ mux = s32cc_clk2mux(clk);
+ if (mux == NULL) {
+ ERROR("Failed to cast clock %lu to clock mux\n", id);
+ return -EINVAL;
+ }
+
+ for (i = 0; i < mux->nclks; i++) {
+ if (mux->clkids[i] == parent_id) {
+ valid_source = true;
+ break;
+ }
+ }
+
+ if (!valid_source) {
+ ERROR("Clock %lu is not a valid clock for mux %lu\n",
+ parent_id, id);
+ return -EINVAL;
+ }
+
+ mux->source_id = parent_id;
+
+ return 0;
}
void s32cc_clk_register_drv(void)
diff --git a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
index 648af63..23a611c 100644
--- a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
+++ b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h
@@ -6,6 +6,7 @@
#define S32CC_CLK_MODULES_H
#include <inttypes.h>
+#include <stdbool.h>
#include <stddef.h>
#define MHZ UL(1000000)
@@ -151,4 +152,33 @@
return (struct s32cc_clk *)clk_addr;
}
+static inline bool is_s32cc_clk_mux(const struct s32cc_clk *clk)
+{
+ const struct s32cc_clk_obj *module;
+
+ module = clk->module;
+ if (module == NULL) {
+ return false;
+ }
+
+ return (module->type == s32cc_clkmux_t);
+}
+
+static inline struct s32cc_clkmux *s32cc_obj2clkmux(const struct s32cc_clk_obj *mod)
+{
+ uintptr_t cmux_addr;
+
+ cmux_addr = ((uintptr_t)mod) - offsetof(struct s32cc_clkmux, desc);
+ return (struct s32cc_clkmux *)cmux_addr;
+}
+
+static inline struct s32cc_clkmux *s32cc_clk2mux(const struct s32cc_clk *clk)
+{
+ if (!is_s32cc_clk_mux(clk)) {
+ return NULL;
+ }
+
+ return s32cc_obj2clkmux(clk->module);
+}
+
#endif /* S32CC_CLK_MODULES_H */