clk: versal: Add clock driver support

[oweals/u-boot.git] / arch / arm / mach-versal / cpu.c

diff --git a/arch/arm/mach-versal/cpu.c b/arch/arm/mach-versal/cpu.c
index f0d047d3232f7e871fa8a9b45345bd012ccbe33d..db9622582748efdf4f5f9d93a2d1e95218752b58 100644 (file)
--- a/arch/arm/mach-versal/cpu.c
+++ b/arch/arm/mach-versal/cpu.c
@@ -121,3 +121,28 @@ void *board_fdt_blob_setup(void)
        return fw_dtb;
 }
 #endif
+
+int versal_pm_request(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
+                     u32 arg3, u32 *ret_payload)
+{
+       struct pt_regs regs;
+
+       if (current_el() == 3)
+               return 0;
+
+       regs.regs[0] = PM_SIP_SVC | api_id;
+       regs.regs[1] = ((u64)arg1 << 32) | arg0;
+       regs.regs[2] = ((u64)arg3 << 32) | arg2;
+
+       smc_call(&regs);
+
+       if (ret_payload) {
+               ret_payload[0] = (u32)regs.regs[0];
+               ret_payload[1] = upper_32_bits(regs.regs[0]);
+               ret_payload[2] = (u32)regs.regs[1];
+               ret_payload[3] = upper_32_bits(regs.regs[1]);
+               ret_payload[4] = (u32)regs.regs[2];
+       }
+
+       return regs.regs[0];
+}