[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/kernel/linux/v4.14/arch/arm/mach-zynq/Kconfig b/src/kernel/linux/v4.14/arch/arm/mach-zynq/Kconfig
new file mode 100644
index 0000000..1ca633e
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/Kconfig
@@ -0,0 +1,19 @@
+# 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 ARCH_SUPPORTS_BIG_ENDIAN
+ select ARM_AMBA
+ select ARM_GIC
+ select ARM_GLOBAL_TIMER if !CPU_FREQ
+ select CADENCE_TTC_TIMER
+ select HAVE_ARM_SCU if SMP
+ select HAVE_ARM_TWD if SMP
+ select ICST
+ select MFD_SYSCON
+ select PINCTRL
+ select PINCTRL_ZYNQ
+ select SOC_BUS
+ help
+ Support for Xilinx Zynq ARM Cortex A9 Platform
diff --git a/src/kernel/linux/v4.14/arch/arm/mach-zynq/Makefile b/src/kernel/linux/v4.14/arch/arm/mach-zynq/Makefile
new file mode 100644
index 0000000..b03a97e
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/Makefile
@@ -0,0 +1,7 @@
+#
+# 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/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.c b/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.c
new file mode 100644
index 0000000..6aba9eb
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.c
@@ -0,0 +1,206 @@
+/*
+ * This file contains common code that is intended to be used across
+ * boards so that it's not replicated.
+ *
+ * Copyright (C) 2011 Xilinx
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/cpumask.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clk/zynq.h>
+#include <linux/clocksource.h>
+#include <linux/of_address.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 <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/pgtable.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);
+ 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/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.h b/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.h
new file mode 100644
index 0000000..e771933
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/common.h
@@ -0,0 +1,51 @@
+/*
+ * This file contains common function prototypes to avoid externs
+ * in the c files.
+ *
+ * Copyright (C) 2011 Xilinx
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#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/src/kernel/linux/v4.14/arch/arm/mach-zynq/headsmp.S b/src/kernel/linux/v4.14/arch/arm/mach-zynq/headsmp.S
new file mode 100644
index 0000000..f6d5de0
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/headsmp.S
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2013 Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ * Copyright (c) 2012-2013 Xilinx
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/assembler.h>
+
+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/src/kernel/linux/v4.14/arch/arm/mach-zynq/platsmp.c b/src/kernel/linux/v4.14/arch/arm/mach-zynq/platsmp.c
new file mode 100644
index 0000000..b296ada
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/platsmp.c
@@ -0,0 +1,169 @@
+/*
+ * 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.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/export.h>
+#include <linux/jiffies.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <asm/cacheflush.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;
+
+ /* 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(cpu);
+ 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(cpu);
+
+ 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), 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/src/kernel/linux/v4.14/arch/arm/mach-zynq/pm.c b/src/kernel/linux/v4.14/arch/arm/mach-zynq/pm.c
new file mode 100644
index 0000000..fa44fc1
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/pm.c
@@ -0,0 +1,83 @@
+/*
+ * Zynq power management
+ *
+ * Copyright (C) 2012 - 2014 Xilinx
+ *
+ * Sören Brinkmann <soren.brinkmann@xilinx.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_device.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/src/kernel/linux/v4.14/arch/arm/mach-zynq/slcr.c b/src/kernel/linux/v4.14/arch/arm/mach-zynq/slcr.c
new file mode 100644
index 0000000..f0292a3
--- /dev/null
+++ b/src/kernel/linux/v4.14/arch/arm/mach-zynq/slcr.c
@@ -0,0 +1,241 @@
+/*
+ * Xilinx SLCR driver
+ *
+ * Copyright (c) 2011-2013 Xilinx Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the Free
+ * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA
+ * 02139, USA.
+ */
+
+#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(®, 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(®, 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__);
+ 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("%s mapped to %p\n", np->name, zynq_slcr_base);
+
+ of_node_put(np);
+
+ return 0;
+}