[PATCH v7 16/22] riscv: Enable cpu clock if it is present

[Pragnesh Patel]

The cpu clock is probably already enabled if we are executing code (though
we could be executing from a different core). This patch prevents the cpu
clock or its parents from being disabled.

Signed-off-by: Sean Anderson <sean...@gmail.com>
Signed-off-by: Pragnesh Patel <pragnesh.pa...@sifive.com>
Reviewed-by: Bin Meng <bmeng...@gmail.com>
---
 drivers/cpu/riscv_cpu.c | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/drivers/cpu/riscv_cpu.c b/drivers/cpu/riscv_cpu.c
index 28ad0aa30f..8ebe0341fd 100644
--- a/drivers/cpu/riscv_cpu.c
+++ b/drivers/cpu/riscv_cpu.c
@@ -9,6 +9,7 @@
 #include <errno.h>
 #include <dm/device-internal.h>
 #include <dm/lists.h>
+#include <clk.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -100,6 +101,37 @@ static int riscv_cpu_bind(struct udevice *dev)
        return 0;
 }
 
+static int riscv_cpu_probe(struct udevice *dev)
+{
+       int ret = 0;
+       u32 clock = 0;
+       struct clk clk;
+
+       /* Get a clock if it exists */
+       ret = clk_get_by_index(dev, 0, &clk);
+       if (ret)
+               return 0;
+
+       ret = dev_read_u32(dev, "clock-frequency", &clock);
+       if (ret) {
+               debug("clock-frequency not found in dt %d\n", ret);
+               return ret;
+       } else {
+               ret = clk_set_rate(&clk, clock);
+               if (ret < 0) {
+                       debug("Could not set CPU clock\n");
+                       return ret;
+               }
+       }
+
+       ret = clk_enable(&clk);
+       clk_free(&clk);
+       if (ret == -ENOSYS || ret == -ENOTSUPP)
+               return 0;
+       else
+               return ret;
+}
+
 static const struct cpu_ops riscv_cpu_ops = {
        .get_desc       = riscv_cpu_get_desc,
        .get_info       = riscv_cpu_get_info,
@@ -116,6 +148,7 @@ U_BOOT_DRIVER(riscv_cpu) = {
        .id = UCLASS_CPU,
        .of_match = riscv_cpu_ids,
        .bind = riscv_cpu_bind,
+       .probe = riscv_cpu_probe,
        .ops = &riscv_cpu_ops,
        .flags = DM_FLAG_PRE_RELOC,
 };
-- 
2.17.1