diff --git a/xen/arch/arm/arm32/head.S b/xen/arch/arm/arm32/head.S
index 55781cd..f701bc0 100644
--- a/xen/arch/arm/arm32/head.S
+++ b/xen/arch/arm/arm32/head.S
@@ -72,7 +72,7 @@ past_zImage:
         cpsid aif                    /* Disable all interrupts */
 
         /* Save the bootloader arguments in less-clobberable registers */
-        /* No need to save r1 == Unused ARM-linux machine type */
+        mov   r5, r1                 /* r5: ARM-linux machine type */
         mov   r8, r2                 /* r8 := DTB base address */
 
         /* Find out where we are */
@@ -122,6 +122,20 @@ boot_cpu:
         teq   r12, #0
         bleq  kick_cpus
 
+        /* Secondary CPUs doesn't have machine ID
+         *  - Store machine on boot CPU
+         *  - Load machine ID on secondary CPUs */
+        ldr   r0, =machine_id           /* VA of machine_id */
+        add   r0, r0, r10               /* PA of machine_id */
+        teq   r12, #0
+        streq r5, [r0]                  /* On boot CPU save machine ID */
+        ldrne r5, [r0]                  /* If non boot cpu r5 := machine ID */
+
+        PRINT("- Machine ID ")
+        mov   r0, r5
+        bl    putn
+        PRINT(" -\r\n")
+
         /* Check that this CPU has Hyp mode */
         mrc   CP32(r0, ID_PFR1)
         and   r0, r0, #0xf000        /* Bits 12-15 define virt extensions */
@@ -402,6 +416,9 @@ putn:   mov   pc, lr
 
 #endif /* !EARLY_PRINTK */
 
+/* Place holder for machine ID */
+machine_id: .word 0x0
+
 /*
  * Local variables:
  * mode: ASM
diff --git a/xen/arch/arm/arm32/mode_switch.S b/xen/arch/arm/arm32/mode_switch.S
index d6741d0..ab40f18 100644
--- a/xen/arch/arm/arm32/mode_switch.S
+++ b/xen/arch/arm/arm32/mode_switch.S
@@ -20,14 +20,20 @@
 #include <asm/config.h>
 #include <asm/page.h>
 #include <asm/platforms/vexpress.h>
+#include <asm/platforms/exynos5.h>
 #include <asm/asm_defns.h>
 #include <asm/gic.h>
 
-
-/* XXX: Versatile Express specific code */
-/* wake up secondary cpus */
+/* Wake up secondary cpus
+ * This code relies on Machine ID and only works for Vexpress and the Arndale
+ * TODO: Move this code either later (via platform specific desc) or in a bootwrapper
+ * r5: Machine ID
+ * Clobber r0 r2 */
 .globl kick_cpus
 kick_cpus:
+        ldr   r0, =MACH_TYPE_SMDK5250
+        teq   r5, r0                          /* Are we running on the arndale? */
+        beq   kick_cpus_arndale
         /* write start paddr to v2m sysreg FLAGSSET register */
         ldr   r0, =(V2M_SYS_MMIO_BASE)        /* base V2M sysreg MMIO address */
         dsb
@@ -38,8 +44,20 @@ kick_cpus:
         add   r2, r2, r10
         str   r2, [r0, #(V2M_SYS_FLAGSSET)]
         dsb
+        ldr   r2, =V2M_GIC_BASE_ADDRESS       /* r2 := VE gic base address */
+        b     kick_cpus_sgi
+kick_cpus_arndale:
+        /* write start paddr to CPU 1 sysreg register */
+        ldr   r0, =(S5P_PA_SYSRAM)
+        ldr   r2, =start
+        add   r2, r2, r10
+        str   r2, [r0]
+        dsb
+        ldr   r2, =EXYNOS5_GIC_BASE_ADDRESS   /* r2 := Exynos5 gic base address */
+kick_cpus_sgi:
         /* send an interrupt */
-        ldr   r0, =(GIC_BASE_ADDRESS + GIC_DR_OFFSET) /* base GICD MMIO address */
+        ldr   r0, =GIC_DR_OFFSET              /* GIC distributor offset */
+        add   r0, r2                          /* r0 := r0 + gic base address */
         mov   r2, #0x1
         str   r2, [r0, #(GICD_CTLR * 4)]      /* enable distributor */
         mov   r2, #0xfe0000
@@ -51,13 +69,15 @@ kick_cpus:
 
 /* Get up a CPU into Hyp mode.  Clobbers r0-r3.
  *
- * Expects r12 == CPU number
+ * r5: Machine ID
+ * r12: CPU number
  *
- * This code is specific to the VE model, and not intended to be used
+ * This code is specific to the VE model/Arndale, and not intended to be used
  * on production systems.  As such it's a bit hackier than the main
  * boot code in head.S.  In future it will be replaced by better
  * integration with the bootloader/firmware so that Xen always starts
- * in Hyp mode. */
+ * in Hyp mode.
+ * Clobber r0 - r4 */
 
 .globl enter_hyp_mode
 enter_hyp_mode:
@@ -68,33 +88,51 @@ enter_hyp_mode:
         orr   r0, r0, #0xb1          /* Set SCD, AW, FW and NS */
         bic   r0, r0, #0xe           /* Clear EA, FIQ and IRQ */
         mcr   CP32(r0, SCR)
+
+        ldr   r2, =MACH_TYPE_SMDK5250   /* r4 := Arndale machine ID */
+        /* By default load Arndale defaults values */
+        ldr   r0, =EXYNOS5_TIMER_FREQUENCY  /* r0 := timer's frequency */
+        ldr   r1, =EXYNOS5_GIC_BASE_ADDRESS /* r1 := GIC base address */
+        /* If it's not the Arndale machine ID, load VE values */
+        teq   r5, r2
+        ldrne r0, =V2M_TIMER_FREQUENCY
+        ldrne r1, =V2M_GIC_BASE_ADDRESS
+
         /* Ugly: the system timer's frequency register is only
          * programmable in Secure state.  Since we don't know where its
          * memory-mapped control registers live, we can't find out the
-         * right frequency.  Use the VE model's default frequency here. */
-        ldr   r0, =0x5f5e100         /* 100 MHz */
+         * right frequency. */
         mcr   CP32(r0, CNTFRQ)
         ldr   r0, =0x40c00           /* SMP, c11, c10 in non-secure mode */
         mcr   CP32(r0, NSACR)
-        mov   r0, #GIC_BASE_ADDRESS
-        add   r0, r0, #GIC_DR_OFFSET
+
+        add   r0, r1, #GIC_DR_OFFSET
         /* Disable the GIC distributor, on the boot CPU only */
-        mov   r1, #0
+        mov   r4, #0
         teq   r12, #0                /* Is this the boot CPU? */
-        streq r1, [r0]
+        streq r4, [r0]
         /* Continuing ugliness: Set up the GIC so NS state owns interrupts,
          * The first 32 interrupts (SGIs & PPIs) must be configured on all
          * CPUs while the remainder are SPIs and only need to be done one, on
          * the boot CPU. */
         add   r0, r0, #0x80          /* GICD_IGROUP0 */
         mov   r2, #0xffffffff        /* All interrupts to group 1 */
-        teq   r12, #0                /* Boot CPU? */
         str   r2, [r0]               /* Interrupts  0-31 (SGI & PPI) */
-        streq r2, [r0, #4]           /* Interrupts 32-63 (SPI) */
-        streq r2, [r0, #8]           /* Interrupts 64-95 (SPI) */
+        teq   r12, #0                /* Boot CPU? */
+        bne   skip_spis              /* Don't route SPIs on secondary CPUs */
+
+        add   r4, r1, #GIC_DR_OFFSET
+        ldr   r4, [r4, #4]            /* r4 := Interrupt Controller Type Reg */
+        and   r4, r4, #GICD_TYPE_LINES /* r4 := number of SPIs */
+        /* Assume we have minimum 32 SPIs */
+1:
+        add   r0, r0, #4             /* Go to the new group */
+        str   r2, [r0]               /* Update the group */
+        subs  r4, r4, #1
+        bne   1b
+skip_spis:
         /* Disable the GIC CPU interface on all processors */
-        mov   r0, #GIC_BASE_ADDRESS
-        add   r0, r0, #GIC_CR_OFFSET
+        add   r0, r1, #GIC_CR_OFFSET
         mov   r1, #0
         str   r1, [r0]
         /* Must drop priority mask below 0x80 before entering NS state */
diff --git a/xen/include/asm-arm/platforms/vexpress.h b/xen/include/asm-arm/platforms/vexpress.h
index 5cf3aba..982a293 100644
--- a/xen/include/asm-arm/platforms/vexpress.h
+++ b/xen/include/asm-arm/platforms/vexpress.h
@@ -32,6 +32,17 @@
 int vexpress_syscfg(int write, int function, int device, uint32_t *data);
 #endif
 
+/* Constants below is only used in assembly because the DTS is not yet parsed */
+#ifdef __ASSEMBLY__
+
+/* GIC base address */
+#define V2M_GIC_BASE_ADDRESS        0x2c000000
+
+/* Timer's frequency */
+#define V2M_TIMER_FREQUENCY         0x5f5e100 /* 100 Mhz */
+
+#endif /* __ASSEMBLY__ */
+
 #endif /* __ASM_ARM_PLATFORMS_VEXPRESS_H */
 /*
  * Local variables:
