This adds support for getting/setting the clk parents in the SCMI clk
driver.
Signed-off-by: Sascha Hauer <s.ha...@pengutronix.de>
---
drivers/clk/clk-scmi.c | 80 +++++++++++++++++++++++++++++++++++++++++++++-----
1 file changed, 72 insertions(+), 8 deletions(-)
diff --git a/drivers/clk/clk-scmi.c b/drivers/clk/clk-scmi.c
index
5c9f61ae0b6f94da104cda682e8b399fc4f9c84e..62228d7ba2c12b6bf4ae41dff4aa4553fd3e325e
100644
--- a/drivers/clk/clk-scmi.c
+++ b/drivers/clk/clk-scmi.c
@@ -20,6 +20,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)
@@ -74,6 +75,34 @@ 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 int 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_enable(struct clk_hw *hw)
{
struct scmi_clk *clk = to_scmi_clk(hw);
@@ -121,6 +150,8 @@ static const struct clk_ops scmi_clk_ops = {
.set_rate = scmi_clk_set_rate,
.enable = scmi_clk_enable,
.disable = scmi_clk_disable,
+ .set_parent = scmi_clk_set_parent,
+ .get_parent = scmi_clk_get_parent,
};
static const struct clk_ops scmi_atomic_clk_ops = {
@@ -129,6 +160,8 @@ static const struct clk_ops scmi_atomic_clk_ops = {
.set_rate = scmi_clk_set_rate,
.enable = scmi_clk_atomic_enable,
.disable = scmi_clk_atomic_disable,
+ .set_parent = scmi_clk_set_parent,
+ .get_parent = scmi_clk_get_parent,
};
static int scmi_clk_ops_init(struct device *dev, struct scmi_clk *sclk,
@@ -136,12 +169,22 @@ 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,
};
+ if (sclk->info->num_parents > 0) {
+ init.parent_hws = devm_kcalloc(dev, sclk->info->num_parents,
+ sizeof(void *), GFP_KERNEL);
+ if (!init.parent_hws)
+ return -ENOMEM;
+
+ for (int i = 0; i < sclk->info->num_parents; i++) {
+ init.parent_hws[i] = sclk->parent_data[i].hw;
+ }
+ }
+
sclk->hw.init = &init;
return clk_hw_register(dev, &sclk->hw);
}
@@ -157,6 +200,8 @@ static int scmi_clocks_probe(struct scmi_device *sdev)
struct device_node *np = dev->of_node;
const struct scmi_handle *handle = sdev->handle;
struct scmi_protocol_handle *ph;
+ struct scmi_clk *sclks;
+ int ret;
if (!handle)
return -ENODEV;
@@ -182,13 +227,17 @@ static int scmi_clocks_probe(struct scmi_device *sdev)
is_atomic = handle->is_transport_atomic(handle, &atomic_threshold);
+ sclks = devm_kzalloc(dev, sizeof(*sclks) * count, GFP_KERNEL);
+
for (idx = 0; idx < count; idx++) {
- struct scmi_clk *sclk;
- const struct clk_ops *scmi_ops;
+ struct scmi_clk *sclk = &sclks[idx];
- sclk = devm_kzalloc(dev, sizeof(*sclk), GFP_KERNEL);
- if (!sclk)
- return -ENOMEM;
+ hws[idx] = &sclk->hw;
+ }
+
+ for (idx = 0; idx < count; idx++) {
+ struct scmi_clk *sclk = &sclks[idx];
+ const struct clk_ops *scmi_ops;
sclk->info = scmi_proto_clk_ops->info_get(ph, idx);
if (!sclk->info) {
@@ -210,9 +259,25 @@ 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) {
+ u32 parent_id;
+
+ 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 {
@@ -220,7 +285,6 @@ static int scmi_clocks_probe(struct scmi_device *sdev)
sclk->info->name,
scmi_ops == &scmi_atomic_clk_ops ?
" (atomic ops)" : "");
- hws[idx] = &sclk->hw;
}
}
--
2.39.5
