diff --git a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c index f076276a1..f35a4695d 100644 --- a/drivers/nxp/clk/s32cc/s32cc_clk_drv.c +++ b/drivers/nxp/clk/s32cc/s32cc_clk_drv.c @@ -316,6 +316,30 @@ static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long ra return 0; } +static int set_fixed_div_freq(const struct s32cc_clk_obj *module, unsigned long rate, + unsigned long *orate, unsigned int *depth) +{ + const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module); + int ret; + + ret = update_stack_depth(depth); + if (ret != 0) { + return ret; + } + + if (fdiv->parent == NULL) { + ERROR("The divider doesn't have a valid parent\b"); + return -EINVAL; + } + + ret = set_module_rate(fdiv->parent, rate * fdiv->rate_div, orate, depth); + + /* Update the output rate based on the parent's rate */ + *orate /= fdiv->rate_div; + + return ret; +} + static int set_module_rate(const struct s32cc_clk_obj *module, unsigned long rate, unsigned long *orate, unsigned int *depth) @@ -340,9 +364,11 @@ static int set_module_rate(const struct s32cc_clk_obj *module, case s32cc_pll_out_div_t: ret = set_pll_div_freq(module, rate, orate, depth); break; + case s32cc_fixed_div_t: + ret = set_fixed_div_freq(module, rate, orate, depth); + break; case s32cc_clkmux_t: case s32cc_shared_clkmux_t: - case s32cc_fixed_div_t: ret = -ENOTSUP; break; default: diff --git a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h index 694510e0b..41fc6f409 100644 --- a/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h +++ b/include/drivers/nxp/clk/s32cc/s32cc-clk-modules.h @@ -229,4 +229,12 @@ static inline struct s32cc_pll_out_div *s32cc_obj2plldiv(const struct s32cc_clk_ return (struct s32cc_pll_out_div *)plldiv_addr; } +static inline struct s32cc_fixed_div *s32cc_obj2fixeddiv(const struct s32cc_clk_obj *mod) +{ + uintptr_t fdiv_addr; + + fdiv_addr = ((uintptr_t)mod) - offsetof(struct s32cc_fixed_div, desc); + return (struct s32cc_fixed_div *)fdiv_addr; +} + #endif /* S32CC_CLK_MODULES_H */