arm64: zynqmp: Move SoC sources to mach-zynqmp

Similar changes was done for Zynq in past and this patch just follow
this pattern to separate cpu code from SoC code.

Move arch/arm/cpu/armv8/zynqmp/* -> arch/arm/mach-zynqmp/*
And also fix references to these files.

Based on
"ARM: zynq: move SoC sources to mach-zynq"
(sha1: 0107f2403669f764ab726d0d404e35bb9447bbcc)

Signed-off-by: Michal Simek <michal.simek@xilinx.com>

13 files changed, 1349 insertions, 0 deletions

diff --git a/arch/arm/mach-zynqmp/Kconfig b/arch/arm/mach-zynqmp/Kconfig
new file mode 100644
index 0000000000..8a311e1c89
--- /dev/null
+++ b/arch/arm/mach-zynqmp/Kconfig
@@ -0,0 +1,156 @@
+if ARCH_ZYNQMP
+
+config SPL_FAT_SUPPORT
+	default y
+
+config SPL_LIBCOMMON_SUPPORT
+	default y
+
+config SPL_LIBDISK_SUPPORT
+	default y
+
+config SPL_LIBGENERIC_SUPPORT
+	default y
+
+config SPL_MMC_SUPPORT
+	default y if MMC_SDHCI_ZYNQ
+
+config SPL_SERIAL_SUPPORT
+	default y
+
+config SPL_SPI_FLASH_SUPPORT
+	default y if ZYNQ_QSPI
+
+config SPL_SPI_SUPPORT
+	default y if ZYNQ_QSPI
+
+config SYS_BOARD
+	default "zynqmp"
+
+config SYS_VENDOR
+	string "Vendor name"
+	default "xilinx"
+
+config SYS_SOC
+	default "zynqmp"
+
+config SYS_CONFIG_NAME
+	string "Board configuration name"
+	default "xilinx_zynqmp"
+	help
+	  This option contains information about board configuration name.
+	  Based on this option include/configs/<CONFIG_SYS_CONFIG_NAME>.h header
+	  will be used for board configuration.
+
+config SYS_MEM_RSVD_FOR_MMU
+	bool "Reserve memory for MMU Table"
+	help
+	  If defined this option is used to setup different space for
+	  MMU table than the one which will be allocated during
+	  relocation.
+
+config BOOT_INIT_FILE
+	string "boot.bin init register filename"
+	depends on SPL
+	default ""
+	help
+	  Add register writes to boot.bin format (max 256 pairs).
+	  Expect a table of register-value pairs, e.g. "0x12345678 0x4321"
+
+config PMUFW_INIT_FILE
+	string "PMU firmware"
+	depends on SPL
+	default ""
+	help
+	  Include external PMUFW (Platform Management Unit FirmWare) to
+	  a Xilinx bootable image (boot.bin).
+
+config ZYNQMP_USB
+	bool "Configure ZynqMP USB"
+
+config ZYNQMP_NO_DDR
+	bool "Disable DDR MMU mapping"
+	help
+	  This option configures MMU with no DDR to avoid speculative
+	  access to DDR memory where DDR is not present.
+
+config SYS_MALLOC_F_LEN
+	default 0x600
+
+config DEFINE_TCM_OCM_MMAP
+	bool "Define TCM and OCM memory in MMU Table"
+	default y if MP
+	help
+	  This option if enabled defines the TCM and OCM memory and its
+	  memory attributes in MMU table entry.
+
+config ZYNQMP_PSU_INIT_ENABLED
+	bool "Include psu_init"
+	help
+	  Include psu_init to full u-boot. SPL include psu_init by default.
+
+config SPL_ZYNQMP_ALT_BOOTMODE_ENABLED
+	bool "Overwrite SPL bootmode"
+	depends on SPL
+	help
+	  Overwrite bootmode selected via boot mode pins to tell SPL what should
+	  be the next boot device.
+
+config ZYNQ_SDHCI_MAX_FREQ
+	default 200000000
+
+config SPL_ZYNQMP_ALT_BOOTMODE
+	hex
+	default 0x0 if JTAG_MODE
+	default 0x1 if QSPI_MODE_24BIT
+	default 0x2 if QSPI_MODE_32BIT
+	default 0x3 if SD_MODE
+	default 0x4 if NAND_MODE
+	default 0x5 if SD_MODE1
+	default 0x6 if EMMC_MODE
+	default 0x7 if USB_MODE
+	default 0xa if SW_USBHOST_MODE
+	default 0xb if SW_SATA_MODE
+	default 0xe if SD1_LSHFT_MODE
+
+choice
+	prompt "Boot mode"
+	depends on SPL_ZYNQMP_ALT_BOOTMODE_ENABLED
+	default JTAG_MODE
+
+config JTAG_MODE
+	bool "JTAG_MODE"
+
+config QSPI_MODE_24BIT
+	bool "QSPI_MODE_24BIT"
+
+config QSPI_MODE_32BIT
+	bool "QSPI_MODE_32BIT"
+
+config SD_MODE
+	bool "SD_MODE"
+
+config SD_MODE1
+	bool "SD_MODE1"
+
+config NAND_MODE
+	bool "NAND_MODE"
+
+config EMMC_MODE
+	bool "EMMC_MODE"
+
+config USB_MODE
+	bool "USB"
+
+config SW_USBHOST_MODE
+	bool "SW USBHOST_MODE"
+
+config SW_SATA_MODE
+	bool "SW SATA_MODE"
+
+config SD1_LSHFT_MODE
+	bool "SD1_LSHFT_MODE"
+
+endchoice
+
+endif
diff --git a/arch/arm/mach-zynqmp/Makefile b/arch/arm/mach-zynqmp/Makefile
new file mode 100644
index 0000000000..8a3b074724
--- /dev/null
+++ b/arch/arm/mach-zynqmp/Makefile
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0+
+#
+# (C) Copyright 2014 - 2015 Xilinx, Inc.
+# Michal Simek <michal.simek@xilinx.com>
+
+obj-y	+= clk.o
+obj-y	+= cpu.o
+obj-$(CONFIG_MP)	+= mp.o
+obj-$(CONFIG_SPL_BUILD) += spl.o handoff.o
+obj-$(CONFIG_ZYNQMP_PSU_INIT_ENABLED)	+= psu_spl_init.o
diff --git a/arch/arm/mach-zynqmp/clk.c b/arch/arm/mach-zynqmp/clk.c
new file mode 100644
index 0000000000..0593b6310f
--- /dev/null
+++ b/arch/arm/mach-zynqmp/clk.c
@@ -0,0 +1,45 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#include <common.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+unsigned long zynqmp_get_system_timer_freq(void)
+{
+	u32 ver = zynqmp_get_silicon_version();
+
+	switch (ver) {
+	case ZYNQMP_CSU_VERSION_QEMU:
+		return 50000000;
+	}
+
+	return 100000000;
+}
+
+#ifdef CONFIG_CLOCKS
+/**
+ * set_cpu_clk_info() - Initialize clock framework
+ * Always returns zero.
+ *
+ * This function is called from common code after relocation and sets up the
+ * clock framework. The framework must not be used before this function had been
+ * called.
+ */
+int set_cpu_clk_info(void)
+{
+	gd->cpu_clk = get_tbclk();
+
+	gd->bd->bi_arm_freq = gd->cpu_clk / 1000000;
+
+	gd->bd->bi_dsp_freq = 0;
+
+	return 0;
+}
+#endif
diff --git a/arch/arm/mach-zynqmp/cpu.c b/arch/arm/mach-zynqmp/cpu.c
new file mode 100644
index 0000000000..5ef1a52862
--- /dev/null
+++ b/arch/arm/mach-zynqmp/cpu.c
@@ -0,0 +1,260 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#include <common.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/armv8/mmu.h>
+#include <asm/io.h>
+
+#define ZYNQ_SILICON_VER_MASK	0xF000
+#define ZYNQ_SILICON_VER_SHIFT	12
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/*
+ * Number of filled static entries and also the first empty
+ * slot in zynqmp_mem_map.
+ */
+#define ZYNQMP_MEM_MAP_USED	4
+
+#if !defined(CONFIG_ZYNQMP_NO_DDR)
+#define DRAM_BANKS CONFIG_NR_DRAM_BANKS
+#else
+#define DRAM_BANKS 0
+#endif
+
+#if defined(CONFIG_DEFINE_TCM_OCM_MMAP)
+#define TCM_MAP 1
+#else
+#define TCM_MAP 0
+#endif
+
+/* +1 is end of list which needs to be empty */
+#define ZYNQMP_MEM_MAP_MAX (ZYNQMP_MEM_MAP_USED + DRAM_BANKS + TCM_MAP + 1)
+
+static struct mm_region zynqmp_mem_map[ZYNQMP_MEM_MAP_MAX] = {
+	{
+		.virt = 0x80000000UL,
+		.phys = 0x80000000UL,
+		.size = 0x70000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.virt = 0xf8000000UL,
+		.phys = 0xf8000000UL,
+		.size = 0x07e00000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.virt = 0x400000000UL,
+		.phys = 0x400000000UL,
+		.size = 0x400000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.virt = 0x1000000000UL,
+		.phys = 0x1000000000UL,
+		.size = 0xf000000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}
+};
+
+void mem_map_fill(void)
+{
+	int banks = ZYNQMP_MEM_MAP_USED;
+
+#if defined(CONFIG_DEFINE_TCM_OCM_MMAP)
+	zynqmp_mem_map[banks].virt = 0xffe00000UL;
+	zynqmp_mem_map[banks].phys = 0xffe00000UL;
+	zynqmp_mem_map[banks].size = 0x00200000UL;
+	zynqmp_mem_map[banks].attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+				      PTE_BLOCK_INNER_SHARE;
+	banks = banks + 1;
+#endif
+
+#if !defined(CONFIG_ZYNQMP_NO_DDR)
+	for (int i = 0; i < CONFIG_NR_DRAM_BANKS; i++) {
+		/* Zero size means no more DDR that's this is end */
+		if (!gd->bd->bi_dram[i].size)
+			break;
+
+		zynqmp_mem_map[banks].virt = gd->bd->bi_dram[i].start;
+		zynqmp_mem_map[banks].phys = gd->bd->bi_dram[i].start;
+		zynqmp_mem_map[banks].size = gd->bd->bi_dram[i].size;
+		zynqmp_mem_map[banks].attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+					      PTE_BLOCK_INNER_SHARE;
+		banks = banks + 1;
+	}
+#endif
+}
+
+struct mm_region *mem_map = zynqmp_mem_map;
+
+u64 get_page_table_size(void)
+{
+	return 0x14000;
+}
+
+#if defined(CONFIG_SYS_MEM_RSVD_FOR_MMU) || defined(CONFIG_DEFINE_TCM_OCM_MMAP)
+void tcm_init(u8 mode)
+{
+	puts("WARNING: Initializing TCM overwrites TCM content\n");
+	initialize_tcm(mode);
+	memset((void *)ZYNQMP_TCM_BASE_ADDR, 0, ZYNQMP_TCM_SIZE);
+}
+#endif
+
+#ifdef CONFIG_SYS_MEM_RSVD_FOR_MMU
+int reserve_mmu(void)
+{
+	tcm_init(TCM_LOCK);
+	gd->arch.tlb_size = PGTABLE_SIZE;
+	gd->arch.tlb_addr = ZYNQMP_TCM_BASE_ADDR;
+
+	return 0;
+}
+#endif
+
+static unsigned int zynqmp_get_silicon_version_secure(void)
+{
+	u32 ver;
+
+	ver = readl(&csu_base->version);
+	ver &= ZYNQMP_SILICON_VER_MASK;
+	ver >>= ZYNQMP_SILICON_VER_SHIFT;
+
+	return ver;
+}
+
+unsigned int zynqmp_get_silicon_version(void)
+{
+	if (current_el() == 3)
+		return zynqmp_get_silicon_version_secure();
+
+	gd->cpu_clk = get_tbclk();
+
+	switch (gd->cpu_clk) {
+	case 50000000:
+		return ZYNQMP_CSU_VERSION_QEMU;
+	}
+
+	return ZYNQMP_CSU_VERSION_SILICON;
+}
+
+#define ZYNQMP_MMIO_READ	0xC2000014
+#define ZYNQMP_MMIO_WRITE	0xC2000013
+
+int __maybe_unused 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];
+}
+
+unsigned int  __maybe_unused zynqmp_pmufw_version(void)
+{
+	int ret;
+	u32 ret_payload[PAYLOAD_ARG_CNT];
+	static u32 pm_api_version = ZYNQMP_PM_VERSION_INVALID;
+
+	/*
+	 * Get PMU version only once and later
+	 * just return stored values instead of
+	 * asking PMUFW again.
+	 */
+	if (pm_api_version == ZYNQMP_PM_VERSION_INVALID) {
+		ret = invoke_smc(ZYNQMP_SIP_SVC_GET_API_VERSION, 0, 0, 0, 0,
+				 ret_payload);
+		pm_api_version = ret_payload[1];
+
+		if (ret)
+			panic("PMUFW is not found - Please load it!\n");
+	}
+
+	return pm_api_version;
+}
+
+static int zynqmp_mmio_rawwrite(const u32 address,
+		      const u32 mask,
+		      const u32 value)
+{
+	u32 data;
+	u32 value_local = value;
+	int ret;
+
+	ret = zynqmp_mmio_read(address, &data);
+	if (ret)
+		return ret;
+
+	data &= ~mask;
+	value_local &= mask;
+	value_local |= data;
+	writel(value_local, (ulong)address);
+	return 0;
+}
+
+static int zynqmp_mmio_rawread(const u32 address, u32 *value)
+{
+	*value = readl((ulong)address);
+	return 0;
+}
+
+int zynqmp_mmio_write(const u32 address,
+		      const u32 mask,
+		      const u32 value)
+{
+	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3)
+		return zynqmp_mmio_rawwrite(address, mask, value);
+	else
+		return invoke_smc(ZYNQMP_MMIO_WRITE, address, mask,
+				  value, 0, NULL);
+
+	return -EINVAL;
+}
+
+int zynqmp_mmio_read(const u32 address, u32 *value)
+{
+	u32 ret_payload[PAYLOAD_ARG_CNT];
+	u32 ret;
+
+	if (!value)
+		return -EINVAL;
+
+	if (IS_ENABLED(CONFIG_SPL_BUILD) || current_el() == 3) {
+		ret = zynqmp_mmio_rawread(address, value);
+	} else {
+		ret = invoke_smc(ZYNQMP_MMIO_READ, address, 0, 0,
+				 0, ret_payload);
+		*value = ret_payload[1];
+	}
+
+	return ret;
+}
diff --git a/arch/arm/mach-zynqmp/handoff.c b/arch/arm/mach-zynqmp/handoff.c
new file mode 100644
index 0000000000..f71ff7b3d2
--- /dev/null
+++ b/arch/arm/mach-zynqmp/handoff.c
@@ -0,0 +1,86 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2016 - 2017 Xilinx, Inc.
+ *
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+
+/*
+ * atfhandoffparams
+ * Parameter	bitfield	encoding
+ * -----------------------------------------------------------------------------
+ * Exec State	0	0 -> Aarch64, 1-> Aarch32
+ * endianness	1	0 -> LE, 1 -> BE
+ * secure (TZ)	2	0 -> Non secure, 1 -> secure
+ * EL		3:4	00 -> EL0, 01 -> EL1, 10 -> EL2, 11 -> EL3
+ * CPU#		5:6	00 -> A53_0, 01 -> A53_1, 10 -> A53_2, 11 -> A53_3
+ */
+
+#define FSBL_FLAGS_ESTATE_SHIFT		0
+#define FSBL_FLAGS_ESTATE_MASK		(1 << FSBL_FLAGS_ESTATE_SHIFT)
+#define FSBL_FLAGS_ESTATE_A64		0
+#define FSBL_FLAGS_ESTATE_A32		1
+
+#define FSBL_FLAGS_ENDIAN_SHIFT		1
+#define FSBL_FLAGS_ENDIAN_MASK		(1 << FSBL_FLAGS_ENDIAN_SHIFT)
+#define FSBL_FLAGS_ENDIAN_LE		0
+#define FSBL_FLAGS_ENDIAN_BE		1
+
+#define FSBL_FLAGS_TZ_SHIFT		2
+#define FSBL_FLAGS_TZ_MASK		(1 << FSBL_FLAGS_TZ_SHIFT)
+#define FSBL_FLAGS_NON_SECURE		0
+#define FSBL_FLAGS_SECURE		1
+
+#define FSBL_FLAGS_EL_SHIFT		3
+#define FSBL_FLAGS_EL_MASK		(3 << FSBL_FLAGS_EL_SHIFT)
+#define FSBL_FLAGS_EL0			0
+#define FSBL_FLAGS_EL1			1
+#define FSBL_FLAGS_EL2			2
+#define FSBL_FLAGS_EL3			3
+
+#define FSBL_FLAGS_CPU_SHIFT		5
+#define FSBL_FLAGS_CPU_MASK		(3 << FSBL_FLAGS_CPU_SHIFT)
+#define FSBL_FLAGS_A53_0		0
+#define FSBL_FLAGS_A53_1		1
+#define FSBL_FLAGS_A53_2		2
+#define FSBL_FLAGS_A53_3		3
+
+#define FSBL_MAX_PARTITIONS		8
+
+/* Structure corresponding to each partition entry */
+struct xfsbl_partition {
+	uint64_t entry_point;
+	uint64_t flags;
+};
+
+/* Structure for handoff parameters to ARM Trusted Firmware (ATF) */
+struct xfsbl_atf_handoff_params {
+	uint8_t magic[4];
+	uint32_t num_entries;
+	struct xfsbl_partition partition[FSBL_MAX_PARTITIONS];
+};
+
+#ifdef CONFIG_SPL_OS_BOOT
+void handoff_setup(void)
+{
+	struct xfsbl_atf_handoff_params *atfhandoffparams;
+
+	atfhandoffparams = (void *)CONFIG_SPL_TEXT_BASE;
+	atfhandoffparams->magic[0] = 'X';
+	atfhandoffparams->magic[1] = 'L';
+	atfhandoffparams->magic[2] = 'N';
+	atfhandoffparams->magic[3] = 'X';
+
+	atfhandoffparams->num_entries = 1;
+	atfhandoffparams->partition[0].entry_point = CONFIG_SYS_TEXT_BASE;
+	atfhandoffparams->partition[0].flags = FSBL_FLAGS_EL2 <<
+					       FSBL_FLAGS_EL_SHIFT;
+
+	writel(CONFIG_SPL_TEXT_BASE, &pmu_base->gen_storage6);
+}
+#endif
diff --git a/arch/arm/mach-zynqmp/include/mach/clk.h b/arch/arm/mach-zynqmp/include/mach/clk.h
new file mode 100644
index 0000000000..cfd44c8e0f
--- /dev/null
+++ b/arch/arm/mach-zynqmp/include/mach/clk.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#ifndef _ASM_ARCH_CLK_H_
+#define _ASM_ARCH_CLK_H_
+
+unsigned long zynqmp_get_system_timer_freq(void);
+
+#endif /* _ASM_ARCH_CLK_H_ */
diff --git a/arch/arm/mach-zynqmp/include/mach/gpio.h b/arch/arm/mach-zynqmp/include/mach/gpio.h
new file mode 100644
index 0000000000..542a5fc3e9
--- /dev/null
+++ b/arch/arm/mach-zynqmp/include/mach/gpio.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2015 Xilinx, Inc.
+ */
+
+#ifndef __ARCH_ZYNQMP_GPIO_H
+#define __ARCH_ZYNQMP_GPIO_H
+
+/* Empty file - sdhci requires this. */
+
+#endif
diff --git a/arch/arm/mach-zynqmp/include/mach/hardware.h b/arch/arm/mach-zynqmp/include/mach/hardware.h
new file mode 100644
index 0000000000..8a505edab3
--- /dev/null
+++ b/arch/arm/mach-zynqmp/include/mach/hardware.h
@@ -0,0 +1,159 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#ifndef _ASM_ARCH_HARDWARE_H
+#define _ASM_ARCH_HARDWARE_H
+
+#define ZYNQ_GEM_BASEADDR0	0xFF0B0000
+#define ZYNQ_GEM_BASEADDR1	0xFF0C0000
+#define ZYNQ_GEM_BASEADDR2	0xFF0D0000
+#define ZYNQ_GEM_BASEADDR3	0xFF0E0000
+
+#define ZYNQ_I2C_BASEADDR0	0xFF020000
+#define ZYNQ_I2C_BASEADDR1	0xFF030000
+
+#define ARASAN_NAND_BASEADDR	0xFF100000
+
+#define ZYNQMP_TCM_BASE_ADDR	0xFFE00000
+#define ZYNQMP_TCM_SIZE		0x40000
+
+#define ZYNQMP_CRL_APB_BASEADDR	0xFF5E0000
+#define ZYNQMP_CRL_APB_TIMESTAMP_REF_CTRL_CLKACT	0x1000000
+#define ZYNQMP_CRL_APB_BOOT_PIN_CTRL_OUT_EN_SHIFT	0
+#define ZYNQMP_CRL_APB_BOOT_PIN_CTRL_OUT_VAL_SHIFT	8
+
+#define PS_MODE0	BIT(0)
+#define PS_MODE1	BIT(1)
+#define PS_MODE2	BIT(2)
+#define PS_MODE3	BIT(3)
+
+#define RESET_REASON_DEBUG_SYS	BIT(6)
+#define RESET_REASON_SOFT	BIT(5)
+#define RESET_REASON_SRST	BIT(4)
+#define RESET_REASON_PSONLY	BIT(3)
+#define RESET_REASON_PMU	BIT(2)
+#define RESET_REASON_INTERNAL	BIT(1)
+#define RESET_REASON_EXTERNAL	BIT(0)
+
+struct crlapb_regs {
+	u32 reserved0[36];
+	u32 cpu_r5_ctrl; /* 0x90 */
+	u32 reserved1[37];
+	u32 timestamp_ref_ctrl; /* 0x128 */
+	u32 reserved2[53];
+	u32 boot_mode; /* 0x200 */
+	u32 reserved3_0[7];
+	u32 reset_reason; /* 0x220 */
+	u32 reserved3_1[6];
+	u32 rst_lpd_top; /* 0x23C */
+	u32 reserved4[4];
+	u32 boot_pin_ctrl; /* 0x250 */
+	u32 reserved5[21];
+};
+
+#define crlapb_base ((struct crlapb_regs *)ZYNQMP_CRL_APB_BASEADDR)
+
+#define ZYNQMP_IOU_SCNTR_SECURE	0xFF260000
+#define ZYNQMP_IOU_SCNTR_COUNTER_CONTROL_REGISTER_EN	0x1
+#define ZYNQMP_IOU_SCNTR_COUNTER_CONTROL_REGISTER_HDBG	0x2
+
+struct iou_scntr_secure {
+	u32 counter_control_register;
+	u32 reserved0[7];
+	u32 base_frequency_id_register;
+};
+
+#define iou_scntr_secure ((struct iou_scntr_secure *)ZYNQMP_IOU_SCNTR_SECURE)
+
+/* Bootmode setting values */
+#define BOOT_MODES_MASK	0x0000000F
+#define QSPI_MODE_24BIT	0x00000001
+#define QSPI_MODE_32BIT	0x00000002
+#define SD_MODE		0x00000003 /* sd 0 */
+#define SD_MODE1	0x00000005 /* sd 1 */
+#define NAND_MODE	0x00000004
+#define EMMC_MODE	0x00000006
+#define USB_MODE	0x00000007
+#define SD1_LSHFT_MODE	0x0000000E /* SD1 Level shifter */
+#define JTAG_MODE	0x00000000
+#define BOOT_MODE_USE_ALT	0x100
+#define BOOT_MODE_ALT_SHIFT	12
+/* SW secondary boot modes 0xa - 0xd */
+#define SW_USBHOST_MODE	0x0000000A
+#define SW_SATA_MODE	0x0000000B
+
+#define ZYNQMP_IOU_SLCR_BASEADDR	0xFF180000
+
+struct iou_slcr_regs {
+	u32 mio_pin[78];
+	u32 reserved[442];
+};
+
+#define slcr_base ((struct iou_slcr_regs *)ZYNQMP_IOU_SLCR_BASEADDR)
+
+#define ZYNQMP_RPU_BASEADDR	0xFF9A0000
+
+struct rpu_regs {
+	u32 rpu_glbl_ctrl;
+	u32 reserved0[63];
+	u32 rpu0_cfg; /* 0x100 */
+	u32 reserved1[63];
+	u32 rpu1_cfg; /* 0x200 */
+};
+
+#define rpu_base ((struct rpu_regs *)ZYNQMP_RPU_BASEADDR)
+
+#define ZYNQMP_CRF_APB_BASEADDR	0xFD1A0000
+
+struct crfapb_regs {
+	u32 reserved0[65];
+	u32 rst_fpd_apu; /* 0x104 */
+	u32 reserved1;
+};
+
+#define crfapb_base ((struct crfapb_regs *)ZYNQMP_CRF_APB_BASEADDR)
+
+#define ZYNQMP_APU_BASEADDR	0xFD5C0000
+
+struct apu_regs {
+	u32 reserved0[16];
+	u32 rvbar_addr0_l; /* 0x40 */
+	u32 rvbar_addr0_h; /* 0x44 */
+	u32 reserved1[20];
+};
+
+#define apu_base ((struct apu_regs *)ZYNQMP_APU_BASEADDR)
+
+/* Board version value */
+#define ZYNQMP_CSU_BASEADDR		0xFFCA0000
+#define ZYNQMP_CSU_VERSION_SILICON	0x0
+#define ZYNQMP_CSU_VERSION_QEMU		0x3
+
+#define ZYNQMP_CSU_VERSION_EMPTY_SHIFT		20
+
+#define ZYNQMP_SILICON_VER_MASK		0xF000
+#define ZYNQMP_SILICON_VER_SHIFT	12
+
+struct csu_regs {
+	u32 reserved0[17];
+	u32 version;
+};
+
+#define csu_base ((struct csu_regs *)ZYNQMP_CSU_BASEADDR)
+
+#define ZYNQMP_PMU_BASEADDR	0xFFD80000
+
+struct pmu_regs {
+	u32 reserved[18];
+	u32 gen_storage6; /* 0x48 */
+};
+
+#define pmu_base ((struct pmu_regs *)ZYNQMP_PMU_BASEADDR)
+
+#define ZYNQMP_CSU_IDCODE_ADDR	0xFFCA0040
+#define ZYNQMP_CSU_VER_ADDR	0xFFCA0044
+
+#endif /* _ASM_ARCH_HARDWARE_H */
diff --git a/arch/arm/mach-zynqmp/include/mach/psu_init_gpl.h b/arch/arm/mach-zynqmp/include/mach/psu_init_gpl.h
new file mode 100644
index 0000000000..15e54c0493
--- /dev/null
+++ b/arch/arm/mach-zynqmp/include/mach/psu_init_gpl.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+
+#ifndef _PSU_INIT_GPL_H_ /* prevent circular inclusions */
+#define _PSU_INIT_GPL_H_
+
+#include <asm/io.h>
+#include <common.h>
+
+int mask_pollonvalue(unsigned long add, u32 mask, u32 value);
+
+int mask_poll(u32 add, u32 mask);
+
+u32 mask_read(u32 add, u32 mask);
+
+void mask_delay(u32 delay);
+
+void psu_mask_write(unsigned long offset, unsigned long mask,
+		    unsigned long val);
+
+void prog_reg(unsigned long addr, unsigned long mask,
+	      unsigned long shift, unsigned long value);
+
+int psu_init(void);
+
+#endif /* _PSU_INIT_GPL_H_ */
diff --git a/arch/arm/mach-zynqmp/include/mach/sys_proto.h b/arch/arm/mach-zynqmp/include/mach/sys_proto.h
new file mode 100644
index 0000000000..385c8825f2
--- /dev/null
+++ b/arch/arm/mach-zynqmp/include/mach/sys_proto.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#ifndef _ASM_ARCH_SYS_PROTO_H
+#define _ASM_ARCH_SYS_PROTO_H
+
+#define PAYLOAD_ARG_CNT		5
+
+#define ZYNQMP_CSU_SILICON_VER_MASK	0xF
+#define ZYNQMP_SIP_SVC_PM_SECURE_IMG_LOAD	0xC200002D
+#define KEY_PTR_LEN	32
+
+#define ZYNQMP_FPGA_BIT_AUTH_DDR	1
+#define ZYNQMP_FPGA_BIT_AUTH_OCM	2
+#define ZYNQMP_FPGA_BIT_ENC_USR_KEY	3
+#define ZYNQMP_FPGA_BIT_ENC_DEV_KEY	4
+#define ZYNQMP_FPGA_BIT_NS		5
+
+#define ZYNQMP_FPGA_AUTH_DDR	1
+
+#define ZYNQMP_SIP_SVC_GET_API_VERSION		0xC2000001
+
+#define ZYNQMP_PM_VERSION_MAJOR		1
+#define ZYNQMP_PM_VERSION_MINOR		0
+#define ZYNQMP_PM_VERSION_MAJOR_SHIFT	16
+#define ZYNQMP_PM_VERSION_MINOR_MASK	0xFFFF
+
+#define ZYNQMP_PM_VERSION	\
+	((ZYNQMP_PM_VERSION_MAJOR << ZYNQMP_PM_VERSION_MAJOR_SHIFT) | \
+				 ZYNQMP_PM_VERSION_MINOR)
+
+#define ZYNQMP_PM_VERSION_INVALID	~0
+
+#define PMUFW_V1_0	((1 << ZYNQMP_PM_VERSION_MAJOR_SHIFT) | 0)
+
+enum {
+	IDCODE,
+	VERSION,
+	IDCODE2,
+};
+
+enum {
+	ZYNQMP_SILICON_V1,
+	ZYNQMP_SILICON_V2,
+	ZYNQMP_SILICON_V3,
+	ZYNQMP_SILICON_V4,
+};
+
+enum {
+	TCM_LOCK,
+	TCM_SPLIT,
+};
+
+int zynq_board_read_rom_ethaddr(unsigned char *ethaddr);
+unsigned int zynqmp_get_silicon_version(void);
+
+void handoff_setup(void);
+
+unsigned int zynqmp_pmufw_version(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);
+
+void initialize_tcm(bool mode);
+void mem_map_fill(void);
+int chip_id(unsigned char id);
+#if defined(CONFIG_SYS_MEM_RSVD_FOR_MMU) || defined(CONFIG_DEFINE_TCM_OCM_MMAP)
+void tcm_init(u8 mode);
+#endif
+
+#endif /* _ASM_ARCH_SYS_PROTO_H */
diff --git a/arch/arm/mach-zynqmp/mp.c b/arch/arm/mach-zynqmp/mp.c
new file mode 100644
index 0000000000..2a71870ae7
--- /dev/null
+++ b/arch/arm/mach-zynqmp/mp.c
@@ -0,0 +1,297 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * (C) Copyright 2014 - 2015 Xilinx, Inc.
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#include <common.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/io.h>
+
+#define LOCK		0
+#define SPLIT		1
+
+#define HALT		0
+#define RELEASE		1
+
+#define ZYNQMP_BOOTADDR_HIGH_MASK		0xFFFFFFFF
+#define ZYNQMP_R5_HIVEC_ADDR			0xFFFF0000
+#define ZYNQMP_R5_LOVEC_ADDR			0x0
+#define ZYNQMP_RPU_CFG_CPU_HALT_MASK		0x01
+#define ZYNQMP_RPU_CFG_HIVEC_MASK		0x04
+#define ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK	0x08
+#define ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK	0x40
+#define ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK	0x10
+
+#define ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK	0x04
+#define ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK	0x01
+#define ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK	0x02
+#define ZYNQMP_CRLAPB_CPU_R5_CTRL_CLKACT_MASK	0x1000000
+
+#define ZYNQMP_TCM_START_ADDRESS		0xFFE00000
+#define ZYNQMP_TCM_BOTH_SIZE			0x40000
+
+#define ZYNQMP_CORE_APU0	0
+#define ZYNQMP_CORE_APU3	3
+
+#define ZYNQMP_MAX_CORES	6
+
+int is_core_valid(unsigned int core)
+{
+	if (core < ZYNQMP_MAX_CORES)
+		return 1;
+
+	return 0;
+}
+
+int cpu_reset(u32 nr)
+{
+	puts("Feature is not implemented.\n");
+	return 0;
+}
+
+static void set_r5_halt_mode(u8 halt, u8 mode)
+{
+	u32 tmp;
+
+	tmp = readl(&rpu_base->rpu0_cfg);
+	if (halt == HALT)
+		tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+	else
+		tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+	writel(tmp, &rpu_base->rpu0_cfg);
+
+	if (mode == LOCK) {
+		tmp = readl(&rpu_base->rpu1_cfg);
+		if (halt == HALT)
+			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		else
+			tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		writel(tmp, &rpu_base->rpu1_cfg);
+	}
+}
+
+static void set_r5_tcm_mode(u8 mode)
+{
+	u32 tmp;
+
+	tmp = readl(&rpu_base->rpu_glbl_ctrl);
+	if (mode == LOCK) {
+		tmp &= ~ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK;
+		tmp |= ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK |
+		       ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK;
+	} else {
+		tmp |= ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK;
+		tmp &= ~(ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK |
+		       ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK);
+	}
+
+	writel(tmp, &rpu_base->rpu_glbl_ctrl);
+}
+
+static void set_r5_reset(u8 mode)
+{
+	u32 tmp;
+
+	tmp = readl(&crlapb_base->rst_lpd_top);
+	tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+
+	if (mode == LOCK)
+		tmp |= ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+
+	writel(tmp, &crlapb_base->rst_lpd_top);
+}
+
+static void release_r5_reset(u8 mode)
+{
+	u32 tmp;
+
+	tmp = readl(&crlapb_base->rst_lpd_top);
+	tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+
+	if (mode == LOCK)
+		tmp &= ~ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+
+	writel(tmp, &crlapb_base->rst_lpd_top);
+}
+
+static void enable_clock_r5(void)
+{
+	u32 tmp;
+
+	tmp = readl(&crlapb_base->cpu_r5_ctrl);
+	tmp |= ZYNQMP_CRLAPB_CPU_R5_CTRL_CLKACT_MASK;
+	writel(tmp, &crlapb_base->cpu_r5_ctrl);
+
+	/* Give some delay for clock
+	 * to propagate */
+	udelay(0x500);
+}
+
+int cpu_disable(u32 nr)
+{
+	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
+		u32 val = readl(&crfapb_base->rst_fpd_apu);
+		val |= 1 << nr;
+		writel(val, &crfapb_base->rst_fpd_apu);
+	} else {
+		set_r5_reset(LOCK);
+	}
+
+	return 0;
+}
+
+int cpu_status(u32 nr)
+{
+	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
+		u32 addr_low = readl(((u8 *)&apu_base->rvbar_addr0_l) + nr * 8);
+		u32 addr_high = readl(((u8 *)&apu_base->rvbar_addr0_h) +
+				      nr * 8);
+		u32 val = readl(&crfapb_base->rst_fpd_apu);
+		val &= 1 << nr;
+		printf("APU CPU%d %s - starting address HI: %x, LOW: %x\n",
+		       nr, val ? "OFF" : "ON" , addr_high, addr_low);
+	} else {
+		u32 val = readl(&crlapb_base->rst_lpd_top);
+		val &= 1 << (nr - 4);
+		printf("RPU CPU%d %s\n", nr - 4, val ? "OFF" : "ON");
+	}
+
+	return 0;
+}
+
+static void set_r5_start(u8 high)
+{
+	u32 tmp;
+
+	tmp = readl(&rpu_base->rpu0_cfg);
+	if (high)
+		tmp |= ZYNQMP_RPU_CFG_HIVEC_MASK;
+	else
+		tmp &= ~ZYNQMP_RPU_CFG_HIVEC_MASK;
+	writel(tmp, &rpu_base->rpu0_cfg);
+
+	tmp = readl(&rpu_base->rpu1_cfg);
+	if (high)
+		tmp |= ZYNQMP_RPU_CFG_HIVEC_MASK;
+	else
+		tmp &= ~ZYNQMP_RPU_CFG_HIVEC_MASK;
+	writel(tmp, &rpu_base->rpu1_cfg);
+}
+
+static void write_tcm_boot_trampoline(u32 boot_addr)
+{
+	if (boot_addr) {
+		/*
+		 * Boot trampoline is simple ASM code below.
+		 *
+		 *		b over;
+		 *	label:
+		 *	.word	0
+		 *	over:	ldr	r0, =label
+		 *		ldr	r1, [r0]
+		 *		bx	r1
+		 */
+		debug("Write boot trampoline for %x\n", boot_addr);
+		writel(0xea000000, ZYNQMP_TCM_START_ADDRESS);
+		writel(boot_addr, ZYNQMP_TCM_START_ADDRESS + 0x4);
+		writel(0xe59f0004, ZYNQMP_TCM_START_ADDRESS + 0x8);
+		writel(0xe5901000, ZYNQMP_TCM_START_ADDRESS + 0xc);
+		writel(0xe12fff11, ZYNQMP_TCM_START_ADDRESS + 0x10);
+		writel(0x00000004, ZYNQMP_TCM_START_ADDRESS + 0x14); // address for
+	}
+}
+
+void initialize_tcm(bool mode)
+{
+	if (!mode) {
+		set_r5_tcm_mode(LOCK);
+		set_r5_halt_mode(HALT, LOCK);
+		enable_clock_r5();
+		release_r5_reset(LOCK);
+	} else {
+		set_r5_tcm_mode(SPLIT);
+		set_r5_halt_mode(HALT, SPLIT);
+		enable_clock_r5();
+		release_r5_reset(SPLIT);
+	}
+}
+
+int cpu_release(u32 nr, int argc, char * const argv[])
+{
+	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
+		u64 boot_addr = simple_strtoull(argv[0], NULL, 16);
+		/* HIGH */
+		writel((u32)(boot_addr >> 32),
+		       ((u8 *)&apu_base->rvbar_addr0_h) + nr * 8);
+		/* LOW */
+		writel((u32)(boot_addr & ZYNQMP_BOOTADDR_HIGH_MASK),
+		       ((u8 *)&apu_base->rvbar_addr0_l) + nr * 8);
+
+		u32 val = readl(&crfapb_base->rst_fpd_apu);
+		val &= ~(1 << nr);
+		writel(val, &crfapb_base->rst_fpd_apu);
+	} else {
+		if (argc != 2) {
+			printf("Invalid number of arguments to release.\n");
+			printf("<addr> <mode>-Start addr lockstep or split\n");
+			return 1;
+		}
+
+		u32 boot_addr = simple_strtoul(argv[0], NULL, 16);
+		u32 boot_addr_uniq = 0;
+		if (!(boot_addr == ZYNQMP_R5_LOVEC_ADDR ||
+		      boot_addr == ZYNQMP_R5_HIVEC_ADDR)) {
+			printf("Using TCM jump trampoline for address 0x%x\n",
+			       boot_addr);
+			/* Save boot address for later usage */
+			boot_addr_uniq = boot_addr;
+			/*
+			 * R5 needs to start from LOVEC at TCM
+			 * OCM will be probably occupied by ATF
+			 */
+			boot_addr = ZYNQMP_R5_LOVEC_ADDR;
+		}
+
+		/*
+		 * Since we don't know where the user may have loaded the image
+		 * for an R5 we have to flush all the data cache to ensure
+		 * the R5 sees it.
+		 */
+		flush_dcache_all();
+
+		if (!strncmp(argv[1], "lockstep", 8)) {
+			printf("R5 lockstep mode\n");
+			set_r5_reset(LOCK);
+			set_r5_tcm_mode(LOCK);
+			set_r5_halt_mode(HALT, LOCK);
+			set_r5_start(boot_addr);
+			enable_clock_r5();
+			release_r5_reset(LOCK);
+			dcache_disable();
+			write_tcm_boot_trampoline(boot_addr_uniq);
+			dcache_enable();
+			set_r5_halt_mode(RELEASE, LOCK);
+		} else if (!strncmp(argv[1], "split", 5)) {
+			printf("R5 split mode\n");
+			set_r5_reset(SPLIT);
+			set_r5_tcm_mode(SPLIT);
+			set_r5_halt_mode(HALT, SPLIT);
+			set_r5_start(boot_addr);
+			enable_clock_r5();
+			release_r5_reset(SPLIT);
+			dcache_disable();
+			write_tcm_boot_trampoline(boot_addr_uniq);
+			dcache_enable();
+			set_r5_halt_mode(RELEASE, SPLIT);
+		} else {
+			printf("Unsupported mode\n");
+			return 1;
+		}
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-zynqmp/psu_spl_init.c b/arch/arm/mach-zynqmp/psu_spl_init.c
new file mode 100644
index 0000000000..b357de3235
--- /dev/null
+++ b/arch/arm/mach-zynqmp/psu_spl_init.c
@@ -0,0 +1,79 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2018 Xilinx, Inc.
+ *
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/psu_init_gpl.h>
+
+#define PSU_MASK_POLL_TIME 1100000
+
+int __maybe_unused mask_pollonvalue(unsigned long add, u32 mask, u32 value)
+{
+	int i = 0;
+
+	while ((__raw_readl(add) & mask) != value) {
+		if (i == PSU_MASK_POLL_TIME)
+			return 0;
+		i++;
+	}
+	return 1;
+}
+
+__weak int mask_poll(u32 add, u32 mask)
+{
+	int i = 0;
+	unsigned long addr = add;
+
+	while (!(__raw_readl(addr) & mask)) {
+		if (i == PSU_MASK_POLL_TIME)
+			return 0;
+		i++;
+	}
+	return 1;
+}
+
+__weak u32 mask_read(u32 add, u32 mask)
+{
+	unsigned long addr = add;
+
+	return __raw_readl(addr) & mask;
+}
+
+__weak void mask_delay(u32 delay)
+{
+	udelay(delay);
+}
+
+__weak void psu_mask_write(unsigned long offset, unsigned long mask,
+			   unsigned long val)
+{
+	unsigned long regval = 0;
+
+	regval = readl(offset);
+	regval &= ~(mask);
+	regval |= (val & mask);
+	writel(regval, offset);
+}
+
+__weak void prog_reg(unsigned long addr, unsigned long mask,
+		     unsigned long shift, unsigned long value)
+{
+	int rdata = 0;
+
+	rdata = readl(addr);
+	rdata = rdata & (~mask);
+	rdata = rdata | (value << shift);
+	writel(rdata, addr);
+}
+
+__weak int psu_init(void)
+{
+	/*
+	 * This function is overridden by the one in
+	 * board/xilinx/zynqmp/(platform)/psu_init_gpl.c, if it exists.
+	 */
+	return -1;
+}
diff --git a/arch/arm/mach-zynqmp/spl.c b/arch/arm/mach-zynqmp/spl.c
new file mode 100644
index 0000000000..01f31d0f0e
--- /dev/null
+++ b/arch/arm/mach-zynqmp/spl.c
@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2015 - 2016 Xilinx, Inc.
+ *
+ * Michal Simek <michal.simek@xilinx.com>
+ */
+
+#include <common.h>
+#include <debug_uart.h>
+#include <spl.h>
+
+#include <asm/io.h>
+#include <asm/spl.h>
+#include <asm/arch/hardware.h>
+#include <asm/arch/sys_proto.h>
+
+void board_init_f(ulong dummy)
+{
+	board_early_init_f();
+	board_early_init_r();
+
+#ifdef CONFIG_DEBUG_UART
+	/* Uart debug for sure */
+	debug_uart_init();
+	puts("Debug uart enabled\n"); /* or printch() */
+#endif
+	/* Delay is required for clocks to be propagated */
+	udelay(1000000);
+
+	/* Clear the BSS */
+	memset(__bss_start, 0, __bss_end - __bss_start);
+
+	/* No need to call timer init - it is empty for ZynqMP */
+	board_init_r(NULL, 0);
+}
+
+static void ps_mode_reset(ulong mode)
+{
+	writel(mode << ZYNQMP_CRL_APB_BOOT_PIN_CTRL_OUT_EN_SHIFT,
+	       &crlapb_base->boot_pin_ctrl);
+	udelay(5);
+	writel(mode << ZYNQMP_CRL_APB_BOOT_PIN_CTRL_OUT_VAL_SHIFT |
+	       mode << ZYNQMP_CRL_APB_BOOT_PIN_CTRL_OUT_EN_SHIFT,
+	       &crlapb_base->boot_pin_ctrl);
+}
+
+/*
+ * Set default PS_MODE1 which is used for USB ULPI phy reset
+ * Also other resets can be connected to this certain pin
+ */
+#ifndef MODE_RESET
+# define MODE_RESET	PS_MODE1
+#endif
+
+#ifdef CONFIG_SPL_BOARD_INIT
+void spl_board_init(void)
+{
+	preloader_console_init();
+	ps_mode_reset(MODE_RESET);
+	board_init();
+}
+#endif
+
+u32 spl_boot_device(void)
+{
+	u32 reg = 0;
+	u8 bootmode;
+
+#if defined(CONFIG_SPL_ZYNQMP_ALT_BOOTMODE_ENABLED)
+	/* Change default boot mode at run-time */
+	writel(CONFIG_SPL_ZYNQMP_ALT_BOOTMODE << BOOT_MODE_ALT_SHIFT,
+	       &crlapb_base->boot_mode);
+#endif
+
+	reg = readl(&crlapb_base->boot_mode);
+	if (reg >> BOOT_MODE_ALT_SHIFT)
+		reg >>= BOOT_MODE_ALT_SHIFT;
+
+	bootmode = reg & BOOT_MODES_MASK;
+
+	switch (bootmode) {
+	case JTAG_MODE:
+		return BOOT_DEVICE_RAM;
+#ifdef CONFIG_SPL_MMC_SUPPORT
+	case SD_MODE1:
+	case SD1_LSHFT_MODE: /* not working on silicon v1 */
+/* if both controllers enabled, then these two are the second controller */
+#if defined(CONFIG_ZYNQ_SDHCI0) && defined(CONFIG_ZYNQ_SDHCI1)
+		return BOOT_DEVICE_MMC2;
+/* else, fall through, the one SDHCI controller that is enabled is number 1 */
+#endif
+	case SD_MODE:
+	case EMMC_MODE:
+		return BOOT_DEVICE_MMC1;
+#endif
+#ifdef CONFIG_SPL_DFU_SUPPORT
+	case USB_MODE:
+		return BOOT_DEVICE_DFU;
+#endif
+#ifdef CONFIG_SPL_SATA_SUPPORT
+	case SW_SATA_MODE:
+		return BOOT_DEVICE_SATA;
+#endif
+#ifdef CONFIG_SPL_SPI_SUPPORT
+	case QSPI_MODE_24BIT:
+	case QSPI_MODE_32BIT:
+		return BOOT_DEVICE_SPI;
+#endif
+	default:
+		printf("Invalid Boot Mode:0x%x\n", bootmode);
+		break;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_SPL_OS_BOOT
+int spl_start_uboot(void)
+{
+	handoff_setup();
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SPL_LOAD_FIT
+int board_fit_config_name_match(const char *name)
+{
+	/* Just empty function now - can't decide what to choose */
+	debug("%s: %s\n", __func__, name);
+
+	return 0;
+}
+#endif