Define routines of mmio write and read functionalities for zynqmp platform.
Signed-off-by: Siva Durga Prasad Paladugu sivadur@xilinx.com --- arch/arm/cpu/armv8/zynqmp/cpu.c | 51 ++++++++++++++++++++++++++++ arch/arm/include/asm/arch-zynqmp/sys_proto.h | 3 ++ 2 files changed, 54 insertions(+)
diff --git a/arch/arm/cpu/armv8/zynqmp/cpu.c b/arch/arm/cpu/armv8/zynqmp/cpu.c index b0f1295..54afa9e 100644 --- a/arch/arm/cpu/armv8/zynqmp/cpu.c +++ b/arch/arm/cpu/armv8/zynqmp/cpu.c @@ -104,3 +104,54 @@ unsigned int zynqmp_get_silicon_version(void)
return ZYNQMP_CSU_VERSION_SILICON; } + +#define PAYLOAD_ARG_CNT 5 +#define ZYNQMP_MMIO_READ 0xC2000014 +#define ZYNQMP_MMIO_WRITE 0xC2000013 + +static int invoke_smc(u32 pm_api_id, u32 arg0, u32 arg1, u32 arg2, u32 arg3, + u32 *ret_payload) +{ + /* + * Added SIP service call Function Identifier + * Make sure to stay in x0 register + */ + struct pt_regs regs; + + regs.regs[0] = pm_api_id; + regs.regs[1] = ((u64)arg1 << 32) | arg0; + regs.regs[2] = ((u64)arg3 << 32) | arg2; + + smc_call(®s); + + if (ret_payload != NULL) { + 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]; +} + +int zynqmp_mmio_write(const u32 address, + const u32 mask, + const u32 value) +{ + return invoke_smc(ZYNQMP_MMIO_WRITE, address, mask, value, 0, NULL); +} + +int zynqmp_mmio_read(const u32 address, u32 *value) +{ + u32 ret_payload[PAYLOAD_ARG_CNT]; + u32 ret; + + if (!value) + return -EINVAL; + + ret = invoke_smc(ZYNQMP_MMIO_READ, address, 0, 0, 0, ret_payload); + *value = ret_payload[1]; + + return ret; +} diff --git a/arch/arm/include/asm/arch-zynqmp/sys_proto.h b/arch/arm/include/asm/arch-zynqmp/sys_proto.h index 8c54fce..446e89c 100644 --- a/arch/arm/include/asm/arch-zynqmp/sys_proto.h +++ b/arch/arm/include/asm/arch-zynqmp/sys_proto.h @@ -23,4 +23,7 @@ void psu_init(void);
void handoff_setup(void);
+int zynqmp_mmio_write(const u32 address, const u32 mask, const u32 value); +int zynqmp_mmio_read(const u32 address, u32 *value); + #endif /* _ASM_ARCH_SYS_PROTO_H */