(v3) arm: SMP boot

diff -r 10c5ba0b5af2 -r 2672314f141d xen/arch/arm/time.c
@@ -171,6 +171,16 @@ void __cpuinit init_timer_interrupt(void
     request_irq(30, timer_interrupt, 0, "phytimer", NULL);
 }
 
+/* Wait a set number of microseconds */
+void udelay(unsigned long usecs)
+{
+    s_time_t deadline = get_s_time() + 1000 * (s_time_t) usecs;
+    while ( get_s_time() - deadline < 0 )
+        ;
+    dsb();
+    isb();
+}
+
 /*
  * Local variables:
  * mode: C

diff -r 10c5ba0b5af2 -r 2672314f141d xen/include/asm-arm/delay.h
@@ -1,8 +1,7 @@
 #ifndef _ARM_DELAY_H
 #define _ARM_DELAY_H
 
-extern void __udelay(unsigned long usecs);
-#define udelay(n) __udelay(n)
+extern void udelay(unsigned long usecs);
 
 #endif /* defined(_ARM_DELAY_H) */
 /*

diff -r 2672314f141d -r ad97676f4dd1 xen/arch/arm/gic.c
@@ -224,7 +224,9 @@ static void __cpuinit gic_cpu_init(void)
 {
     int i;
 
-    /* Disable all PPI and enable all SGI */
+    /* The first 32 interrupts (PPI and SGI) are banked per-cpu, so 
+     * even though they are controlled with GICD registers, they must 
+     * be set up here with the other per-cpu state. */
     GICD[GICD_ICENABLER] = 0xffff0000; /* Disable all PPI */
     GICD[GICD_ISENABLER] = 0x0000ffff; /* Enable all SGI */
     /* Set PPI and SGI priorities */

diff -r ad97676f4dd1 -r 849c8e3ac747 xen/arch/arm/gic.c
@@ -252,7 +252,7 @@ static void __cpuinit gic_hyp_init(void)
 }
 
 /* Set up the GIC */
-void gic_init(void)
+int __init gic_init(void)
 {
     /* XXX FIXME get this from devicetree */
     gic.dbase = GIC_BASE_ADDRESS + GIC_DR_OFFSET;

@@ -274,6 +274,8 @@ void gic_init(void)
     gic_hyp_init();
 
     spin_unlock(&gic.lock);
+
+    return gic.cpus;
 }
 
 void gic_route_irqs(void)

diff -r ad97676f4dd1 -r 849c8e3ac747 xen/arch/arm/gic.h
@@ -138,8 +138,8 @@ extern int gic_route_irq_to_guest(struct
 
 /* Accept an interrupt from the GIC and dispatch its handler */
 extern void gic_interrupt(struct cpu_user_regs *regs, int is_fiq);
-/* Bring up the interrupt controller */
-extern void gic_init(void);
+/* Bring up the interrupt controller, and report # cpus attached */
+extern int gic_init(void);
 /* setup the gic virtual interface for a guest */
 extern void gicv_setup(struct domain *d);
 #endif

diff -r ad97676f4dd1 -r 849c8e3ac747 xen/arch/arm/mm.c
@@ -36,6 +36,9 @@ lpae_t xen_second[LPAE_ENTRIES*4] __attr
 static lpae_t xen_fixmap[LPAE_ENTRIES] __attribute__((__aligned__(4096)));
 static lpae_t xen_xenmap[LPAE_ENTRIES] __attribute__((__aligned__(4096)));
 
+/* Non-boot CPUs use this to find the correct pagetables. */
+uint64_t boot_httbr;
+
 /* Limits of the Xen heap */
 unsigned long xenheap_mfn_start, xenheap_mfn_end;
 unsigned long xenheap_virt_end;

@@ -156,14 +159,6 @@ void __init setup_pagetables(unsigned lo
     lpae_t pte, *p;
     int i;
 
-    if ( boot_phys_offset != 0 )
-    {
-        /* Remove the old identity mapping of the boot paddr */
-        pte.bits = 0;
-        dest_va = (unsigned long)_start + boot_phys_offset;
-        write_pte(xen_second + second_linear_offset(dest_va), pte);
-    }
-
     xen_paddr = device_tree_get_xen_paddr();
 
     /* Map the destination in the boot misc area. */

@@ -186,11 +181,18 @@ void __init setup_pagetables(unsigned lo
     for ( i = 0; i < 4; i++)
         p[i].pt.base += (phys_offset - boot_phys_offset) >> PAGE_SHIFT;
     p = (void *) xen_second + dest_va - (unsigned long) _start;
+    if ( boot_phys_offset != 0 )
+    {
+        /* Remove the old identity mapping of the boot paddr */
+        unsigned long va = (unsigned long)_start + boot_phys_offset;
+        p[second_linear_offset(va)].bits = 0;
+    }
     for ( i = 0; i < 4 * LPAE_ENTRIES; i++)
         if ( p[i].pt.valid )
                 p[i].pt.base += (phys_offset - boot_phys_offset) >> PAGE_SHIFT;
 
     /* Change pagetables to the copy in the relocated Xen */
+    boot_httbr = (unsigned long) xen_pgtable + phys_offset;
     asm volatile (
         STORE_CP64(0, HTTBR)          /* Change translation base */
         "dsb;"                        /* Ensure visibility of HTTBR update */

@@ -198,7 +200,7 @@ void __init setup_pagetables(unsigned lo
         STORE_CP32(0, BPIALL)         /* Flush branch predictor */
         "dsb;"                        /* Ensure completion of TLB+BP flush */
         "isb;"
-        : : "r" ((unsigned long) xen_pgtable + phys_offset) : "memory");
+        : : "r" (boot_httbr) : "memory");
 
     /* Undo the temporary map */
     pte.bits = 0;

diff -r ad97676f4dd1 -r 849c8e3ac747 xen/arch/arm/setup.c
@@ -44,7 +44,12 @@ static unsigned int __initdata max_cpus 
 /* Xen stack for bringing up the first CPU. */
 unsigned char __initdata init_stack[STACK_SIZE] __attribute__((__aligned__(STACK_SIZE)));
 
-extern char __init_begin[], __init_end[], __bss_start[];
+extern const char __init_begin[], __init_end[], __bss_start[];
+
+/* Spinlock for serializing CPU bringup */
+unsigned long __initdata boot_gate = 1;
+/* Number of non-boot CPUs ready to enter C */
+unsigned long __initdata ready_cpus = 0;
 
 static __attribute_used__ void init_done(void)
 {

@@ -151,14 +156,17 @@ static void __init setup_mm(unsigned lon
     end_boot_allocator();
 }
 
+/* C entry point for boot CPU */
 void __init start_xen(unsigned long boot_phys_offset,
                       unsigned long arm_type,
-                      unsigned long atag_paddr)
-
+                      unsigned long atag_paddr,
+                      unsigned long cpuid)
 {
     void *fdt;
     size_t fdt_size;
-    int i;
+    int cpus, i;
+    paddr_t gate_pa;
+    unsigned long *gate;
 
     fdt = (void *)BOOT_MISC_VIRT_START
         + (atag_paddr & ((1 << SECOND_SHIFT) - 1));

@@ -174,14 +182,26 @@ void __init start_xen(unsigned long boot
     console_init_preirq();
 #endif
 
+    cpus = gic_init();
+
+    printk("Waiting for %i other CPUs to be ready\n", cpus - 1);
+    /* Bring the other CPUs up to paging before the original
+     * copy of .text gets overwritten.  We need to use the unrelocated
+     * copy of boot_gate as that's the one the others can see. */ 
+    gate_pa = ((unsigned long) &boot_gate) + boot_phys_offset;
+    gate = map_domain_page(gate_pa >> PAGE_SHIFT) + (gate_pa & ~PAGE_MASK); 
+    *gate = 0;
+    unmap_domain_page(gate);
+    /* Now send an event to wake the first non-boot CPU */
+    asm volatile("dsb; isb; sev");
+    /* And wait for them all to be ready. */
+    while ( ready_cpus + 1 < cpus )
+        smp_rmb();
+
     __set_current((struct vcpu *)0xfffff000); /* debug sanity */
     idle_vcpu[0] = current;
     set_processor_id(0); /* needed early, for smp_processor_id() */
 
-    /* TODO: smp_prepare_boot_cpu(void) */
-    cpumask_set_cpu(smp_processor_id(), &cpu_online_map);
-    cpumask_set_cpu(smp_processor_id(), &cpu_present_map);
-
     smp_prepare_cpus(max_cpus);
 
     init_xen_time();

@@ -208,8 +228,6 @@ void __init start_xen(unsigned long boot
 
     init_IRQ();
 
-    gic_init();
-
     gic_route_irqs();
 
     init_maintenance_interrupt();

85 lines not on this page · raw message

diff -r 849c8e3ac747 -r 5254b3d4015e xen/arch/arm/setup.c
@@ -182,6 +182,9 @@ void __init start_xen(unsigned long boot
     console_init_preirq();
 #endif
 
+    percpu_init_areas();
+    set_processor_id(0); /* needed early, for smp_processor_id() */
+
     cpus = gic_init();
 
     printk("Waiting for %i other CPUs to be ready\n", cpus - 1);

@@ -200,7 +203,6 @@ void __init start_xen(unsigned long boot
 
     __set_current((struct vcpu *)0xfffff000); /* debug sanity */
     idle_vcpu[0] = current;
-    set_processor_id(0); /* needed early, for smp_processor_id() */
 
     smp_prepare_cpus(max_cpus);
 

diff -r 849c8e3ac747 -r 5254b3d4015e xen/include/asm-arm/current.h
@@ -40,7 +40,9 @@ static inline struct cpu_info *get_cpu_i
 #define get_current()         (this_cpu(curr_vcpu))
 #define __set_current(vcpu)   (this_cpu(curr_vcpu) = (vcpu))
 #define set_current(vcpu)     do {                                      \
-    vcpu->arch.cpu_info->processor_id = get_processor_id();             \
+    int cpu = get_processor_id();                                       \
+    vcpu->arch.cpu_info->processor_id = cpu;                            \
+    vcpu->arch.cpu_info->per_cpu_offset = __per_cpu_offset[cpu];        \
     __set_current(vcpu);                                                \
 } while (0)
 #define current               (get_current())

diff -r 849c8e3ac747 -r 5254b3d4015e xen/include/asm-arm/percpu.h
@@ -12,8 +12,11 @@ void percpu_init_areas(void);
     __attribute__((__section__(".bss.percpu" #suffix)))         \
     __typeof__(type) per_cpu_##name
 
-#define per_cpu(var, cpu) ((&per_cpu__##var)[cpu?0:0])
-#define __get_cpu_var(var) per_cpu__##var
+
+#define per_cpu(var, cpu)  \
+    (*RELOC_HIDE(&per_cpu__##var, __per_cpu_offset[cpu]))
+#define __get_cpu_var(var) \
+    (*RELOC_HIDE(&per_cpu__##var, get_cpu_info()->per_cpu_offset))
 
 #define DECLARE_PER_CPU(type, name) extern __typeof__(type) per_cpu__##name

diff -r 5254b3d4015e -r 33d06f47c02a xen/arch/arm/setup.c
@@ -38,9 +38,6 @@
 #include <asm/setup.h>
 #include "gic.h"
 
-/* maxcpus: maximum number of CPUs to activate. */
-static unsigned int __initdata max_cpus = NR_CPUS;
-
 /* Xen stack for bringing up the first CPU. */
 unsigned char __initdata init_stack[STACK_SIZE] __attribute__((__aligned__(STACK_SIZE)));
 

@@ -204,7 +201,7 @@ void __init start_xen(unsigned long boot
     __set_current((struct vcpu *)0xfffff000); /* debug sanity */
     idle_vcpu[0] = current;
 
-    smp_prepare_cpus(max_cpus);
+    smp_prepare_cpus(cpus);
 
     init_xen_time();
 

@@ -251,7 +248,7 @@ void __init start_xen(unsigned long boot
 
     for_each_present_cpu ( i )
     {
-        if ( (num_online_cpus() < max_cpus) && !cpu_online(i) )
+        if ( (num_online_cpus() < cpus) && !cpu_online(i) )
         {
             int ret = cpu_up(i);
             if ( ret != 0 )

55 lines not on this page · raw message

diff -r 33d06f47c02a -r 60f25ee4c640 xen/arch/arm/gic.c
@@ -245,7 +245,6 @@ static void __cpuinit gic_hyp_init(void)
 
     vtr = GICH[GICH_VTR];
     nr_lrs  = (vtr & GICH_VTR_NRLRGS) + 1;
-    printk("GICH: %d list registers available\n", nr_lrs);
 
     GICH[GICH_HCR] = GICH_HCR_EN;
     GICH[GICH_MISR] = GICH_MISR_EOI;

@@ -278,6 +277,15 @@ int __init gic_init(void)
     return gic.cpus;
 }
 
+/* Set up the per-CPU parts of the GIC for a secondary CPU */
+void __cpuinit gic_init_secondary_cpu(void)
+{
+    spin_lock(&gic.lock);
+    gic_cpu_init();
+    gic_hyp_init();
+    spin_unlock(&gic.lock);
+}
+
 void gic_route_irqs(void)
 {
     /* XXX should get these from DT */

diff -r 33d06f47c02a -r 60f25ee4c640 xen/arch/arm/gic.h
@@ -140,6 +140,8 @@ extern int gic_route_irq_to_guest(struct
 extern void gic_interrupt(struct cpu_user_regs *regs, int is_fiq);
 /* Bring up the interrupt controller, and report # cpus attached */
 extern int gic_init(void);
+/* Bring up a secondary CPU's per-CPU GIC interface */
+extern void gic_init_secondary_cpu(void);
 /* setup the gic virtual interface for a guest */
 extern void gicv_setup(struct domain *d);
 #endif

diff -r 33d06f47c02a -r 60f25ee4c640 xen/arch/arm/setup.c
@@ -38,9 +38,6 @@
 #include <asm/setup.h>
 #include "gic.h"
 
-/* Xen stack for bringing up the first CPU. */
-unsigned char __initdata init_stack[STACK_SIZE] __attribute__((__aligned__(STACK_SIZE)));
-
 extern const char __init_begin[], __init_end[], __bss_start[];
 
 /* Spinlock for serializing CPU bringup */

@@ -286,7 +283,11 @@ void __init start_xen(unsigned long boot
 
     domain_unpause_by_systemcontroller(dom0);
 
-    reset_stack_and_jump(init_done);
+    /* Switch on to the dynamically allocated stack for the idle vcpu
+     * since the static one we're running on is about to be freed. */
+    memcpy(idle_vcpu[0]->arch.cpu_info, get_cpu_info(), 
+           sizeof(struct cpu_info));
+    switch_stack_and_jump(idle_vcpu[0]->arch.cpu_info, init_done);
 }
 
 void arch_get_xen_caps(xen_capabilities_info_t *info)

103 lines not on this page · raw message

diff -r 33d06f47c02a -r 60f25ee4c640 xen/include/asm-arm/current.h
@@ -49,10 +49,10 @@ static inline struct cpu_info *get_cpu_i
 
 #define guest_cpu_user_regs() (&get_cpu_info()->guest_cpu_user_regs)
 
-#define reset_stack_and_jump(__fn)              \
-    __asm__ __volatile__ (                      \
-        "mov sp,%0; b "STR(__fn)      \
-        : : "r" (guest_cpu_user_regs()) : "memory" )
+#define switch_stack_and_jump(stack, fn)                                \
+    asm volatile ("mov sp,%0; b " STR(fn) : : "r" (stack) : "memory" )
+
+#define reset_stack_and_jump(fn) switch_stack_and_jump(get_cpu_info(), fn)
 
 #endif

diff -r 60f25ee4c640 -r 0bfd379d2501 xen/arch/arm/domain.c
@@ -31,12 +31,10 @@ void idle_loop(void)
 {
     for ( ; ; )
     {
-        /* TODO
-           if ( cpu_is_offline(smp_processor_id()) )
-           play_dead();
-           (*pm_idle)();
-           BUG();
-        */
+        if ( cpu_is_offline(smp_processor_id()) )
+            stop_cpu();
+
+        /* TODO: (*pm_idle)(); */
         do_tasklet();
         do_softirq();
     }

diff -r 60f25ee4c640 -r 0bfd379d2501 xen/arch/arm/gic.c
@@ -239,6 +239,11 @@ static void __cpuinit gic_cpu_init(void)
     GICC[GICC_CTLR] = GICC_CTL_ENABLE|GICC_CTL_EOI;    /* Turn on delivery */
 }
 
+static void gic_cpu_disable(void)
+{
+    GICC[GICC_CTLR] = 0;
+}
+
 static void __cpuinit gic_hyp_init(void)
 {
     uint32_t vtr;

@@ -250,6 +255,11 @@ static void __cpuinit gic_hyp_init(void)
     GICH[GICH_MISR] = GICH_MISR_EOI;
 }
 
+static void __cpuinit gic_hyp_disable(void)
+{
+    GICH[GICH_HCR] = 0;
+}
+
 /* Set up the GIC */
 int __init gic_init(void)
 {

@@ -286,6 +296,15 @@ void __cpuinit gic_init_secondary_cpu(vo
     spin_unlock(&gic.lock);
 }
 
+/* Shut down the per-CPU GIC interface */
+void gic_disable_cpu(void)
+{
+    spin_lock(&gic.lock);
+    gic_cpu_disable();
+    gic_hyp_disable();
+    spin_unlock(&gic.lock);
+}
+
 void gic_route_irqs(void)
 {
     /* XXX should get these from DT */

diff -r 60f25ee4c640 -r 0bfd379d2501 xen/arch/arm/gic.h
@@ -142,6 +142,8 @@ extern void gic_interrupt(struct cpu_use
 extern int gic_init(void);
 /* Bring up a secondary CPU's per-CPU GIC interface */
 extern void gic_init_secondary_cpu(void);
+/* Take down a CPU's per-CPU GIC interface */
+extern void gic_disable_cpu(void);
 /* setup the gic virtual interface for a guest */
 extern void gicv_setup(struct domain *d);
 #endif

72 lines not on this page · raw message

diff -r 60f25ee4c640 -r 0bfd379d2501 xen/include/asm-arm/smp.h
@@ -14,6 +14,8 @@ DECLARE_PER_CPU(cpumask_var_t, cpu_core_
 
 #define raw_smp_processor_id() (get_processor_id())
 
+extern void stop_cpu(void);
+
 #endif
 /*
  * Local variables:

diff -r 0bfd379d2501 -r 9b746bfac6bb 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 -r 0bfd379d2501 -r 9b746bfac6bb 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__ */
 /*

diff -r 9b746bfac6bb -r 80b8756718af xen/arch/arm/mm.c
@@ -263,6 +263,13 @@ void __init setup_pagetables(unsigned lo
     WRITE_CP32(READ_CP32(HSCTLR) | SCTLR_WXN, HSCTLR);
 }
 
+/* MMU setup for secondary CPUS (which already have paging enabled) */
+void __cpuinit mmu_init_secondary_cpu(void)
+{
+    /* From now on, no mapping may be both writable and executable. */
+    WRITE_CP32(READ_CP32(HSCTLR) | SCTLR_WXN, HSCTLR);
+}
+
 /* Create Xen's mappings of memory.
  * Base and virt must be 32MB aligned and size a multiple of 32MB. */
 static void __init create_mappings(unsigned long virt,

diff -r 9b746bfac6bb -r 80b8756718af xen/arch/arm/smpboot.c
@@ -76,6 +76,7 @@ void __cpuinit start_secondary(unsigned 
 
     dprintk(XENLOG_DEBUG, "CPU %li awake.\n", cpuid);
 
+    mmu_init_secondary_cpu();
     gic_init_secondary_cpu();
 
     set_current(idle_vcpu[cpuid]);

diff -r 9b746bfac6bb -r 80b8756718af xen/include/asm-arm/mm.h
@@ -136,6 +136,8 @@ extern unsigned long total_pages;
 
 /* Boot-time pagetable setup */
 extern void setup_pagetables(unsigned long boot_phys_offset);
+/* MMU setup for seccondary CPUS (which already have paging enabled) */
+extern void __cpuinit mmu_init_secondary_cpu(void);
 /* Set up the xenheap: up to 1GB of contiguous, always-mapped memory.
  * Base must be 32MB aligned and size a multiple of 32MB. */
 extern void setup_xenheap_mappings(unsigned long base_mfn, unsigned long nr_mfns);

diff -r 80b8756718af -r ec6712d0af27 xen/arch/arm/mm.c
@@ -48,6 +48,8 @@ unsigned long frametable_virt_end;
 
 unsigned long max_page;
 
+extern char __init_begin[], __init_end[];
+
 /* Map a 4k page in a fixmap entry */
 void set_fixmap(unsigned map, unsigned long mfn, unsigned attributes)
 {

@@ -205,17 +207,7 @@ void __init setup_pagetables(unsigned lo
     /* Undo the temporary map */
     pte.bits = 0;
     write_pte(xen_second + second_table_offset(dest_va), pte);
-    /*
-     * Have removed a mapping previously used for .text. Flush everything
-     * for safety.
-     */
-    asm volatile (
-        "dsb;"                        /* Ensure visibility of PTE write */
-        STORE_CP32(0, TLBIALLH)       /* Flush hypervisor TLB */
-        STORE_CP32(0, BPIALL)         /* Flush branch predictor */
-        "dsb;"                        /* Ensure completion of TLB+BP flush */
-        "isb;"
-        : : "r" (i /*dummy*/) : "memory");
+    flush_xen_text_tlb();
 
     /* Link in the fixmap pagetable */
     pte = mfn_to_xen_entry((((unsigned long) xen_fixmap) + phys_offset)

@@ -251,13 +243,7 @@ void __init setup_pagetables(unsigned lo
     pte.pt.table = 1;
     write_pte(xen_second + second_linear_offset(XEN_VIRT_START), pte);
     /* Have changed a mapping used for .text. Flush everything for safety. */
-    asm volatile (
-        "dsb;"                        /* Ensure visibility of PTE write */
-        STORE_CP32(0, TLBIALLH)       /* Flush hypervisor TLB */
-        STORE_CP32(0, BPIALL)         /* Flush branch predictor */
-        "dsb;"                        /* Ensure completion of TLB+BP flush */
-        "isb;"
-        : : "r" (i /*dummy*/) : "memory");
+    flush_xen_text_tlb();
 
     /* From now on, no mapping may be both writable and executable. */
     WRITE_CP32(READ_CP32(HSCTLR) | SCTLR_WXN, HSCTLR);

@@ -328,6 +314,64 @@ void __init setup_frametable_mappings(pa
     frametable_virt_end = FRAMETABLE_VIRT_START + (nr_pages * sizeof(struct page_info));
 }
 
+enum mg { mg_clear, mg_ro, mg_rw, mg_rx };
+static void set_pte_flags_on_range(const char *p, unsigned long l, enum mg mg)
+{
+    lpae_t pte;
+    int i;
+
+    ASSERT(is_kernel(p) && is_kernel(p + l));
+
+    /* Can only guard in page granularity */
+    ASSERT(!((unsigned long) p & ~PAGE_MASK));
+    ASSERT(!(l & ~PAGE_MASK));
+
+    for ( i = (p - _start) / PAGE_SIZE; 
+          i < (p + l - _start) / PAGE_SIZE; 
+          i++ )
+    {
+        pte = xen_xenmap[i];
+        switch ( mg )
+        {
+        case mg_clear:
+            pte.pt.valid = 0;
+            break;
+        case mg_ro:
+            pte.pt.valid = 1;
+            pte.pt.pxn = 1;
+            pte.pt.xn = 1;
+            pte.pt.ro = 1;
+            break;
+        case mg_rw:
+            pte.pt.valid = 1;
+            pte.pt.pxn = 1;
+            pte.pt.xn = 1;
+            pte.pt.ro = 0;
+            break;
+        case mg_rx:
+            pte.pt.valid = 1;
+            pte.pt.pxn = 0;
+            pte.pt.xn = 0;
+            pte.pt.ro = 1;
+            break;
+        }
+        write_pte(xen_xenmap + i, pte);
+    }
+    flush_xen_text_tlb();
+}
+
+/* Release all __init and __initdata ranges to be reused */
+void free_init_memory(void)
+{
+    paddr_t pa = virt_to_maddr(__init_begin);
+    unsigned long len = __init_end - __init_begin;
+    set_pte_flags_on_range(__init_begin, len, mg_rw);
+    memset(__init_begin, 0xcc, len);
+    set_pte_flags_on_range(__init_begin, len, mg_clear);
+    init_domheap_pages(pa, pa + len);
+    printk("Freed %ldkB init memory.\n", (long)(__init_end-__init_begin)>>10);
+}
+
 void arch_dump_shared_mem_info(void)
 {
 }

diff -r 80b8756718af -r ec6712d0af27 xen/arch/arm/setup.c
@@ -38,8 +38,6 @@
 #include <asm/setup.h>
 #include "gic.h"
 
-extern const char __init_begin[], __init_end[], __bss_start[];
-
 /* Spinlock for serializing CPU bringup */
 unsigned long __initdata boot_gate = 1;
 /* Number of non-boot CPUs ready to enter C */

@@ -47,12 +45,7 @@ unsigned long __initdata ready_cpus = 0;
 
 static __attribute_used__ void init_done(void)
 {
-    /* TODO: free (or page-protect) the init areas.
-       memset(__init_begin, 0xcc, __init_end - __init_begin);
-       free_xen_data(__init_begin, __init_end);
-    */
-    printk("Freed %ldkB init memory.\n", (long)(__init_end-__init_begin)>>10);
-
+    free_init_memory();
     startup_cpu_idle_loop();
 }
 

diff -r 80b8756718af -r ec6712d0af27 xen/include/asm-arm/mm.h
@@ -278,6 +278,10 @@ extern struct domain *dom_xen, *dom_io, 
 #define memguard_guard_stack(_p)       ((void)0)
 #define memguard_guard_range(_p,_l)    ((void)0)
 #define memguard_unguard_range(_p,_l)  ((void)0)
+
+/* Release all __init and __initdata ranges to be reused */
+void free_init_memory(void);
+
 int guest_physmap_mark_populate_on_demand(struct domain *d, unsigned long gfn,
                                           unsigned int order);
 

diff -r 80b8756718af -r ec6712d0af27 xen/include/asm-arm/page.h
@@ -203,6 +203,22 @@ static inline void write_pte(lpae_t *p, 
 }
 
 /*
+ * Flush all hypervisor mappings from the TLB and branch predictor.
+ * This is needed after changing Xen code mappings. 
+ */
+static inline void flush_xen_text_tlb(void)
+{
+    register unsigned long r0 asm ("r0");
+    asm volatile (
+        "dsb;"                        /* Ensure visibility of PTE writes */
+        STORE_CP32(0, TLBIALLH)       /* Flush hypervisor TLB */
+        STORE_CP32(0, BPIALL)         /* Flush branch predictor */
+        "dsb;"                        /* Ensure completion of TLB+BP flush */
+        "isb;"
+        : : "r" (r0) /*dummy*/ : "memory");
+}
+
+/*
  * Flush all hypervisor mappings from the data TLB. This is not
  * sufficient when changing code mappings or for self modifying code.
  */

diff -r ec6712d0af27 -r 258d71e6c048 xen/arch/arm/domain.c
@@ -34,7 +34,11 @@ void idle_loop(void)
         if ( cpu_is_offline(smp_processor_id()) )
             stop_cpu();
 
-        /* TODO: (*pm_idle)(); */
+        local_irq_disable();
+        if ( cpu_is_haltable(smp_processor_id()) )
+            asm volatile ("dsb; wfi");
+        local_irq_enable();
+
         do_tasklet();
         do_softirq();
     }