[U-Boot] [UBOOT v2 07/15] zynqmp: Define routines for mmio write and read

[Siva Durga Prasad Paladugu]

Define routines of mmio write and read functionalities
for zynqmp platform.

Signed-off-by: Siva Durga Prasad Paladugu <siva...@xilinx.com>
---
Changes from v1:
- None
---
 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(&regs);
+
+       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 */
-- 
2.7.4

_______________________________________________
U-Boot mailing list
U-Boot@lists.denx.de
http://lists.denx.de/mailman/listinfo/u-boot