3 files changed, 26 insertions, 16 deletions

diff --git a/arch/arm/mach-socfpga/core.h b/arch/arm/mach-socfpga/core.h
index 315edff610f2..572b8f719ffb 100644
--- a/arch/arm/mach-socfpga/core.h
+++ b/arch/arm/mach-socfpga/core.h
@@ -20,12 +20,23 @@
 #ifndef __MACH_CORE_H
 #define __MACH_CORE_H
 
+#define SOCFPGA_RSTMGR_CTRL     0x04
+#define SOCFPGA_RSTMGR_MODPERRST        0x14
+#define SOCFPGA_RSTMGR_BRGMODRST        0x1c
+
+/* System Manager bits */
+#define RSTMGR_CTRL_SWCOLDRSTREQ        0x1     /* Cold Reset */
+#define RSTMGR_CTRL_SWWARMRSTREQ        0x2     /* Warm Reset */
+
 extern void socfpga_secondary_startup(void);
 extern void __iomem *socfpga_scu_base_addr;
 
 extern void socfpga_init_clocks(void);
 extern void socfpga_sysmgr_init(void);
 
+extern void __iomem *sys_manager_base_addr;
+extern void __iomem *rst_manager_base_addr;
+
 extern struct smp_operations socfpga_smp_ops;
 extern char secondary_trampoline, secondary_trampoline_end;
 
diff --git a/arch/arm/mach-socfpga/platsmp.c b/arch/arm/mach-socfpga/platsmp.c
index 84c60fa8daa0..b51ce8c7929d 100644
--- a/arch/arm/mach-socfpga/platsmp.c
+++ b/arch/arm/mach-socfpga/platsmp.c
@@ -22,7 +22,6 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
-#include <linux/irqchip/arm-gic.h>
 
 #include <asm/cacheflush.h>
 #include <asm/smp_scu.h>
@@ -30,19 +29,6 @@
 
 #include "core.h"
 
-extern void __iomem *sys_manager_base_addr;
-extern void __iomem *rst_manager_base_addr;
-
-static void __cpuinit socfpga_secondary_init(unsigned int cpu)
-{
-        /*
-         * if any interrupts are already enabled for the primary
-         * core (e.g. timer irq), then they will not have been enabled
-         * for us: do so
-         */
-        gic_secondary_init(0);
-}
-
 static int __cpuinit socfpga_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
         int trampoline_size = &secondary_trampoline_end - &secondary_trampoline;
@@ -109,7 +95,6 @@ static void socfpga_cpu_die(unsigned int cpu)
 struct smp_operations socfpga_smp_ops __initdata = {
         .smp_init_cpus          = socfpga_smp_init_cpus,
         .smp_prepare_cpus       = socfpga_smp_prepare_cpus,
-        .smp_secondary_init     = socfpga_secondary_init,
         .smp_boot_secondary     = socfpga_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_die                = socfpga_cpu_die,
diff --git a/arch/arm/mach-socfpga/socfpga.c b/arch/arm/mach-socfpga/socfpga.c
index 1042c023cf24..46a051359f02 100644
--- a/arch/arm/mach-socfpga/socfpga.c
+++ b/arch/arm/mach-socfpga/socfpga.c
@@ -15,6 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <linux/dw_apb_timer.h>
+#include <linux/clk-provider.h>
 #include <linux/irqchip.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -29,6 +30,7 @@
 void __iomem *socfpga_scu_base_addr = ((void __iomem *)(SOCFPGA_SCU_VIRT_BASE));
 void __iomem *sys_manager_base_addr;
 void __iomem *rst_manager_base_addr;
+void __iomem *clk_mgr_base_addr;
 unsigned long cpu1start_addr;
 
 static struct map_desc scu_io_desc __initdata = {
@@ -77,6 +79,9 @@ void __init socfpga_sysmgr_init(void)
 
         np = of_find_compatible_node(NULL, NULL, "altr,rst-mgr");
         rst_manager_base_addr = of_iomap(np, 0);
+
+        np = of_find_compatible_node(NULL, NULL, "altr,clk-mgr");
+        clk_mgr_base_addr = of_iomap(np, 0);
 }
 
 static void __init socfpga_init_irq(void)
@@ -87,13 +92,22 @@ static void __init socfpga_init_irq(void)
 
 static void socfpga_cyclone5_restart(char mode, const char *cmd)
 {
-        /* TODO: */
+        u32 temp;
+
+        temp = readl(rst_manager_base_addr + SOCFPGA_RSTMGR_CTRL);
+
+        if (mode == 'h')
+                temp |= RSTMGR_CTRL_SWCOLDRSTREQ;
+        else
+                temp |= RSTMGR_CTRL_SWWARMRSTREQ;
+        writel(temp, rst_manager_base_addr + SOCFPGA_RSTMGR_CTRL);
 }
 
 static void __init socfpga_cyclone5_init(void)
 {
         l2x0_of_init(0, ~0UL);
         of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
+        of_clk_init(NULL);
         socfpga_init_clocks();
 }