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 <[email protected]> --- drivers/firmware/firmware-zynqmp.c | 34 ++++++++++++++++++++++++++++++ include/zynqmp_firmware.h | 6 ++++++ 2 files changed, 40 insertions(+) diff --git a/drivers/firmware/firmware-zynqmp.c b/drivers/firmware/firmware-zynqmp.c index 3742467caee1..2ef499bf4084 100644 --- 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(®s); + + 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 7f93241b1932..72f6459b91c8 100644 --- 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_ */ -- 2.43.0 base-commit: f109829793900e57558d98ed22caf80c1a72b232
