firmware: xilinx: Add support for enhancement SMC format

Versal Gen 2 is using different SMC format that's why firmware and clock
drivers needs to be align with it.

Signed-off-by: Michal Simek <michal.simek@amd.com>
Link: https://lore.kernel.org/r/16bdee56fd75113c6d531bae7a8a34900b10280d.1762788250.git.michal.simek@amd.com

diff --git a/drivers/firmware/firmware-zynqmp.c b/drivers/firmware/firmware-zynqmp.c
index 3742467caee1415cef09e99f2b408ecf7c73bebb..2ef499bf4084c1580b9c5c649c511a8411d1f55d 100644 (file)
--- a/drivers/firmware/firmware-zynqmp.c
+++ b/drivers/firmware/firmware-zynqmp.c
@@ -16,6 +16,7 @@
 #include <zynqmp_firmware.h>
 #include <asm/cache.h>
 #include <asm/ptrace.h>
+#include <linux/bitfield.h>
 
 #if defined(CONFIG_ZYNQMP_IPI)
 #include <mailbox.h>
@@ -451,6 +452,38 @@ static int smc_call_legacy(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
        return (ret_payload) ? ret_payload[0] : 0;
 }
 
+static int smc_call_enhanced(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
+                            u32 arg3, u32 arg4, u32 arg5, u32 *ret_payload)
+{
+       struct pt_regs regs;
+       u32 module_id = FIELD_GET(PLM_MODULE_ID_MASK, api_id);
+
+       if (module_id == 0)
+               module_id = PM_MODULE_ID;
+
+       regs.regs[0] = PM_SIP_SVC | PASS_THROUGH_FW_CMD_ID;
+       regs.regs[1] = ((u64)arg0 << 32U) |
+                       FIELD_PREP(PLM_MODULE_ID_MASK, module_id) |
+                       (api_id & API_ID_MASK);
+       regs.regs[2] = arg1 | ((u64)arg2 << 32);
+       regs.regs[3] = arg3 | ((u64)arg4 << 32);
+       regs.regs[4] = arg5;
+
+       smc_call(&regs);
+
+       if (ret_payload) {
+               ret_payload[0] = 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];
+               ret_payload[5] = upper_32_bits((u32)regs.regs[2]);
+               ret_payload[6] = (u32)regs.regs[3];
+       }
+
+       return regs.regs[0];
+}
+
 int __maybe_unused xilinx_pm_request(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
                                     u32 arg3, u32 arg4, u32 arg5, u32 *ret_payload)
 {
@@ -494,6 +527,7 @@ static const struct udevice_id zynqmp_firmware_ids[] = {
        { .compatible = "xlnx,zynqmp-firmware", .data = (ulong)smc_call_legacy },
        { .compatible = "xlnx,versal-firmware", .data = (ulong)smc_call_legacy},
        { .compatible = "xlnx,versal-net-firmware", .data = (ulong)smc_call_legacy },
+       { .compatible = "xlnx,versal2-firmware", .data = (ulong)smc_call_enhanced},
        { }
 };
 

diff --git a/include/zynqmp_firmware.h b/include/zynqmp_firmware.h
index 7f93241b19323f0ad914c7efa82ff34676119b6e..72f6459b91c86809a1a54ca07ac7d26ce5fe8302 100644 (file)
--- a/include/zynqmp_firmware.h
+++ b/include/zynqmp_firmware.h
@@ -521,4 +521,10 @@ typedef int (*smc_call_handler_t)(u32 api_id, u32 arg0, u32 arg1, u32 arg2,
 
 extern smc_call_handler_t __data smc_call_handler;
 
+#define PM_MODULE_ID           2
+
+#define PASS_THROUGH_FW_CMD_ID GENMASK(11, 0)
+#define PLM_MODULE_ID_MASK     GENMASK(15, 8)
+#define API_ID_MASK            GENMASK(7, 0)
+
 #endif /* _ZYNQMP_FIRMWARE_H_ */