From e0752bc18422ec0dc93a770e96b1c2f44ee64ed7 Mon Sep 17 00:00:00 2001
From: Siva Durga Prasad Paladugu <siva.durga.paladugu@xilinx.com>
Date: Thu, 2 Feb 2017 01:10:46 +0530
Subject: [PATCH] arm64: zynqmp: Define routines for mmio write and read

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

Also do not call SMC from SPL because SPL is running before ATF in EL3
that's why SMCs can't be called because there is nothing to call.
zynqmp_mmio*() are doing direct read/write accesses and this patch does
the same. PMUFW is up and running at this time and there is a way to talk
to pmufw via IPI but there is no reason to implement IPI stuff in SPL if
we need just simple read for getting clock driver to work.

Also make invoke_smc as global so that it can be reused in
multile places where ever possible.

Signed-off-by: Siva Durga Prasad Paladugu <sivadur@xilinx.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>
---
 arch/arm/cpu/armv8/zynqmp/cpu.c              | 73 ++++++++++++++++++++++++++++
 arch/arm/include/asm/arch-zynqmp/sys_proto.h |  7 +++
 2 files changed, 80 insertions(+)

diff --git a/arch/arm/cpu/armv8/zynqmp/cpu.c b/arch/arm/cpu/armv8/zynqmp/cpu.c
index 280e07a..f7ed179 100644
--- a/arch/arm/cpu/armv8/zynqmp/cpu.c
+++ b/arch/arm/cpu/armv8/zynqmp/cpu.c
@@ -98,3 +98,76 @@ unsigned int zynqmp_get_silicon_version(void)
 
 	return ZYNQMP_CSU_VERSION_SILICON;
 }
+
+#define ZYNQMP_MMIO_READ	0xC2000014
+#define ZYNQMP_MMIO_WRITE	0xC2000013
+
+#ifndef CONFIG_SPL_BUILD
+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;
+}
+#else
+int zynqmp_mmio_write(const u32 address,
+		      const u32 mask,
+		      const u32 value)
+{
+	u32 data;
+	u32 value_local = value;
+
+	zynqmp_mmio_read(address, &data);
+	data &= ~mask;
+	value_local &= mask;
+	value_local |= data;
+	writel(value_local, (ulong)address);
+	return 0;
+}
+
+int zynqmp_mmio_read(const u32 address, u32 *value)
+{
+	*value = readl((ulong)address);
+	return 0;
+}
+#endif
diff --git a/arch/arm/include/asm/arch-zynqmp/sys_proto.h b/arch/arm/include/asm/arch-zynqmp/sys_proto.h
index 7b11895..4ae1bb6 100644
--- a/arch/arm/include/asm/arch-zynqmp/sys_proto.h
+++ b/arch/arm/include/asm/arch-zynqmp/sys_proto.h
@@ -8,6 +8,8 @@
 #ifndef _ASM_ARCH_SYS_PROTO_H
 #define _ASM_ARCH_SYS_PROTO_H
 
+#define PAYLOAD_ARG_CNT		5
+
 int zynq_slcr_get_mio_pin_status(const char *periph);
 
 unsigned int zynqmp_get_silicon_version(void);
@@ -16,4 +18,9 @@ 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);
+int invoke_smc(u32 pm_api_id, u32 arg0, u32 arg1, u32 arg2, u32 arg3,
+	       u32 *ret_payload);
+
 #endif /* _ASM_ARCH_SYS_PROTO_H */