diff --git a/drivers/clk/clk-scmi.c b/drivers/clk/clk-scmi.c index 3f525bed9794..8cbe24789c24 100644 --- a/drivers/clk/clk-scmi.c +++ b/drivers/clk/clk-scmi.c @@ -24,6 +24,7 @@ struct scmi_clk { struct clk_hw hw; const struct scmi_clock_info *info; const struct scmi_protocol_handle *ph; + struct clk_parent_data *parent_data; }; #define to_scmi_clk(clk) container_of(clk, struct scmi_clk, hw) @@ -78,6 +79,43 @@ static int scmi_clk_set_rate(struct clk_hw *hw, unsigned long rate, return scmi_proto_clk_ops->rate_set(clk->ph, clk->id, rate); } +static int scmi_clk_set_parent(struct clk_hw *hw, u8 parent_index) +{ + struct scmi_clk *clk = to_scmi_clk(hw); + + return scmi_proto_clk_ops->parent_set(clk->ph, clk->id, parent_index); +} + +static u8 scmi_clk_get_parent(struct clk_hw *hw) +{ + struct scmi_clk *clk = to_scmi_clk(hw); + u32 parent_id, p_idx; + int ret; + + ret = scmi_proto_clk_ops->parent_get(clk->ph, clk->id, &parent_id); + if (ret) + return 0; + + for (p_idx = 0; p_idx < clk->info->num_parents; p_idx++) { + if (clk->parent_data[p_idx].index == parent_id) + break; + } + + if (p_idx == clk->info->num_parents) + return 0; + + return p_idx; +} + +static int scmi_clk_determine_rate(struct clk_hw *hw, struct clk_rate_request *req) +{ + /* + * Suppose all the requested rates are supported, and let firmware + * to handle the left work. + */ + return 0; +} + static int scmi_clk_enable(struct clk_hw *hw) { struct scmi_clk *clk = to_scmi_clk(hw); @@ -139,6 +177,9 @@ static const struct clk_ops scmi_clk_ops = { .set_rate = scmi_clk_set_rate, .prepare = scmi_clk_enable, .unprepare = scmi_clk_disable, + .set_parent = scmi_clk_set_parent, + .get_parent = scmi_clk_get_parent, + .determine_rate = scmi_clk_determine_rate, }; static const struct clk_ops scmi_atomic_clk_ops = { @@ -148,6 +189,9 @@ static const struct clk_ops scmi_atomic_clk_ops = { .enable = scmi_clk_atomic_enable, .disable = scmi_clk_atomic_disable, .is_enabled = scmi_clk_atomic_is_enabled, + .set_parent = scmi_clk_set_parent, + .get_parent = scmi_clk_get_parent, + .determine_rate = scmi_clk_determine_rate, }; static int scmi_clk_ops_init(struct device *dev, struct scmi_clk *sclk, @@ -158,9 +202,10 @@ static int scmi_clk_ops_init(struct device *dev, struct scmi_clk *sclk, struct clk_init_data init = { .flags = CLK_GET_RATE_NOCACHE, - .num_parents = 0, + .num_parents = sclk->info->num_parents, .ops = scmi_ops, .name = sclk->info->name, + .parent_data = sclk->parent_data, }; sclk->hw.init = &init; @@ -251,9 +296,23 @@ static int scmi_clocks_probe(struct scmi_device *sdev) else scmi_ops = &scmi_clk_ops; + /* Initialize clock parent data. */ + if (sclk->info->num_parents > 0) { + sclk->parent_data = devm_kcalloc(dev, sclk->info->num_parents, + sizeof(*sclk->parent_data), GFP_KERNEL); + if (!sclk->parent_data) + return -ENOMEM; + + for (int i = 0; i < sclk->info->num_parents; i++) { + sclk->parent_data[i].index = sclk->info->parents[i]; + sclk->parent_data[i].hw = hws[sclk->info->parents[i]]; + } + } + err = scmi_clk_ops_init(dev, sclk, scmi_ops); if (err) { dev_err(dev, "failed to register clock %d\n", idx); + devm_kfree(dev, sclk->parent_data); devm_kfree(dev, sclk); hws[idx] = NULL; } else {