Adding upstream version 6.6.15.upstream/6.6.15

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

8 files changed, 758 insertions, 0 deletions

diff --git a/arch/arm/mach-zynq/Kconfig b/arch/arm/mach-zynq/Kconfig
new file mode 100644
index 0000000000..05be5aa9b4
--- /dev/null
+++ b/arch/arm/mach-zynq/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+config ARCH_ZYNQ
+	bool "Xilinx Zynq ARM Cortex A9 Platform"
+	depends on ARCH_MULTI_V7
+	select ARCH_HAS_RESET_CONTROLLER
+	select ARM_AMBA
+	select ARM_GIC
+	select ARM_GLOBAL_TIMER
+	select CADENCE_TTC_TIMER
+	select HAVE_ARM_SCU if SMP
+	select HAVE_ARM_TWD if SMP
+	select MFD_SYSCON
+	select PINCTRL
+	select PINCTRL_ZYNQ
+	select SOC_BUS
+	help
+	  Support for Xilinx Zynq ARM Cortex A9 Platform
diff --git a/arch/arm/mach-zynq/Makefile b/arch/arm/mach-zynq/Makefile
new file mode 100644
index 0000000000..9df74cd85f
--- /dev/null
+++ b/arch/arm/mach-zynq/Makefile
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# Makefile for the linux kernel.
+#
+
+# Common support
+obj-y				:= common.o slcr.o pm.o
+obj-$(CONFIG_SMP)		+= headsmp.o platsmp.o
diff --git a/arch/arm/mach-zynq/common.c b/arch/arm/mach-zynq/common.c
new file mode 100644
index 0000000000..15e8a321a7
--- /dev/null
+++ b/arch/arm/mach-zynq/common.c
@@ -0,0 +1,200 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * This file contains common code that is intended to be used across
+ * boards so that it's not replicated.
+ *
+ *  Copyright (C) 2011 Xilinx
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/cpumask.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/clk/zynq.h>
+#include <linux/clocksource.h>
+#include <linux/of_address.h>
+#include <linux/of_clk.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/of.h>
+#include <linux/memblock.h>
+#include <linux/irqchip.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
+#include <linux/pgtable.h>
+
+#include <asm/mach/arch.h>
+#include <asm/mach/map.h>
+#include <asm/mach/time.h>
+#include <asm/mach-types.h>
+#include <asm/page.h>
+#include <asm/smp_scu.h>
+#include <asm/system_info.h>
+#include <asm/hardware/cache-l2x0.h>
+
+#include "common.h"
+
+#define ZYNQ_DEVCFG_MCTRL		0x80
+#define ZYNQ_DEVCFG_PS_VERSION_SHIFT	28
+#define ZYNQ_DEVCFG_PS_VERSION_MASK	0xF
+
+void __iomem *zynq_scu_base;
+
+/**
+ * zynq_memory_init - Initialize special memory
+ *
+ * We need to stop things allocating the low memory as DMA can't work in
+ * the 1st 512K of memory.
+ */
+static void __init zynq_memory_init(void)
+{
+	if (!__pa(PAGE_OFFSET))
+		memblock_reserve(__pa(PAGE_OFFSET), 0x80000);
+}
+
+static struct platform_device zynq_cpuidle_device = {
+	.name = "cpuidle-zynq",
+};
+
+/**
+ * zynq_get_revision - Get Zynq silicon revision
+ *
+ * Return: Silicon version or -1 otherwise
+ */
+static int __init zynq_get_revision(void)
+{
+	struct device_node *np;
+	void __iomem *zynq_devcfg_base;
+	u32 revision;
+
+	np = of_find_compatible_node(NULL, NULL, "xlnx,zynq-devcfg-1.0");
+	if (!np) {
+		pr_err("%s: no devcfg node found\n", __func__);
+		return -1;
+	}
+
+	zynq_devcfg_base = of_iomap(np, 0);
+	of_node_put(np);
+	if (!zynq_devcfg_base) {
+		pr_err("%s: Unable to map I/O memory\n", __func__);
+		return -1;
+	}
+
+	revision = readl(zynq_devcfg_base + ZYNQ_DEVCFG_MCTRL);
+	revision >>= ZYNQ_DEVCFG_PS_VERSION_SHIFT;
+	revision &= ZYNQ_DEVCFG_PS_VERSION_MASK;
+
+	iounmap(zynq_devcfg_base);
+
+	return revision;
+}
+
+static void __init zynq_init_late(void)
+{
+	zynq_core_pm_init();
+	zynq_pm_late_init();
+}
+
+/**
+ * zynq_init_machine - System specific initialization, intended to be
+ *		       called from board specific initialization.
+ */
+static void __init zynq_init_machine(void)
+{
+	struct soc_device_attribute *soc_dev_attr;
+	struct soc_device *soc_dev;
+	struct device *parent = NULL;
+
+	soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+	if (!soc_dev_attr)
+		goto out;
+
+	system_rev = zynq_get_revision();
+
+	soc_dev_attr->family = kasprintf(GFP_KERNEL, "Xilinx Zynq");
+	soc_dev_attr->revision = kasprintf(GFP_KERNEL, "0x%x", system_rev);
+	soc_dev_attr->soc_id = kasprintf(GFP_KERNEL, "0x%x",
+					 zynq_slcr_get_device_id());
+
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		kfree(soc_dev_attr->family);
+		kfree(soc_dev_attr->revision);
+		kfree(soc_dev_attr->soc_id);
+		kfree(soc_dev_attr);
+		goto out;
+	}
+
+	parent = soc_device_to_device(soc_dev);
+
+out:
+	/*
+	 * Finished with the static registrations now; fill in the missing
+	 * devices
+	 */
+	of_platform_default_populate(NULL, NULL, parent);
+
+	platform_device_register(&zynq_cpuidle_device);
+}
+
+static void __init zynq_timer_init(void)
+{
+	zynq_clock_init();
+	of_clk_init(NULL);
+	timer_probe();
+}
+
+static struct map_desc zynq_cortex_a9_scu_map __initdata = {
+	.length	= SZ_256,
+	.type	= MT_DEVICE,
+};
+
+static void __init zynq_scu_map_io(void)
+{
+	unsigned long base;
+
+	base = scu_a9_get_base();
+	zynq_cortex_a9_scu_map.pfn = __phys_to_pfn(base);
+	/* Expected address is in vmalloc area that's why simple assign here */
+	zynq_cortex_a9_scu_map.virtual = base;
+	iotable_init(&zynq_cortex_a9_scu_map, 1);
+	zynq_scu_base = (void __iomem *)base;
+	BUG_ON(!zynq_scu_base);
+}
+
+/**
+ * zynq_map_io - Create memory mappings needed for early I/O.
+ */
+static void __init zynq_map_io(void)
+{
+	debug_ll_io_init();
+	zynq_scu_map_io();
+}
+
+static void __init zynq_irq_init(void)
+{
+	zynq_early_slcr_init();
+	irqchip_init();
+}
+
+static const char * const zynq_dt_match[] = {
+	"xlnx,zynq-7000",
+	NULL
+};
+
+DT_MACHINE_START(XILINX_EP107, "Xilinx Zynq Platform")
+	/* 64KB way size, 8-way associativity, parity disabled */
+	.l2c_aux_val    = 0x00400000,
+	.l2c_aux_mask	= 0xffbfffff,
+	.smp		= smp_ops(zynq_smp_ops),
+	.map_io		= zynq_map_io,
+	.init_irq	= zynq_irq_init,
+	.init_machine	= zynq_init_machine,
+	.init_late	= zynq_init_late,
+	.init_time	= zynq_timer_init,
+	.dt_compat	= zynq_dt_match,
+	.reserve	= zynq_memory_init,
+MACHINE_END
diff --git a/arch/arm/mach-zynq/common.h b/arch/arm/mach-zynq/common.h
new file mode 100644
index 0000000000..60e6623246
--- /dev/null
+++ b/arch/arm/mach-zynq/common.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * This file contains common function prototypes to avoid externs
+ * in the c files.
+ *
+ *  Copyright (C) 2011 Xilinx
+ */
+
+#ifndef __MACH_ZYNQ_COMMON_H__
+#define __MACH_ZYNQ_COMMON_H__
+
+extern int zynq_slcr_init(void);
+extern int zynq_early_slcr_init(void);
+extern void zynq_slcr_cpu_stop(int cpu);
+extern void zynq_slcr_cpu_start(int cpu);
+extern bool zynq_slcr_cpu_state_read(int cpu);
+extern void zynq_slcr_cpu_state_write(int cpu, bool die);
+extern u32 zynq_slcr_get_device_id(void);
+
+#ifdef CONFIG_SMP
+extern char zynq_secondary_trampoline;
+extern char zynq_secondary_trampoline_jump;
+extern char zynq_secondary_trampoline_end;
+extern int zynq_cpun_start(u32 address, int cpu);
+extern const struct smp_operations zynq_smp_ops;
+#endif
+
+extern void __iomem *zynq_scu_base;
+
+void zynq_pm_late_init(void);
+
+static inline void zynq_core_pm_init(void)
+{
+	/* A9 clock gating */
+	asm volatile ("mrc  p15, 0, r12, c15, c0, 0\n"
+		      "orr  r12, r12, #1\n"
+		      "mcr  p15, 0, r12, c15, c0, 0\n"
+		      : /* no outputs */
+		      : /* no inputs */
+		      : "r12");
+}
+
+#endif
diff --git a/arch/arm/mach-zynq/headsmp.S b/arch/arm/mach-zynq/headsmp.S
new file mode 100644
index 0000000000..3449e0d1f9
--- /dev/null
+++ b/arch/arm/mach-zynq/headsmp.S
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2013 Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ * Copyright (c) 2012-2013 Xilinx
+ */
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/assembler.h>
+
+	.arm
+
+ENTRY(zynq_secondary_trampoline)
+ARM_BE8(setend	be)				@ ensure we are in BE8 mode
+	ldr	r0, zynq_secondary_trampoline_jump
+ARM_BE8(rev	r0, r0)
+	bx	r0
+.globl zynq_secondary_trampoline_jump
+zynq_secondary_trampoline_jump:
+	/* Space for jumping address */
+	.word	0	/* cpu 1 */
+.globl zynq_secondary_trampoline_end
+zynq_secondary_trampoline_end:
+ENDPROC(zynq_secondary_trampoline)
diff --git a/arch/arm/mach-zynq/platsmp.c b/arch/arm/mach-zynq/platsmp.c
new file mode 100644
index 0000000000..68ec303fa2
--- /dev/null
+++ b/arch/arm/mach-zynq/platsmp.c
@@ -0,0 +1,163 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * This file contains Xilinx specific SMP code, used to start up
+ * the second processor.
+ *
+ * Copyright (C) 2011-2013 Xilinx
+ *
+ * based on linux/arch/arm/mach-realview/platsmp.c
+ *
+ * Copyright (C) 2002 ARM Ltd.
+ */
+
+#include <linux/export.h>
+#include <linux/jiffies.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <asm/cacheflush.h>
+#include <asm/smp_plat.h>
+#include <asm/smp_scu.h>
+#include <linux/irqchip/arm-gic.h>
+#include "common.h"
+
+/*
+ * Store number of cores in the system
+ * Because of scu_get_core_count() must be in __init section and can't
+ * be called from zynq_cpun_start() because it is not in __init section.
+ */
+static int ncores;
+
+int zynq_cpun_start(u32 address, int cpu)
+{
+	u32 trampoline_code_size = &zynq_secondary_trampoline_end -
+						&zynq_secondary_trampoline;
+	u32 phy_cpuid = cpu_logical_map(cpu);
+
+	/* MS: Expectation that SLCR are directly map and accessible */
+	/* Not possible to jump to non aligned address */
+	if (!(address & 3) && (!address || (address >= trampoline_code_size))) {
+		/* Store pointer to ioremap area which points to address 0x0 */
+		static u8 __iomem *zero;
+		u32 trampoline_size = &zynq_secondary_trampoline_jump -
+						&zynq_secondary_trampoline;
+
+		zynq_slcr_cpu_stop(phy_cpuid);
+		if (address) {
+			if (__pa(PAGE_OFFSET)) {
+				zero = ioremap(0, trampoline_code_size);
+				if (!zero) {
+					pr_warn("BOOTUP jump vectors not accessible\n");
+					return -1;
+				}
+			} else {
+				zero = (__force u8 __iomem *)PAGE_OFFSET;
+			}
+
+			/*
+			* This is elegant way how to jump to any address
+			* 0x0: Load address at 0x8 to r0
+			* 0x4: Jump by mov instruction
+			* 0x8: Jumping address
+			*/
+			memcpy_toio(zero, &zynq_secondary_trampoline,
+							trampoline_size);
+			writel(address, zero + trampoline_size);
+
+			flush_cache_all();
+			outer_flush_range(0, trampoline_code_size);
+			smp_wmb();
+
+			if (__pa(PAGE_OFFSET))
+				iounmap(zero);
+		}
+		zynq_slcr_cpu_start(phy_cpuid);
+
+		return 0;
+	}
+
+	pr_warn("Can't start CPU%d: Wrong starting address %x\n", cpu, address);
+
+	return -1;
+}
+EXPORT_SYMBOL(zynq_cpun_start);
+
+static int zynq_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	return zynq_cpun_start(__pa_symbol(secondary_startup_arm), cpu);
+}
+
+/*
+ * Initialise the CPU possible map early - this describes the CPUs
+ * which may be present or become present in the system.
+ */
+static void __init zynq_smp_init_cpus(void)
+{
+	int i;
+
+	ncores = scu_get_core_count(zynq_scu_base);
+
+	for (i = 0; i < ncores && i < CONFIG_NR_CPUS; i++)
+		set_cpu_possible(i, true);
+}
+
+static void __init zynq_smp_prepare_cpus(unsigned int max_cpus)
+{
+	scu_enable(zynq_scu_base);
+}
+
+/**
+ * zynq_secondary_init - Initialize secondary CPU cores
+ * @cpu:	CPU that is initialized
+ *
+ * This function is in the hotplug path. Don't move it into the
+ * init section!!
+ */
+static void zynq_secondary_init(unsigned int cpu)
+{
+	zynq_core_pm_init();
+}
+
+#ifdef CONFIG_HOTPLUG_CPU
+static int zynq_cpu_kill(unsigned cpu)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(50);
+
+	while (zynq_slcr_cpu_state_read(cpu))
+		if (time_after(jiffies, timeout))
+			return 0;
+
+	zynq_slcr_cpu_stop(cpu);
+	return 1;
+}
+
+/**
+ * zynq_cpu_die - Let a CPU core die
+ * @cpu:	Dying CPU
+ *
+ * Platform-specific code to shutdown a CPU.
+ * Called with IRQs disabled on the dying CPU.
+ */
+static void zynq_cpu_die(unsigned int cpu)
+{
+	zynq_slcr_cpu_state_write(cpu, true);
+
+	/*
+	 * there is no power-control hardware on this platform, so all
+	 * we can do is put the core into WFI; this is safe as the calling
+	 * code will have already disabled interrupts
+	 */
+	for (;;)
+		cpu_do_idle();
+}
+#endif
+
+const struct smp_operations zynq_smp_ops __initconst = {
+	.smp_init_cpus		= zynq_smp_init_cpus,
+	.smp_prepare_cpus	= zynq_smp_prepare_cpus,
+	.smp_boot_secondary	= zynq_boot_secondary,
+	.smp_secondary_init	= zynq_secondary_init,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_die		= zynq_cpu_die,
+	.cpu_kill		= zynq_cpu_kill,
+#endif
+};
diff --git a/arch/arm/mach-zynq/pm.c b/arch/arm/mach-zynq/pm.c
new file mode 100644
index 0000000000..61ad965ef3
--- /dev/null
+++ b/arch/arm/mach-zynq/pm.c
@@ -0,0 +1,71 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Zynq power management
+ *
+ *  Copyright (C) 2012 - 2014 Xilinx
+ *
+ *  Sören Brinkmann <soren.brinkmann@xilinx.com>
+ */
+
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include "common.h"
+
+/* register offsets */
+#define DDRC_CTRL_REG1_OFFS		0x60
+#define DDRC_DRAM_PARAM_REG3_OFFS	0x20
+
+/* bitfields */
+#define DDRC_CLOCKSTOP_MASK	BIT(23)
+#define DDRC_SELFREFRESH_MASK	BIT(12)
+
+static void __iomem *ddrc_base;
+
+/**
+ * zynq_pm_ioremap() - Create IO mappings
+ * @comp:	DT compatible string
+ * Return: Pointer to the mapped memory or NULL.
+ *
+ * Remap the memory region for a compatible DT node.
+ */
+static void __iomem *zynq_pm_ioremap(const char *comp)
+{
+	struct device_node *np;
+	void __iomem *base = NULL;
+
+	np = of_find_compatible_node(NULL, NULL, comp);
+	if (np) {
+		base = of_iomap(np, 0);
+		of_node_put(np);
+	} else {
+		pr_warn("%s: no compatible node found for '%s'\n", __func__,
+				comp);
+	}
+
+	return base;
+}
+
+/**
+ * zynq_pm_late_init() - Power management init
+ *
+ * Initialization of power management related features and infrastructure.
+ */
+void __init zynq_pm_late_init(void)
+{
+	u32 reg;
+
+	ddrc_base = zynq_pm_ioremap("xlnx,zynq-ddrc-a05");
+	if (!ddrc_base) {
+		pr_warn("%s: Unable to map DDRC IO memory.\n", __func__);
+	} else {
+		/*
+		 * Enable DDRC clock stop feature. The HW takes care of
+		 * entering/exiting the correct mode depending
+		 * on activity state.
+		 */
+		reg = readl(ddrc_base + DDRC_DRAM_PARAM_REG3_OFFS);
+		reg |= DDRC_CLOCKSTOP_MASK;
+		writel(reg, ddrc_base + DDRC_DRAM_PARAM_REG3_OFFS);
+	}
+}
diff --git a/arch/arm/mach-zynq/slcr.c b/arch/arm/mach-zynq/slcr.c
new file mode 100644
index 0000000000..9765b3f4c2
--- /dev/null
+++ b/arch/arm/mach-zynq/slcr.c
@@ -0,0 +1,233 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Xilinx SLCR driver
+ *
+ * Copyright (c) 2011-2013 Xilinx Inc.
+ */
+
+#include <linux/io.h>
+#include <linux/reboot.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_address.h>
+#include <linux/regmap.h>
+#include <linux/clk/zynq.h>
+#include "common.h"
+
+/* register offsets */
+#define SLCR_UNLOCK_OFFSET		0x8   /* SCLR unlock register */
+#define SLCR_PS_RST_CTRL_OFFSET		0x200 /* PS Software Reset Control */
+#define SLCR_A9_CPU_RST_CTRL_OFFSET	0x244 /* CPU Software Reset Control */
+#define SLCR_REBOOT_STATUS_OFFSET	0x258 /* PS Reboot Status */
+#define SLCR_PSS_IDCODE			0x530 /* PS IDCODE */
+#define SLCR_L2C_RAM			0xA1C /* L2C_RAM in AR#54190 */
+
+#define SLCR_UNLOCK_MAGIC		0xDF0D
+#define SLCR_A9_CPU_CLKSTOP		0x10
+#define SLCR_A9_CPU_RST			0x1
+#define SLCR_PSS_IDCODE_DEVICE_SHIFT	12
+#define SLCR_PSS_IDCODE_DEVICE_MASK	0x1F
+
+static void __iomem *zynq_slcr_base;
+static struct regmap *zynq_slcr_regmap;
+
+/**
+ * zynq_slcr_write - Write to a register in SLCR block
+ *
+ * @val:	Value to write to the register
+ * @offset:	Register offset in SLCR block
+ *
+ * Return:	a negative value on error, 0 on success
+ */
+static int zynq_slcr_write(u32 val, u32 offset)
+{
+	return regmap_write(zynq_slcr_regmap, offset, val);
+}
+
+/**
+ * zynq_slcr_read - Read a register in SLCR block
+ *
+ * @val:	Pointer to value to be read from SLCR
+ * @offset:	Register offset in SLCR block
+ *
+ * Return:	a negative value on error, 0 on success
+ */
+static int zynq_slcr_read(u32 *val, u32 offset)
+{
+	return regmap_read(zynq_slcr_regmap, offset, val);
+}
+
+/**
+ * zynq_slcr_unlock - Unlock SLCR registers
+ *
+ * Return:	a negative value on error, 0 on success
+ */
+static inline int zynq_slcr_unlock(void)
+{
+	zynq_slcr_write(SLCR_UNLOCK_MAGIC, SLCR_UNLOCK_OFFSET);
+
+	return 0;
+}
+
+/**
+ * zynq_slcr_get_device_id - Read device code id
+ *
+ * Return:	Device code id
+ */
+u32 zynq_slcr_get_device_id(void)
+{
+	u32 val;
+
+	zynq_slcr_read(&val, SLCR_PSS_IDCODE);
+	val >>= SLCR_PSS_IDCODE_DEVICE_SHIFT;
+	val &= SLCR_PSS_IDCODE_DEVICE_MASK;
+
+	return val;
+}
+
+/**
+ * zynq_slcr_system_restart - Restart the entire system.
+ *
+ * @nb:		Pointer to restart notifier block (unused)
+ * @action:	Reboot mode (unused)
+ * @data:	Restart handler private data (unused)
+ *
+ * Return:	0 always
+ */
+static
+int zynq_slcr_system_restart(struct notifier_block *nb,
+			     unsigned long action, void *data)
+{
+	u32 reboot;
+
+	/*
+	 * Clear 0x0F000000 bits of reboot status register to workaround
+	 * the FSBL not loading the bitstream after soft-reboot
+	 * This is a temporary solution until we know more.
+	 */
+	zynq_slcr_read(&reboot, SLCR_REBOOT_STATUS_OFFSET);
+	zynq_slcr_write(reboot & 0xF0FFFFFF, SLCR_REBOOT_STATUS_OFFSET);
+	zynq_slcr_write(1, SLCR_PS_RST_CTRL_OFFSET);
+	return 0;
+}
+
+static struct notifier_block zynq_slcr_restart_nb = {
+	.notifier_call	= zynq_slcr_system_restart,
+	.priority	= 192,
+};
+
+/**
+ * zynq_slcr_cpu_start - Start cpu
+ * @cpu:	cpu number
+ */
+void zynq_slcr_cpu_start(int cpu)
+{
+	u32 reg;
+
+	zynq_slcr_read(&reg, SLCR_A9_CPU_RST_CTRL_OFFSET);
+	reg &= ~(SLCR_A9_CPU_RST << cpu);
+	zynq_slcr_write(reg, SLCR_A9_CPU_RST_CTRL_OFFSET);
+	reg &= ~(SLCR_A9_CPU_CLKSTOP << cpu);
+	zynq_slcr_write(reg, SLCR_A9_CPU_RST_CTRL_OFFSET);
+
+	zynq_slcr_cpu_state_write(cpu, false);
+}
+
+/**
+ * zynq_slcr_cpu_stop - Stop cpu
+ * @cpu:	cpu number
+ */
+void zynq_slcr_cpu_stop(int cpu)
+{
+	u32 reg;
+
+	zynq_slcr_read(&reg, SLCR_A9_CPU_RST_CTRL_OFFSET);
+	reg |= (SLCR_A9_CPU_CLKSTOP | SLCR_A9_CPU_RST) << cpu;
+	zynq_slcr_write(reg, SLCR_A9_CPU_RST_CTRL_OFFSET);
+}
+
+/**
+ * zynq_slcr_cpu_state - Read/write cpu state
+ * @cpu:	cpu number
+ *
+ * SLCR_REBOOT_STATUS save upper 2 bits (31/30 cpu states for cpu0 and cpu1)
+ * 0 means cpu is running, 1 cpu is going to die.
+ *
+ * Return: true if cpu is running, false if cpu is going to die
+ */
+bool zynq_slcr_cpu_state_read(int cpu)
+{
+	u32 state;
+
+	state = readl(zynq_slcr_base + SLCR_REBOOT_STATUS_OFFSET);
+	state &= 1 << (31 - cpu);
+
+	return !state;
+}
+
+/**
+ * zynq_slcr_cpu_state - Read/write cpu state
+ * @cpu:	cpu number
+ * @die:	cpu state - true if cpu is going to die
+ *
+ * SLCR_REBOOT_STATUS save upper 2 bits (31/30 cpu states for cpu0 and cpu1)
+ * 0 means cpu is running, 1 cpu is going to die.
+ */
+void zynq_slcr_cpu_state_write(int cpu, bool die)
+{
+	u32 state, mask;
+
+	state = readl(zynq_slcr_base + SLCR_REBOOT_STATUS_OFFSET);
+	mask = 1 << (31 - cpu);
+	if (die)
+		state |= mask;
+	else
+		state &= ~mask;
+	writel(state, zynq_slcr_base + SLCR_REBOOT_STATUS_OFFSET);
+}
+
+/**
+ * zynq_early_slcr_init - Early slcr init function
+ *
+ * Return:	0 on success, negative errno otherwise.
+ *
+ * Called very early during boot from platform code to unlock SLCR.
+ */
+int __init zynq_early_slcr_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_compatible_node(NULL, NULL, "xlnx,zynq-slcr");
+	if (!np) {
+		pr_err("%s: no slcr node found\n", __func__);
+		BUG();
+	}
+
+	zynq_slcr_base = of_iomap(np, 0);
+	if (!zynq_slcr_base) {
+		pr_err("%s: Unable to map I/O memory\n", __func__);
+		BUG();
+	}
+
+	np->data = (__force void *)zynq_slcr_base;
+
+	zynq_slcr_regmap = syscon_regmap_lookup_by_compatible("xlnx,zynq-slcr");
+	if (IS_ERR(zynq_slcr_regmap)) {
+		pr_err("%s: failed to find zynq-slcr\n", __func__);
+		of_node_put(np);
+		return -ENODEV;
+	}
+
+	/* unlock the SLCR so that registers can be changed */
+	zynq_slcr_unlock();
+
+	/* See AR#54190 design advisory */
+	regmap_update_bits(zynq_slcr_regmap, SLCR_L2C_RAM, 0x70707, 0x20202);
+
+	register_restart_handler(&zynq_slcr_restart_nb);
+
+	pr_info("%pOFn mapped to %p\n", np, zynq_slcr_base);
+
+	of_node_put(np);
+
+	return 0;
+}