2 files changed, 136 insertions, 1 deletions

diff --git a/arch/arm/mach-zynq/include/mach/ps7_init_gpl.h b/arch/arm/mach-zynq/include/mach/ps7_init_gpl.h
index 6e30108b23..c7c716e7f2 100644
--- a/arch/arm/mach-zynq/include/mach/ps7_init_gpl.h
+++ b/arch/arm/mach-zynq/include/mach/ps7_init_gpl.h
@@ -1,5 +1,6 @@
 /*
- * (c) Copyright 2010-2017 Xilinx, Inc. All rights reserved.
+ * (c) Copyright 2010-2014 Xilinx, Inc. All rights reserved.
+ * (c) Copyright 2016 Topic Embedded Products.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -7,8 +8,33 @@
 #ifndef _ASM_ARCH_PS7_INIT_GPL_H
 #define _ASM_ARCH_PS7_INIT_GPL_H
 
+/* Opcode exit is 0 all the time */
+#define OPCODE_EXIT		0U
+#define OPCODE_MASKWRITE	0U
+#define OPCODE_MASKPOLL		1U
+#define OPCODE_MASKDELAY	2U
+#define OPCODE_ADDRESS_MASK	(~3U)
+
+/* Sentinel */
+#define EMIT_EXIT()			OPCODE_EXIT
+/* Opcode is in lower 2 bits of address, address is always 4-byte aligned */
+#define EMIT_MASKWRITE(addr, mask, val)	OPCODE_MASKWRITE | addr, mask, val
+#define EMIT_MASKPOLL(addr, mask)	OPCODE_MASKPOLL | addr, mask
+#define EMIT_MASKDELAY(addr, mask)	OPCODE_MASKDELAY | addr, mask
+
+/* Returns codes of ps7_init* */
+#define PS7_INIT_SUCCESS		(0)
+#define PS7_INIT_CORRUPT		(1)
+#define PS7_INIT_TIMEOUT		(2)
+#define PS7_POLL_FAILED_DDR_INIT	(3)
+#define PS7_POLL_FAILED_DMA		(4)
+#define PS7_POLL_FAILED_PLL		(5)
+
 /* Called by spl.c */
 int ps7_init(void);
 int ps7_post_config(void);
 
+/* Defined in ps7_init_common.c */
+int ps7_config(unsigned long *ps7_config_init);
+
 #endif /* _ASM_ARCH_PS7_INIT_GPL_H */
diff --git a/arch/arm/mach-zynq/ps7_spl_init.c b/arch/arm/mach-zynq/ps7_spl_init.c
index 6adf852578..180099577b 100644
--- a/arch/arm/mach-zynq/ps7_spl_init.c
+++ b/arch/arm/mach-zynq/ps7_spl_init.c
@@ -1,5 +1,6 @@
 /*
  * (c) Copyright 2010-2017 Xilinx, Inc. All rights reserved.
+ * (c) Copyright 2016 Topic Embedded Products.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -25,3 +26,111 @@ __weak int ps7_post_config(void)
 	 */
 	return 0;
 }
+
+/* For delay calculation using global registers*/
+#define SCU_GLOBAL_TIMER_COUNT_L32	0xF8F00200
+#define SCU_GLOBAL_TIMER_COUNT_U32	0xF8F00204
+#define SCU_GLOBAL_TIMER_CONTROL	0xF8F00208
+#define SCU_GLOBAL_TIMER_AUTO_INC	0xF8F00218
+#define APU_FREQ  666666666
+
+#define PS7_MASK_POLL_TIME 100000000
+
+/* IO accessors. No memory barriers desired. */
+static inline void iowrite(unsigned long val, unsigned long addr)
+{
+	__raw_writel(val, addr);
+}
+
+static inline unsigned long ioread(unsigned long addr)
+{
+	return __raw_readl(addr);
+}
+
+/* start timer */
+static void perf_start_clock(void)
+{
+	iowrite((1 << 0) | /* Timer Enable */
+		(1 << 3) | /* Auto-increment */
+		(0 << 8), /* Pre-scale */
+		SCU_GLOBAL_TIMER_CONTROL);
+}
+
+/* Compute mask for given delay in miliseconds*/
+static int get_number_of_cycles_for_delay(unsigned int delay)
+{
+	return (APU_FREQ / (2 * 1000)) * delay;
+}
+
+/* stop timer */
+static void perf_disable_clock(void)
+{
+	iowrite(0, SCU_GLOBAL_TIMER_CONTROL);
+}
+
+/* stop timer and reset timer count regs */
+static void perf_reset_clock(void)
+{
+	perf_disable_clock();
+	iowrite(0, SCU_GLOBAL_TIMER_COUNT_L32);
+	iowrite(0, SCU_GLOBAL_TIMER_COUNT_U32);
+}
+
+static void perf_reset_and_start_timer(void)
+{
+	perf_reset_clock();
+	perf_start_clock();
+}
+
+int __weak ps7_config(unsigned long *ps7_config_init)
+{
+	unsigned long *ptr = ps7_config_init;
+	unsigned long opcode;
+	unsigned long addr;
+	unsigned long val;
+	unsigned long mask;
+	unsigned int numargs;
+	int i;
+	int delay;
+
+	for (;;) {
+		opcode = ptr[0];
+		if (opcode == OPCODE_EXIT)
+			return PS7_INIT_SUCCESS;
+		addr = (opcode & OPCODE_ADDRESS_MASK);
+
+		switch (opcode & ~OPCODE_ADDRESS_MASK) {
+		case OPCODE_MASKWRITE:
+			numargs = 3;
+			mask = ptr[1];
+			val = ptr[2];
+			iowrite((ioread(addr) & ~mask) | (val & mask), addr);
+			break;
+
+		case OPCODE_MASKPOLL:
+			numargs = 2;
+			mask = ptr[1];
+			i = 0;
+			while (!(ioread(addr) & mask)) {
+				if (i == PS7_MASK_POLL_TIME)
+					return PS7_INIT_TIMEOUT;
+				i++;
+			}
+			break;
+
+		case OPCODE_MASKDELAY:
+			numargs = 2;
+			mask = ptr[1];
+			delay = get_number_of_cycles_for_delay(mask);
+			perf_reset_and_start_timer();
+			while (ioread(addr) < delay)
+				;
+			break;
+
+		default:
+			return PS7_INIT_CORRUPT;
+		}
+
+		ptr += numargs;
+	}
+}