ARM: Machine specific boot blobs

index bef451b..a0dd1df 100644
@@ -31,6 +31,7 @@ typedef enum {
     FIXUP_NONE = 0,   /* do nothing */
     FIXUP_TERMINATOR, /* end of insns */
     FIXUP_BOARDID,    /* overwrite with board ID number */
+    FIXUP_BOARDSETUP, /* 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 */

@@ -60,6 +61,11 @@ static const ARMInsnFixup bootloader_aarch64[] = {
 
 /* The worlds second smallest bootloader.  Set r0-r2, then jump to kernel.  */
 static const ARMInsnFixup bootloader[] = {
+    { 0xe28fe008 }, /* add     lr, pc, #8 */
+    { 0xe59ff000 }, /* ldr     pc, [pc, #0] */
+    { 0 },
+    { 0, FIXUP_BOARDSETUP },
+#define BOOTLOADER_NO_BOARDSETUP_OFFSET 4
     { 0xe3a00000 }, /* mov     r0, #0 */
     { 0xe59f1004 }, /* ldr     r1, [pc, #4] */
     { 0xe59f2004 }, /* ldr     r2, [pc, #4] */

@@ -131,6 +137,7 @@ static void write_bootloader(const char *name, hwaddr addr,
         case FIXUP_NONE:
             break;
         case FIXUP_BOARDID:
+        case FIXUP_BOARDSETUP:
         case FIXUP_ARGPTR:
         case FIXUP_ENTRYPOINT:
         case FIXUP_GIC_CPU_IF:

@@ -640,6 +647,9 @@ static void arm_load_kernel_notify(Notifier *notifier, void *data)
         elf_machine = EM_AARCH64;
     } else {
         primary_loader = bootloader;
+        if (!info->board_setup_blob) {
+            primary_loader += BOOTLOADER_NO_BOARDSETUP_OFFSET;
+        }
         kernel_load_offset = KERNEL_LOAD_ADDR;
         elf_machine = EM_ARM;
     }

@@ -745,6 +755,7 @@ static void arm_load_kernel_notify(Notifier *notifier, void *data)
         info->initrd_size = initrd_size;
 
         fixupcontext[FIXUP_BOARDID] = info->board_id;
+        fixupcontext[FIXUP_BOARDSETUP] = info->board_setup_blob_addr;
 
         /* for device tree boot, we pass the DTB directly in r2. Otherwise
          * we point to the kernel args.

index 4dcd4f9..0e3a652 100644
@@ -87,6 +87,13 @@ struct arm_boot_info {
      * -pflash. It also implies that fw_cfg_find() will succeed.
      */
     bool firmware_loaded;
+
+    /* Address as which board specific loader/setup code exists. If enabled,
+     * this code-blob will run before anything else. It must return to the
+     * caller via the link register. There is no stack setup.
+     */
+    bool board_setup_blob;
+    hwaddr board_setup_blob_addr;
 };
 
 /**

index be04b27..1fca057 100644
@@ -28,6 +28,9 @@
 #include "exec/address-spaces.h"
 #include "qemu/error-report.h"
 
+#define FW_BLOB_ADDR            0x0
+#define LOAD_ADDR               0x1000
+
 #define SMP_BOOT_ADDR           0x100
 #define SMP_BOOT_REG            0x40
 #define MPCORE_PERIPHBASE       0xfff10000

@@ -36,6 +39,36 @@
 
 /* Board init.  */
 
+static void hb_write_fw_blob(void)
+{
+    int n;
+    uint32_t fw_blob[] = {
+        /* Reset */
+        0xe320f000, /* nop */
+        0xe320f000, /* nop */
+        /* smc */
+        0xe10f0000, /* mrs r0, CPSR */
+        0xe200001f, /* and r0, r0, #0x1f - mask off mode bits */
+        0xe3500016, /* cmp r0, #0x16 - are we in monitor mode? */
+        /* if (!monitor_mode) { */
+            0x11600070, /* smcne - go to monitor mode */
+            0x112fff1e, /* bxne lr - return to caller */
+        /* } */
+        /* do setup from monitor mode */
+        0xe3a00000 + FW_BLOB_ADDR, /* mov r0, #FW_BLOB_ADDR */
+        0xee0c0f30, /* mcr p15, 0, r0, c12, c0, 1 - set mvbar */
+        0xe58fe008, /* save lr */
+        0xe8dfc000, /* exception return */
+        0,
+        0,
+        0, /* exception return link will end up here */
+    };
+    for (n = 0; n < ARRAY_SIZE(fw_blob); n++) {
+        fw_blob[n] = tswap32(fw_blob[n]);
+    }
+    rom_add_blob_fixed("firmware", fw_blob, sizeof(fw_blob), FW_BLOB_ADDR);
+}
+
 static void hb_write_secondary(ARMCPU *cpu, const struct arm_boot_info *info)
 {
     int n;

@@ -248,16 +281,13 @@ static void calxeda_init(MachineState *machine, enum cxmachines machine_id)
         cpuobj = object_new(object_class_get_name(oc));
         cpu = ARM_CPU(cpuobj);
 
-        /* By default A9 and A15 CPUs have EL3 enabled.  This board does not
-         * currently support EL3 so the CPU EL3 property is disabled before
-         * realization.
-         */
-        if (object_property_find(cpuobj, "has_el3", NULL)) {
-            object_property_set_bool(cpuobj, false, "has_el3", &err);
-            if (err) {
-                error_report_err(err);
-                exit(1);
-            }
+        object_property_set_int(cpuobj, QEMU_PSCI_CONDUIT_SMC,
+                                "psci-conduit", &error_abort);
+
+        if (n) {
+            /* Secondary CPUs start in PSCI powered-down state */
+            object_property_set_bool(cpuobj, true,
+                                     "start-powered-off", &error_abort);
         }
 
         if (object_property_find(cpuobj, "reset-cbar", NULL)) {

@@ -375,9 +405,13 @@ static void calxeda_init(MachineState *machine, enum cxmachines machine_id)
      */
     highbank_binfo.board_id = -1;
     highbank_binfo.nb_cpus = smp_cpus;
-    highbank_binfo.loader_start = 0;
+    highbank_binfo.loader_start = LOAD_ADDR;
     highbank_binfo.write_secondary_boot = hb_write_secondary;
     highbank_binfo.secondary_cpu_reset_hook = hb_reset_secondary;
+    highbank_binfo.board_setup_blob = true;
+    highbank_binfo.board_setup_blob_addr = FW_BLOB_ADDR;
+
+    hb_write_fw_blob();
     arm_load_kernel(ARM_CPU(first_cpu), &highbank_binfo);
 }
 

index 9f89483..5bf03de 100644
@@ -43,6 +43,41 @@ static const int dma_irqs[8] = {
     46, 47, 48, 49, 72, 73, 74, 75
 };
 
+#define LOAD_ADDR       0x1000
+#define FW_BLOB_ADDR    0x0
+
+#define SLCR_ARM_PLL_OFFSET     0x100
+#define SLCR_LOCK_OFFSET        0x004
+#define SLCR_UNLOCK_OFFSET      0x008
+
+#define SLCR_XILINX_UNLOCK_KEY  0xdf0d
+#define SLCR_XILINX_LOCK_KEY    0x767b
+
+#define SLCR_WRITE(addr, val) \
+        0xe3a01000 + extract32((val), 0, 8), \
+        0xe3811c00 + extract32((val), 8, 8), \
+        0xe3811800 + extract32((val), 16, 8), \
+        0xe3811400 + extract32((val), 24, 8), \
+        0xe5801000 + (addr),
+
+static void zynq_write_fw_blob(void)
+{
+    int n;
+    uint32_t fw_blob[] = {
+        0xe3a004f8, /* mov r0, #0xf8000000 */
+        SLCR_WRITE(SLCR_UNLOCK_OFFSET, SLCR_XILINX_UNLOCK_KEY)
+        SLCR_WRITE(SLCR_ARM_PLL_OFFSET, 0x00014008)
+        //SLCR_WRITE(0x120, 0x1f000200)
+        SLCR_WRITE(SLCR_LOCK_OFFSET, SLCR_XILINX_LOCK_KEY)
+        0xe12fff1e, /* bx lr */
+    };
+    for (n = 0; n < ARRAY_SIZE(fw_blob); n++) {
+        fw_blob[n] = tswap32(fw_blob[n]);
+    }
+    rom_add_blob_fixed("firmware", fw_blob, sizeof(fw_blob), FW_BLOB_ADDR);
+}
+
+
 static struct arm_boot_info zynq_binfo = {};
 
 static void gem_init(NICInfo *nd, uint32_t base, qemu_irq irq)

@@ -251,7 +286,11 @@ static void zynq_init(MachineState *machine)
     zynq_binfo.initrd_filename = initrd_filename;
     zynq_binfo.nb_cpus = 1;
     zynq_binfo.board_id = 0xd32;
-    zynq_binfo.loader_start = 0;
+    zynq_binfo.loader_start = LOAD_ADDR;
+    zynq_binfo.board_setup_blob = true;
+    zynq_binfo.board_setup_blob_addr = FW_BLOB_ADDR;
+
+    zynq_write_fw_blob();
     arm_load_kernel(ARM_CPU(first_cpu), &zynq_binfo);
 }