[PULL,16/17] arm/boot: move highbank secure board setup code to common routine

diff --git a/hw/arm/boot.c b/hw/arm/boot.c
index d05a998..cce8c7c 100644
--- a/hw/arm/boot.c
+++ b/hw/arm/boot.c
@@ -178,6 +178,57 @@  static void default_write_secondary(ARMCPU *cpu,
                      smpboot, fixupcontext);
 }
 
+void arm_write_secure_board_setup_dummy_smc(ARMCPU *cpu,
+                                            const struct arm_boot_info *info,
+                                            hwaddr mvbar_addr)
+{
+    int n;
+    uint32_t mvbar_blob[] = {
+        /* mvbar_addr: secure monitor vectors
+         * Default unimplemented and unused vectors to spin. Makes it
+         * easier to debug (as opposed to the CPU running away).
+         */
+        0xeafffffe, /* (spin) */
+        0xeafffffe, /* (spin) */
+        0xe1b0f00e, /* movs pc, lr ;SMC exception return */
+        0xeafffffe, /* (spin) */
+        0xeafffffe, /* (spin) */
+        0xeafffffe, /* (spin) */
+        0xeafffffe, /* (spin) */
+        0xeafffffe, /* (spin) */
+    };
+    uint32_t board_setup_blob[] = {
+        /* board setup addr */
+        0xe3a00e00 + (mvbar_addr >> 4), /* mov r0, #mvbar_addr */
+        0xee0c0f30, /* mcr     p15, 0, r0, c12, c0, 1 ;set MVBAR */
+        0xee110f11, /* mrc     p15, 0, r0, c1 , c1, 0 ;read SCR */
+        0xe3800031, /* orr     r0, #0x31              ;enable AW, FW, NS */
+        0xee010f11, /* mcr     p15, 0, r0, c1, c1, 0  ;write SCR */
+        0xe1a0100e, /* mov     r1, lr                 ;save LR across SMC */
+        0xe1600070, /* smc     #0                     ;call monitor to flush SCR */
+        0xe1a0f001, /* mov     pc, r1                 ;return */
+    };
+
+    /* check that mvbar_addr is correctly aligned and relocatable (using MOV) */
+    assert((mvbar_addr & 0x1f) == 0 && (mvbar_addr >> 4) < 0x100);
+
+    /* check that these blobs don't overlap */
+    assert((mvbar_addr + sizeof(mvbar_blob) <= info->board_setup_addr)
+          || (info->board_setup_addr + sizeof(board_setup_blob) <= mvbar_addr));
+
+    for (n = 0; n < ARRAY_SIZE(mvbar_blob); n++) {
+        mvbar_blob[n] = tswap32(mvbar_blob[n]);
+    }
+    rom_add_blob_fixed("board-setup-mvbar", mvbar_blob, sizeof(mvbar_blob),
+                       mvbar_addr);
+
+    for (n = 0; n < ARRAY_SIZE(board_setup_blob); n++) {
+        board_setup_blob[n] = tswap32(board_setup_blob[n]);
+    }
+    rom_add_blob_fixed("board-setup", board_setup_blob,
+                       sizeof(board_setup_blob), info->board_setup_addr);
+}
+
 static void default_reset_secondary(ARMCPU *cpu,
                                     const struct arm_boot_info *info)
 {
diff --git a/hw/arm/highbank.c b/hw/arm/highbank.c
index 620b526..e25cf5e 100644
--- a/hw/arm/highbank.c
+++ b/hw/arm/highbank.c
@@ -35,49 +35,16 @@ 
 #define MPCORE_PERIPHBASE       0xfff10000
 
 #define MVBAR_ADDR              0x200
+#define BOARD_SETUP_ADDR        (MVBAR_ADDR + 8 * sizeof(uint32_t))
 
 #define NIRQ_GIC                160
 
 /* Board init.  */
 
-/* MVBAR_ADDR is limited by precision of movw */
-
-QEMU_BUILD_BUG_ON(MVBAR_ADDR >= (1 << 16));
-
-#define ARMV7_IMM16(x) (extract32((x),  0, 12) | \
-                        extract32((x), 12,  4) << 16)
-
 static void hb_write_board_setup(ARMCPU *cpu,
                                  const struct arm_boot_info *info)
 {
-    int n;
-    uint32_t board_setup_blob[] = {
-        /* MVBAR_ADDR */
-        /* Default unimplemented and unused vectors to spin. Makes it
-         * easier to debug (as opposed to the CPU running away).
-         */
-        0xeafffffe, /* notused1: b notused */
-        0xeafffffe, /* notused2: b notused */
-        0xe1b0f00e, /* smc: movs pc, lr - exception return */
-        0xeafffffe, /* prefetch_abort: b prefetch_abort */
-        0xeafffffe, /* data_abort: b data_abort */
-        0xeafffffe, /* notused3: b notused3 */
-        0xeafffffe, /* irq: b irq */
-        0xeafffffe, /* fiq: b fiq */
-#define BOARD_SETUP_ADDR (MVBAR_ADDR + 8 * sizeof(uint32_t))
-        0xe3000000 + ARMV7_IMM16(MVBAR_ADDR), /* movw r0, MVBAR_ADDR */
-        0xee0c0f30, /* mcr p15, 0, r0, c12, c0, 1 - set MVBAR */
-        0xee110f11, /* mrc p15, 0, r0, c1 , c1, 0 - get SCR */
-        0xe3810001, /* orr r0, #1 - set NS */
-        0xee010f11, /* mcr p15, 0, r0, c1 , c1, 0 - set SCR */
-        0xe1600070, /* smc - go to monitor mode to flush NS change */
-        0xe12fff1e, /* bx lr - return to caller */
-    };
-    for (n = 0; n < ARRAY_SIZE(board_setup_blob); n++) {
-        board_setup_blob[n] = tswap32(board_setup_blob[n]);
-    }
-    rom_add_blob_fixed("board-setup", board_setup_blob,
-                       sizeof(board_setup_blob), MVBAR_ADDR);
+    arm_write_secure_board_setup_dummy_smc(cpu, info, MVBAR_ADDR);
 }
 
 static void hb_write_secondary(ARMCPU *cpu, const struct arm_boot_info *info)
diff --git a/include/hw/arm/arm.h b/include/hw/arm/arm.h
index c26b0e3..52ecf4a 100644
--- a/include/hw/arm/arm.h
+++ b/include/hw/arm/arm.h
@@ -122,6 +122,11 @@  struct arm_boot_info {
  */
 void arm_load_kernel(ARMCPU *cpu, struct arm_boot_info *info);
 
+/* Write a secure board setup routine with a dummy handler for SMCs */
+void arm_write_secure_board_setup_dummy_smc(ARMCPU *cpu,
+                                            const struct arm_boot_info *info,
+                                            hwaddr mvbar_addr);
+
 /* Multiplication factor to convert from system clock ticks to qemu timer
    ticks.  */
 extern int system_clock_scale;