From 2d311889b532a822e1c68d9fb488910700ae17dd Mon Sep 17 00:00:00 2001
From: Tim Deegan <tim@xen.org>
Date: Tue, 13 Mar 2012 15:10:57 +0000
Subject: [PATCH] arm: Shutdown and reboot

Reboot runes grabbed from linux's SP810 reset function.
Doesn't seem to work on the model, though.

Signed-off-by: Tim Deegan <tim@xen.org>
Acked-by: Ian Campbell <ian.campbell@citrix.com>
Committed-by: Ian Campbell <ian.campbell@citrix.com>
---
 xen/arch/arm/shutdown.c      | 56 ++++++++++++++++++++++++++++++++----
 xen/include/asm-arm/config.h |  3 ++
 2 files changed, 54 insertions(+), 5 deletions(-)

diff --git a/xen/arch/arm/shutdown.c b/xen/arch/arm/shutdown.c
index 2e35d2de7f..12787845d6 100644
--- a/xen/arch/arm/shutdown.c
+++ b/xen/arch/arm/shutdown.c
@@ -1,18 +1,64 @@
 #include <xen/config.h>
+#include <xen/console.h>
+#include <xen/cpu.h>
+#include <xen/delay.h>
 #include <xen/lib.h>
+#include <xen/mm.h>
+#include <xen/smp.h>
+
+static void raw_machine_reset(void)
+{
+    /* XXX get this from device tree */
+#ifdef SP810_ADDRESS
+    /* Use the SP810 system controller to force a reset */
+    volatile uint32_t *sp810;
+    set_fixmap(FIXMAP_MISC, SP810_ADDRESS >> PAGE_SHIFT, DEV_SHARED);
+    sp810 = ((uint32_t *)
+             (FIXMAP_ADDR(FIXMAP_MISC) + (SP810_ADDRESS & ~PAGE_MASK)));
+    sp810[0] = 0x3; /* switch to slow mode */
+    dsb(); isb();
+    sp810[1] = 0x1; /* writing any value to SCSYSSTAT reg will reset system */
+    dsb(); isb();
+    clear_fixmap(FIXMAP_MISC);
+#endif
+}
+
+static void halt_this_cpu(void *arg)
+{
+    __cpu_disable();
+    stop_cpu();
+}
 
 void machine_halt(void)
 {
-        /* TODO: halt */
-        while(1) ;
+    watchdog_disable();
+    console_start_sync();
+    local_irq_enable();
+    smp_call_function(halt_this_cpu, NULL, 0);
+    halt_this_cpu(NULL);
 }
 
 void machine_restart(unsigned int delay_millisecs)
 {
-        /* TODO: restart */
-        printk("Cannot restart yet\n");
-        while(1);
+    int timeout = 10;
+
+    local_irq_enable();
+    smp_call_function(halt_this_cpu, NULL, 0);
+    local_irq_disable();
+
+    mdelay(delay_millisecs);
+
+    /* Wait at most another 10ms for all other CPUs to go offline. */
+    while ( (num_online_cpus() > 1) && (timeout-- > 0) )
+        mdelay(1);
+
+    while ( 1 )
+    {
+        raw_machine_reset();
+        mdelay(100);
+    }
 }
+
 /*
  * Local variables:
  * mode: C
diff --git a/xen/include/asm-arm/config.h b/xen/include/asm-arm/config.h
index c2ab0a21a2..1e4f108f9e 100644
--- a/xen/include/asm-arm/config.h
+++ b/xen/include/asm-arm/config.h
@@ -119,6 +119,9 @@ extern unsigned long frametable_virt_end;
 #define GIC_CR_OFFSET 0x2000
 #define GIC_HR_OFFSET 0x4000 /* Guess work http://lists.infradead.org/pipermail/linux-arm-kernel/2011-September/064219.html */
 #define GIC_VR_OFFSET 0x6000 /* Virtual Machine CPU interface) */
+/* Board-specific: base address of system controller */
+#define SP810_ADDRESS 0x1C020000
+
 
 #endif /* __ARM_CONFIG_H__ */
 /*
-- 
2.30.2