[PATCH 4/4] clk: scmi: Add workaround for set_rate/enable/disable

[alice.guo at oss.nxp.com]

From: Ye Li <ye.li at nxp.com>

Add workaround to set_rate/enable/disable to bus clock that SM
will reply DENIED error.

Signed-off-by: Ye Li <ye.li at nxp.com>
Signed-off-by: Alice Guo <alice.guo at nxp.com>
Reviewed-by: Peng Fan <peng.fan at nxp.com>
---
 drivers/clk/clk_scmi.c | 21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/drivers/clk/clk_scmi.c b/drivers/clk/clk_scmi.c
index a01292c479..a860a653ba 100644
--- a/drivers/clk/clk_scmi.c
+++ b/drivers/clk/clk_scmi.c
@@ -73,7 +73,13 @@ static int scmi_clk_gate(struct clk *clk, int enable)
 	if (ret)
 		return ret;
 
-	return scmi_to_linux_errno(out.status);
+	ret = scmi_to_linux_errno(out.status);
+	if (ret == -EACCES) {
+		debug("Ignore %s enable failure\n", clk_hw_get_name(clk));
+		ret = 0;
+	}
+
+	return ret;
 }
 
 static int scmi_clk_enable(struct clk *clk)
@@ -108,7 +114,7 @@ static ulong scmi_clk_get_rate(struct clk *clk)
 	return (ulong)(((u64)out.rate_msb << 32) | out.rate_lsb);
 }
 
-static ulong scmi_clk_set_rate(struct clk *clk, ulong rate)
+static ulong __scmi_clk_set_rate(struct clk *clk, ulong rate)
 {
 	struct scmi_clk_rate_set_in in = {
 		.clock_id = clk->id,
@@ -133,6 +139,17 @@ static ulong scmi_clk_set_rate(struct clk *clk, ulong rate)
 	return scmi_clk_get_rate(clk);
 }
 
+static ulong scmi_clk_set_rate(struct clk *clk, ulong rate)
+{
+	ulong orig_rate;
+
+	orig_rate = scmi_clk_get_rate(clk);
+	if (orig_rate == rate)
+		return orig_rate;
+
+	return __scmi_clk_set_rate(clk, rate);
+}
+
 static int scmi_clk_probe(struct udevice *dev)
 {
 	struct clk *clk;
-- 
2.34.1