clk: scmi: Add support for clock {set,get}_parent

SCMI v3.2 adds set/get parent clock commands, so update the SCMI clock
driver to support them.

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Link: https://lore.kernel.org/r/20231004-scmi-clock-v3-v5-2-1b8a1435673e@nxp.com
Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>

diff --git a/drivers/clk/clk-scmi.c b/drivers/clk/clk-scmi.c
index 3f525bed9794f963df017d322d5797c20a9b945c..8cbe24789c24bbb1db6213501fc2a21f45a49f40 100644 (file)
--- 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 {