1 files changed, 27 insertions, 9 deletions

diff --git a/hw/arm/boot.c b/hw/arm/boot.c
index bef451b..b0879a5 100644
--- a/hw/arm/boot.c
+++ b/hw/arm/boot.c
@@ -28,14 +28,15 @@
 #define KERNEL64_LOAD_ADDR 0x00080000
 
 typedef enum {
-    FIXUP_NONE = 0,   /* do nothing */
-    FIXUP_TERMINATOR, /* end of insns */
-    FIXUP_BOARDID,    /* overwrite with board ID number */
-    FIXUP_ARGPTR,     /* overwrite with pointer to kernel args */
-    FIXUP_ENTRYPOINT, /* overwrite with kernel entry point */
-    FIXUP_GIC_CPU_IF, /* overwrite with GIC CPU interface address */
-    FIXUP_BOOTREG,    /* overwrite with boot register address */
-    FIXUP_DSB,        /* overwrite with correct DSB insn for cpu */
+    FIXUP_NONE = 0,     /* do nothing */
+    FIXUP_TERMINATOR,   /* end of insns */
+    FIXUP_BOARDID,      /* overwrite with board ID number */
+    FIXUP_BOARD_SETUP,  /* overwrite with board specific setup code address */
+    FIXUP_ARGPTR,       /* overwrite with pointer to kernel args */
+    FIXUP_ENTRYPOINT,   /* overwrite with kernel entry point */
+    FIXUP_GIC_CPU_IF,   /* overwrite with GIC CPU interface address */
+    FIXUP_BOOTREG,      /* overwrite with boot register address */
+    FIXUP_DSB,          /* overwrite with correct DSB insn for cpu */
     FIXUP_MAX,
 } FixupType;
 
@@ -58,8 +59,17 @@ static const ARMInsnFixup bootloader_aarch64[] = {
     { 0, FIXUP_TERMINATOR }
 };
 
-/* The worlds second smallest bootloader.  Set r0-r2, then jump to kernel.  */
+/* A very small bootloader: call the board-setup code (if needed),
+ * set r0-r2, then jump to the kernel.
+ * If we're not calling boot setup code then we don't copy across
+ * the first BOOTLOADER_NO_BOARD_SETUP_OFFSET insns in this array.
+ */
+
 static const ARMInsnFixup bootloader[] = {
+    { 0xe28fe008 }, /* add     lr, pc, #8 */
+    { 0xe51ff004 }, /* ldr     pc, [pc, #-4] */
+    { 0, FIXUP_BOARD_SETUP },
+#define BOOTLOADER_NO_BOARD_SETUP_OFFSET 3
     { 0xe3a00000 }, /* mov     r0, #0 */
     { 0xe59f1004 }, /* ldr     r1, [pc, #4] */
     { 0xe59f2004 }, /* ldr     r2, [pc, #4] */
@@ -131,6 +141,7 @@ static void write_bootloader(const char *name, hwaddr addr,
         case FIXUP_NONE:
             break;
         case FIXUP_BOARDID:
+        case FIXUP_BOARD_SETUP:
         case FIXUP_ARGPTR:
         case FIXUP_ENTRYPOINT:
         case FIXUP_GIC_CPU_IF:
@@ -640,6 +651,9 @@ static void arm_load_kernel_notify(Notifier *notifier, void *data)
         elf_machine = EM_AARCH64;
     } else {
         primary_loader = bootloader;
+        if (!info->write_board_setup) {
+            primary_loader += BOOTLOADER_NO_BOARD_SETUP_OFFSET;
+        }
         kernel_load_offset = KERNEL_LOAD_ADDR;
         elf_machine = EM_ARM;
     }
@@ -745,6 +759,7 @@ static void arm_load_kernel_notify(Notifier *notifier, void *data)
         info->initrd_size = initrd_size;
 
         fixupcontext[FIXUP_BOARDID] = info->board_id;
+        fixupcontext[FIXUP_BOARD_SETUP] = info->board_setup_addr;
 
         /* for device tree boot, we pass the DTB directly in r2. Otherwise
          * we point to the kernel args.
@@ -793,6 +808,9 @@ static void arm_load_kernel_notify(Notifier *notifier, void *data)
         if (info->nb_cpus > 1) {
             info->write_secondary_boot(cpu, info);
         }
+        if (info->write_board_setup) {
+            info->write_board_setup(cpu, info);
+        }
 
         /* Notify devices which need to fake up firmware initialization
          * that we're doing a direct kernel boot.