Merge remote-tracking branch 'remotes/pmaydell/tags/pull-target-arm-20220208' into staging

target-arm queue:
 * Fix handling of SVE ZCR_LEN when using VHE
 * xlnx-zynqmp: 'Or' the QSPI / QSPI DMA IRQs
 * Don't ever enable PSCI when booting guest in EL3
 * Adhere to SMCCC 1.3 section 5.2
 * highbank: Fix issues with booting SMP
 * midway: Fix issues booting at all
 * boot: Drop existing dtb /psci node rather than retaining it
 * versal-virt: Always call arm_load_kernel()
 * force flag recalculation when messing with DAIF
 * hw/timer/armv7m_systick: Update clock source before enabling timer
 * hw/arm/smmuv3: Fix device reset
 * hw/intc/arm_gicv3_its: refactorings and minor bug fixes
 * hw/sensor: Add lsm303dlhc magnetometer device

# gpg: Signature made Tue 08 Feb 2022 11:39:15 GMT
# gpg:                using RSA key E1A5C593CD419DE28E8315CF3C2525ED14360CDE
# gpg:                issuer "peter.maydell@linaro.org"
# gpg: Good signature from "Peter Maydell <peter.maydell@linaro.org>" [ultimate]
# gpg:                 aka "Peter Maydell <pmaydell@gmail.com>" [ultimate]
# gpg:                 aka "Peter Maydell <pmaydell@chiark.greenend.org.uk>" [ultimate]
# Primary key fingerprint: E1A5 C593 CD41 9DE2 8E83  15CF 3C25 25ED 1436 0CDE

* remotes/pmaydell/tags/pull-target-arm-20220208: (39 commits)
  hw/sensor: Add lsm303dlhc magnetometer device
  hw/intc/arm_gicv3_its: Split error checks
  hw/intc/arm_gicv3_its: Don't allow intid 1023 in MAPI/MAPTI
  hw/intc/arm_gicv3_its: In MAPC with V=0, don't check rdbase field
  hw/intc/arm_gicv3_its: Drop TableDesc and CmdQDesc valid fields
  hw/intc/arm_gicv3_its: Make update_ite() use ITEntry
  hw/intc/arm_gicv3_its: Pass ITE values back from get_ite() via a struct
  hw/intc/arm_gicv3_its: Avoid nested ifs in get_ite()
  hw/intc/arm_gicv3_its: Fix address calculation in get_ite() and update_ite()
  hw/intc/arm_gicv3_its: Pass CTEntry to update_cte()
  hw/intc/arm_gicv3_its: Keep CTEs as a struct, not a raw uint64_t
  hw/intc/arm_gicv3_its: Pass DTEntry to update_dte()
  hw/intc/arm_gicv3_its: Keep DTEs as a struct, not a raw uint64_t
  hw/intc/arm_gicv3_its: Use address_space_map() to access command queue packets
  hw/arm/smmuv3: Fix device reset
  hw/timer/armv7m_systick: Update clock source before enabling timer
  arm: force flag recalculation when messing with DAIF
  hw/arm: versal-virt: Always call arm_load_kernel()
  hw/arm/boot: Drop existing dtb /psci node rather than retaining it
  hw/arm/boot: Drop nb_cpus field from arm_boot_info
  ...

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>

42 files changed, 1302 insertions, 614 deletions

diff --git a/cpu.c b/cpu.c
index 016bf06a1a..3ea38aea70 100644
--- a/cpu.c
+++ b/cpu.c
@@ -196,13 +196,33 @@ static Property cpu_common_props[] = {
     DEFINE_PROP_LINK("memory", CPUState, memory, TYPE_MEMORY_REGION,
                      MemoryRegion *),
 #endif
-    DEFINE_PROP_BOOL("start-powered-off", CPUState, start_powered_off, false),
     DEFINE_PROP_END_OF_LIST(),
 };
 
+static bool cpu_get_start_powered_off(Object *obj, Error **errp)
+{
+    CPUState *cpu = CPU(obj);
+    return cpu->start_powered_off;
+}
+
+static void cpu_set_start_powered_off(Object *obj, bool value, Error **errp)
+{
+    CPUState *cpu = CPU(obj);
+    cpu->start_powered_off = value;
+}
+
 void cpu_class_init_props(DeviceClass *dc)
 {
+    ObjectClass *oc = OBJECT_CLASS(dc);
+
     device_class_set_props(dc, cpu_common_props);
+    /*
+     * We can't use DEFINE_PROP_BOOL in the Property array for this
+     * property, because we want this to be settable after realize.
+     */
+    object_class_property_add_bool(oc, "start-powered-off",
+                                   cpu_get_start_powered_off,
+                                   cpu_set_start_powered_off);
 }
 
 void cpu_exec_initfn(CPUState *cpu)
diff --git a/hw/arm/allwinner-h3.c b/hw/arm/allwinner-h3.c
index f9b7ed1871..318ed4348c 100644
--- a/hw/arm/allwinner-h3.c
+++ b/hw/arm/allwinner-h3.c
@@ -235,11 +235,10 @@ static void allwinner_h3_realize(DeviceState *dev, Error **errp)
     /* CPUs */
     for (i = 0; i < AW_H3_NUM_CPUS; i++) {
 
-        /* Provide Power State Coordination Interface */
-        qdev_prop_set_int32(DEVICE(&s->cpus[i]), "psci-conduit",
-                            QEMU_PSCI_CONDUIT_SMC);
-
-        /* Disable secondary CPUs */
+        /*
+         * Disable secondary CPUs. Guest EL3 firmware will start
+         * them via CPU reset control registers.
+         */
         qdev_prop_set_bit(DEVICE(&s->cpus[i]), "start-powered-off",
                           i > 0);
 
diff --git a/hw/arm/aspeed.c b/hw/arm/aspeed.c
index cf20ae0db5..d911dc904f 100644
--- a/hw/arm/aspeed.c
+++ b/hw/arm/aspeed.c
@@ -431,7 +431,6 @@ static void aspeed_machine_init(MachineState *machine)
 
     aspeed_board_binfo.ram_size = machine->ram_size;
     aspeed_board_binfo.loader_start = sc->memmap[ASPEED_DEV_SDRAM];
-    aspeed_board_binfo.nb_cpus = sc->num_cpus;
 
     if (amc->i2c_init) {
         amc->i2c_init(bmc);
diff --git a/hw/arm/boot.c b/hw/arm/boot.c
index 399f8e837c..b1e95978f2 100644
--- a/hw/arm/boot.c
+++ b/hw/arm/boot.c
@@ -478,12 +478,13 @@ static void fdt_add_psci_node(void *fdt)
     }
 
     /*
-     * If /psci node is present in provided DTB, assume that no fixup
-     * is necessary and all PSCI configuration should be taken as-is
+     * A pre-existing /psci node might specify function ID values
+     * that don't match QEMU's PSCI implementation. Delete the whole
+     * node and put our own in instead.
      */
     rc = fdt_path_offset(fdt, "/psci");
     if (rc >= 0) {
-        return;
+        qemu_fdt_nop_node(fdt, "/psci");
     }
 
     qemu_fdt_add_subnode(fdt, "/psci");
@@ -804,7 +805,7 @@ static void do_cpu_reset(void *opaque)
                         set_kernel_args(info, as);
                     }
                 }
-            } else {
+            } else if (info->secondary_cpu_reset_hook) {
                 info->secondary_cpu_reset_hook(cpu, info);
             }
         }
@@ -1030,16 +1031,6 @@ static void arm_setup_direct_kernel_boot(ARMCPU *cpu,
         elf_machine = EM_ARM;
     }
 
-    if (!info->secondary_cpu_reset_hook) {
-        info->secondary_cpu_reset_hook = default_reset_secondary;
-    }
-    if (!info->write_secondary_boot) {
-        info->write_secondary_boot = default_write_secondary;
-    }
-
-    if (info->nb_cpus == 0)
-        info->nb_cpus = 1;
-
     /* Assume that raw images are linux kernels, and ELF images are not.  */
     kernel_size = arm_load_elf(info, &elf_entry, &image_low_addr,
                                &image_high_addr, elf_machine, as);
@@ -1216,9 +1207,6 @@ static void arm_setup_direct_kernel_boot(ARMCPU *cpu,
         write_bootloader("bootloader", info->loader_start,
                          primary_loader, fixupcontext, as);
 
-        if (info->nb_cpus > 1) {
-            info->write_secondary_boot(cpu, info);
-        }
         if (info->write_board_setup) {
             info->write_board_setup(cpu, info);
         }
@@ -1299,6 +1287,9 @@ void arm_load_kernel(ARMCPU *cpu, MachineState *ms, struct arm_boot_info *info)
 {
     CPUState *cs;
     AddressSpace *as = arm_boot_address_space(cpu, info);
+    int boot_el;
+    CPUARMState *env = &cpu->env;
+    int nb_cpus = 0;
 
     /*
      * CPU objects (unlike devices) are not automatically reset on system
@@ -1308,6 +1299,7 @@ void arm_load_kernel(ARMCPU *cpu, MachineState *ms, struct arm_boot_info *info)
      */
     for (cs = first_cpu; cs; cs = CPU_NEXT(cs)) {
         qemu_register_reset(do_cpu_reset, ARM_CPU(cs));
+        nb_cpus++;
     }
 
     /*
@@ -1329,6 +1321,87 @@ void arm_load_kernel(ARMCPU *cpu, MachineState *ms, struct arm_boot_info *info)
         arm_setup_direct_kernel_boot(cpu, info);
     }
 
+    /*
+     * Disable the PSCI conduit if it is set up to target the same
+     * or a lower EL than the one we're going to start the guest code in.
+     * This logic needs to agree with the code in do_cpu_reset() which
+     * decides whether we're going to boot the guest in the highest
+     * supported exception level or in a lower one.
+     */
+
+    /*
+     * If PSCI is enabled, then SMC calls all go to the PSCI handler and
+     * are never emulated to trap into guest code. It therefore does not
+     * make sense for the board to have a setup code fragment that runs
+     * in Secure, because this will probably need to itself issue an SMC of some
+     * kind as part of its operation.
+     */
+    assert(info->psci_conduit == QEMU_PSCI_CONDUIT_DISABLED ||
+           !info->secure_board_setup);
+
+    /* Boot into highest supported EL ... */
+    if (arm_feature(env, ARM_FEATURE_EL3)) {
+        boot_el = 3;
+    } else if (arm_feature(env, ARM_FEATURE_EL2)) {
+        boot_el = 2;
+    } else {
+        boot_el = 1;
+    }
+    /* ...except that if we're booting Linux we adjust the EL we boot into */
+    if (info->is_linux && !info->secure_boot) {
+        boot_el = arm_feature(env, ARM_FEATURE_EL2) ? 2 : 1;
+    }
+
+    if ((info->psci_conduit == QEMU_PSCI_CONDUIT_HVC && boot_el >= 2) ||
+        (info->psci_conduit == QEMU_PSCI_CONDUIT_SMC && boot_el == 3)) {
+        info->psci_conduit = QEMU_PSCI_CONDUIT_DISABLED;
+    }
+
+    if (info->psci_conduit != QEMU_PSCI_CONDUIT_DISABLED) {
+        for (cs = first_cpu; cs; cs = CPU_NEXT(cs)) {
+            Object *cpuobj = OBJECT(cs);
+
+            object_property_set_int(cpuobj, "psci-conduit", info->psci_conduit,
+                                    &error_abort);
+            /*
+             * Secondary CPUs start in PSCI powered-down state. Like the
+             * code in do_cpu_reset(), we assume first_cpu is the primary
+             * CPU.
+             */
+            if (cs != first_cpu) {
+                object_property_set_bool(cpuobj, "start-powered-off", true,
+                                         &error_abort);
+            }
+        }
+    }
+
+    if (info->psci_conduit == QEMU_PSCI_CONDUIT_DISABLED &&
+        info->is_linux && nb_cpus > 1) {
+        /*
+         * We're booting Linux but not using PSCI, so for SMP we need
+         * to write a custom secondary CPU boot loader stub, and arrange
+         * for the secondary CPU reset to make the accompanying initialization.
+         */
+        if (!info->secondary_cpu_reset_hook) {
+            info->secondary_cpu_reset_hook = default_reset_secondary;
+        }
+        if (!info->write_secondary_boot) {
+            info->write_secondary_boot = default_write_secondary;
+        }
+        info->write_secondary_boot(cpu, info);
+    } else {
+        /*
+         * No secondary boot stub; don't use the reset hook that would
+         * have set the CPU up to call it
+         */
+        info->write_secondary_boot = NULL;
+        info->secondary_cpu_reset_hook = NULL;
+    }
+
+    /*
+     * arm_load_dtb() may add a PSCI node so it must be called after we have
+     * decided whether to enable PSCI and set the psci-conduit CPU properties.
+     */
     if (!info->skip_dtb_autoload && have_dtb(info)) {
         if (arm_load_dtb(info->dtb_start, info, info->dtb_limit, as, ms) < 0) {
             exit(1);
diff --git a/hw/arm/exynos4_boards.c b/hw/arm/exynos4_boards.c
index 35dd9875da..ef5bcbc212 100644
--- a/hw/arm/exynos4_boards.c
+++ b/hw/arm/exynos4_boards.c
@@ -67,7 +67,6 @@ static unsigned long exynos4_board_ram_size[EXYNOS4_NUM_OF_BOARDS] = {
 static struct arm_boot_info exynos4_board_binfo = {
     .loader_start     = EXYNOS4210_BASE_BOOT_ADDR,
     .smp_loader_start = EXYNOS4210_SMP_BOOT_ADDR,
-    .nb_cpus          = EXYNOS4210_NCPUS,
     .write_secondary_boot = exynos4210_write_secondary,
 };
 
diff --git a/hw/arm/fsl-imx6ul.c b/hw/arm/fsl-imx6ul.c
index 1d1a708dd9..f189712329 100644
--- a/hw/arm/fsl-imx6ul.c
+++ b/hw/arm/fsl-imx6ul.c
@@ -166,8 +166,6 @@ static void fsl_imx6ul_realize(DeviceState *dev, Error **errp)
         return;
     }
 
-    object_property_set_int(OBJECT(&s->cpu), "psci-conduit",
-                            QEMU_PSCI_CONDUIT_SMC, &error_abort);
     qdev_realize(DEVICE(&s->cpu), NULL, &error_abort);
 
     /*
diff --git a/hw/arm/fsl-imx7.c b/hw/arm/fsl-imx7.c
index 149885f2b8..cc6fdb9373 100644
--- a/hw/arm/fsl-imx7.c
+++ b/hw/arm/fsl-imx7.c
@@ -159,9 +159,6 @@ static void fsl_imx7_realize(DeviceState *dev, Error **errp)
     for (i = 0; i < smp_cpus; i++) {
         o = OBJECT(&s->cpu[i]);
 
-        object_property_set_int(o, "psci-conduit", QEMU_PSCI_CONDUIT_SMC,
-                                &error_abort);
-
         /* On uniprocessor, the CBAR is set to 0 */
         if (smp_cpus > 1) {
             object_property_set_int(o, "reset-cbar", FSL_IMX7_A7MPCORE_ADDR,
@@ -169,7 +166,10 @@ static void fsl_imx7_realize(DeviceState *dev, Error **errp)
         }
 
         if (i) {
-            /* Secondary CPUs start in PSCI powered-down state */
+            /*
+             * Secondary CPUs start in powered-down state (and can be
+             * powered up via the SRC system reset controller)
+             */
             object_property_set_bool(o, "start-powered-off", true,
                                      &error_abort);
         }
diff --git a/hw/arm/highbank.c b/hw/arm/highbank.c
index 4210894d81..f12aacea6b 100644
--- a/hw/arm/highbank.c
+++ b/hw/arm/highbank.c
@@ -48,66 +48,6 @@
 
 /* Board init.  */
 
-static void hb_write_board_setup(ARMCPU *cpu,
-                                 const struct arm_boot_info *info)
-{
-    arm_write_secure_board_setup_dummy_smc(cpu, info, MVBAR_ADDR);
-}
-
-static void hb_write_secondary(ARMCPU *cpu, const struct arm_boot_info *info)
-{
-    int n;
-    uint32_t smpboot[] = {
-        0xee100fb0, /* mrc p15, 0, r0, c0, c0, 5 - read current core id */
-        0xe210000f, /* ands r0, r0, #0x0f */
-        0xe3a03040, /* mov r3, #0x40 - jump address is 0x40 + 0x10 * core id */
-        0xe0830200, /* add r0, r3, r0, lsl #4 */
-        0xe59f2024, /* ldr r2, privbase */
-        0xe3a01001, /* mov r1, #1 */
-        0xe5821100, /* str r1, [r2, #256] - set GICC_CTLR.Enable */
-        0xe3a010ff, /* mov r1, #0xff */
-        0xe5821104, /* str r1, [r2, #260] - set GICC_PMR.Priority to 0xff */
-        0xf57ff04f, /* dsb */
-        0xe320f003, /* wfi */
-        0xe5901000, /* ldr     r1, [r0] */
-        0xe1110001, /* tst     r1, r1 */
-        0x0afffffb, /* beq     <wfi> */
-        0xe12fff11, /* bx      r1 */
-        MPCORE_PERIPHBASE   /* privbase: MPCore peripheral base address.  */
-    };
-    for (n = 0; n < ARRAY_SIZE(smpboot); n++) {
-        smpboot[n] = tswap32(smpboot[n]);
-    }
-    rom_add_blob_fixed_as("smpboot", smpboot, sizeof(smpboot), SMP_BOOT_ADDR,
-                          arm_boot_address_space(cpu, info));
-}
-
-static void hb_reset_secondary(ARMCPU *cpu, const struct arm_boot_info *info)
-{
-    CPUARMState *env = &cpu->env;
-
-    switch (info->nb_cpus) {
-    case 4:
-        address_space_stl_notdirty(&address_space_memory,
-                                   SMP_BOOT_REG + 0x30, 0,
-                                   MEMTXATTRS_UNSPECIFIED, NULL);
-        /* fallthrough */
-    case 3:
-        address_space_stl_notdirty(&address_space_memory,
-                                   SMP_BOOT_REG + 0x20, 0,
-                                   MEMTXATTRS_UNSPECIFIED, NULL);
-        /* fallthrough */
-    case 2:
-        address_space_stl_notdirty(&address_space_memory,
-                                   SMP_BOOT_REG + 0x10, 0,
-                                   MEMTXATTRS_UNSPECIFIED, NULL);
-        env->regs[15] = SMP_BOOT_ADDR;
-        break;
-    default:
-        break;
-    }
-}
-
 #define NUM_REGS      0x200
 static void hb_regs_write(void *opaque, hwaddr offset,
                           uint64_t value, unsigned size)
@@ -271,12 +211,6 @@ static void calxeda_init(MachineState *machine, enum cxmachines machine_id)
         object_property_set_int(cpuobj, "psci-conduit", QEMU_PSCI_CONDUIT_SMC,
                                 &error_abort);
 
-        if (n) {
-            /* Secondary CPUs start in PSCI powered-down state */
-            object_property_set_bool(cpuobj, "start-powered-off", true,
-                                     &error_abort);
-        }
-
         if (object_property_find(cpuobj, "reset-cbar")) {
             object_property_set_int(cpuobj, "reset-cbar", MPCORE_PERIPHBASE,
                                     &error_abort);
@@ -390,13 +324,9 @@ static void calxeda_init(MachineState *machine, enum cxmachines machine_id)
      * clear that the value is meaningless.
      */
     highbank_binfo.board_id = -1;
-    highbank_binfo.nb_cpus = smp_cpus;
     highbank_binfo.loader_start = 0;
-    highbank_binfo.write_secondary_boot = hb_write_secondary;
-    highbank_binfo.secondary_cpu_reset_hook = hb_reset_secondary;
     highbank_binfo.board_setup_addr = BOARD_SETUP_ADDR;
-    highbank_binfo.write_board_setup = hb_write_board_setup;
-    highbank_binfo.secure_board_setup = true;
+    highbank_binfo.psci_conduit = QEMU_PSCI_CONDUIT_SMC;
 
     arm_load_kernel(ARM_CPU(first_cpu), machine, &highbank_binfo);
 }
diff --git a/hw/arm/imx25_pdk.c b/hw/arm/imx25_pdk.c
index 6dff000163..b4f7f4e8a7 100644
--- a/hw/arm/imx25_pdk.c
+++ b/hw/arm/imx25_pdk.c
@@ -114,8 +114,7 @@ static void imx25_pdk_init(MachineState *machine)
 
     imx25_pdk_binfo.ram_size = machine->ram_size;
     imx25_pdk_binfo.loader_start = FSL_IMX25_SDRAM0_ADDR;
-    imx25_pdk_binfo.board_id = 1771,
-    imx25_pdk_binfo.nb_cpus = 1;
+    imx25_pdk_binfo.board_id = 1771;
 
     for (i = 0; i < FSL_IMX25_NUM_ESDHCS; i++) {
         BusState *bus;
diff --git a/hw/arm/kzm.c b/hw/arm/kzm.c
index 39559c44c2..b1b281c9ac 100644
--- a/hw/arm/kzm.c
+++ b/hw/arm/kzm.c
@@ -124,7 +124,6 @@ static void kzm_init(MachineState *machine)
     }
 
     kzm_binfo.ram_size = machine->ram_size;
-    kzm_binfo.nb_cpus = 1;
 
     if (!qtest_enabled()) {
         arm_load_kernel(&s->soc.cpu, machine, &kzm_binfo);
diff --git a/hw/arm/mcimx6ul-evk.c b/hw/arm/mcimx6ul-evk.c
index 28b4886f48..d83c3c380e 100644
--- a/hw/arm/mcimx6ul-evk.c
+++ b/hw/arm/mcimx6ul-evk.c
@@ -34,7 +34,7 @@ static void mcimx6ul_evk_init(MachineState *machine)
         .loader_start = FSL_IMX6UL_MMDC_ADDR,
         .board_id = -1,
         .ram_size = machine->ram_size,
-        .nb_cpus = machine->smp.cpus,
+        .psci_conduit = QEMU_PSCI_CONDUIT_SMC,
     };
 
     s = FSL_IMX6UL(object_new(TYPE_FSL_IMX6UL));
diff --git a/hw/arm/mcimx7d-sabre.c b/hw/arm/mcimx7d-sabre.c
index 50a5ecde31..6182b15f19 100644
--- a/hw/arm/mcimx7d-sabre.c
+++ b/hw/arm/mcimx7d-sabre.c
@@ -36,7 +36,7 @@ static void mcimx7d_sabre_init(MachineState *machine)
         .loader_start = FSL_IMX7_MMDC_ADDR,
         .board_id = -1,
         .ram_size = machine->ram_size,
-        .nb_cpus = machine->smp.cpus,
+        .psci_conduit = QEMU_PSCI_CONDUIT_SMC,
     };
 
     s = FSL_IMX7(object_new(TYPE_FSL_IMX7));
diff --git a/hw/arm/npcm7xx.c b/hw/arm/npcm7xx.c
index 878c2208e0..d85cc02765 100644
--- a/hw/arm/npcm7xx.c
+++ b/hw/arm/npcm7xx.c
@@ -355,10 +355,7 @@ static struct arm_boot_info npcm7xx_binfo = {
 
 void npcm7xx_load_kernel(MachineState *machine, NPCM7xxState *soc)
 {
-    NPCM7xxClass *sc = NPCM7XX_GET_CLASS(soc);
-
     npcm7xx_binfo.ram_size = machine->ram_size;
-    npcm7xx_binfo.nb_cpus = sc->num_cpus;
 
     arm_load_kernel(&soc->cpu[0], machine, &npcm7xx_binfo);
 }
diff --git a/hw/arm/orangepi.c b/hw/arm/orangepi.c
index e796382236..3ace474870 100644
--- a/hw/arm/orangepi.c
+++ b/hw/arm/orangepi.c
@@ -25,9 +25,7 @@
 #include "hw/qdev-properties.h"
 #include "hw/arm/allwinner-h3.h"
 
-static struct arm_boot_info orangepi_binfo = {
-    .nb_cpus = AW_H3_NUM_CPUS,
-};
+static struct arm_boot_info orangepi_binfo;
 
 static void orangepi_init(MachineState *machine)
 {
@@ -105,6 +103,7 @@ static void orangepi_init(MachineState *machine)
     }
     orangepi_binfo.loader_start = h3->memmap[AW_H3_DEV_SDRAM];
     orangepi_binfo.ram_size = machine->ram_size;
+    orangepi_binfo.psci_conduit = QEMU_PSCI_CONDUIT_SMC;
     arm_load_kernel(ARM_CPU(first_cpu), machine, &orangepi_binfo);
 }
 
diff --git a/hw/arm/raspi.c b/hw/arm/raspi.c
index b4dd6c1e99..92d068d1f9 100644
--- a/hw/arm/raspi.c
+++ b/hw/arm/raspi.c
@@ -204,7 +204,6 @@ static void setup_boot(MachineState *machine, RaspiProcessorId processor_id,
 
     s->binfo.board_id = MACH_TYPE_BCM2708;
     s->binfo.ram_size = ram_size;
-    s->binfo.nb_cpus = machine->smp.cpus;
 
     if (processor_id <= PROCESSOR_ID_BCM2836) {
         /*
diff --git a/hw/arm/realview.c b/hw/arm/realview.c
index ddc70b54a5..7b424e94a5 100644
--- a/hw/arm/realview.c
+++ b/hw/arm/realview.c
@@ -363,7 +363,6 @@ static void realview_init(MachineState *machine,
     memory_region_add_subregion(sysmem, SMP_BOOT_ADDR, ram_hack);
 
     realview_binfo.ram_size = ram_size;
-    realview_binfo.nb_cpus = smp_cpus;
     realview_binfo.board_id = realview_board_id[board_type];
     realview_binfo.loader_start = (board_type == BOARD_PB_A8 ? 0x70000000 : 0);
     arm_load_kernel(ARM_CPU(first_cpu), machine, &realview_binfo);
diff --git a/hw/arm/sabrelite.c b/hw/arm/sabrelite.c
index cce49aa25c..41191245b8 100644
--- a/hw/arm/sabrelite.c
+++ b/hw/arm/sabrelite.c
@@ -93,7 +93,6 @@ static void sabrelite_init(MachineState *machine)
     }
 
     sabrelite_binfo.ram_size = machine->ram_size;
-    sabrelite_binfo.nb_cpus = machine->smp.cpus;
     sabrelite_binfo.secure_boot = true;
     sabrelite_binfo.write_secondary_boot = sabrelite_write_secondary;
     sabrelite_binfo.secondary_cpu_reset_hook = sabrelite_reset_secondary;
diff --git a/hw/arm/sbsa-ref.c b/hw/arm/sbsa-ref.c
index dd944553f7..2387401963 100644
--- a/hw/arm/sbsa-ref.c
+++ b/hw/arm/sbsa-ref.c
@@ -776,7 +776,6 @@ static void sbsa_ref_init(MachineState *machine)
     create_secure_ec(secure_sysmem);
 
     sms->bootinfo.ram_size = machine->ram_size;
-    sms->bootinfo.nb_cpus = smp_cpus;
     sms->bootinfo.board_id = -1;
     sms->bootinfo.loader_start = sbsa_ref_memmap[SBSA_MEM].base;
     sms->bootinfo.get_dtb = sbsa_ref_dtb;
diff --git a/hw/arm/smmuv3.c b/hw/arm/smmuv3.c
index 3b43368be0..674623aabe 100644
--- a/hw/arm/smmuv3.c
+++ b/hw/arm/smmuv3.c
@@ -278,6 +278,12 @@ static void smmuv3_init_regs(SMMUv3State *s)
     s->features = 0;
     s->sid_split = 0;
     s->aidr = 0x1;
+    s->cr[0] = 0;
+    s->cr0ack = 0;
+    s->irq_ctrl = 0;
+    s->gerror = 0;
+    s->gerrorn = 0;
+    s->statusr = 0;
 }
 
 static int smmu_get_ste(SMMUv3State *s, dma_addr_t addr, STE *buf,
diff --git a/hw/arm/vexpress.c b/hw/arm/vexpress.c
index 3e99b7918a..e1d1983ae6 100644
--- a/hw/arm/vexpress.c
+++ b/hw/arm/vexpress.c
@@ -708,7 +708,6 @@ static void vexpress_common_init(MachineState *machine)
     }
 
     daughterboard->bootinfo.ram_size = machine->ram_size;
-    daughterboard->bootinfo.nb_cpus = machine->smp.cpus;
     daughterboard->bootinfo.board_id = VEXPRESS_BOARD_ID;
     daughterboard->bootinfo.loader_start = daughterboard->loader_start;
     daughterboard->bootinfo.smp_loader_start = map[VE_SRAM];
diff --git a/hw/arm/virt.c b/hw/arm/virt.c
index 141350bf21..46bf7ceddf 100644
--- a/hw/arm/virt.c
+++ b/hw/arm/virt.c
@@ -2088,17 +2088,6 @@ static void machvirt_init(MachineState *machine)
             object_property_set_bool(cpuobj, "has_el2", false, NULL);
         }
 
-        if (vms->psci_conduit != QEMU_PSCI_CONDUIT_DISABLED) {
-            object_property_set_int(cpuobj, "psci-conduit", vms->psci_conduit,
-                                    NULL);
-
-            /* Secondary CPUs start in PSCI powered-down state */
-            if (n > 0) {
-                object_property_set_bool(cpuobj, "start-powered-off", true,
-                                         NULL);
-            }
-        }
-
         if (vmc->kvm_no_adjvtime &&
             object_property_find(cpuobj, "kvm-no-adjvtime")) {
             object_property_set_bool(cpuobj, "kvm-no-adjvtime", true, NULL);
@@ -2240,12 +2229,12 @@ static void machvirt_init(MachineState *machine)
     }
 
     vms->bootinfo.ram_size = machine->ram_size;
-    vms->bootinfo.nb_cpus = smp_cpus;
     vms->bootinfo.board_id = -1;
     vms->bootinfo.loader_start = vms->memmap[VIRT_MEM].base;
     vms->bootinfo.get_dtb = machvirt_dtb;
     vms->bootinfo.skip_dtb_autoload = true;
     vms->bootinfo.firmware_loaded = firmware_loaded;
+    vms->bootinfo.psci_conduit = vms->psci_conduit;
     arm_load_kernel(ARM_CPU(first_cpu), machine, &vms->bootinfo);
 
     vms->machine_done.notify = virt_machine_done;
diff --git a/hw/arm/xilinx_zynq.c b/hw/arm/xilinx_zynq.c
index 50e7268396..3190cc0b8d 100644
--- a/hw/arm/xilinx_zynq.c
+++ b/hw/arm/xilinx_zynq.c
@@ -343,7 +343,6 @@ static void zynq_init(MachineState *machine)
     sysbus_mmio_map(busdev, 0, 0xF8007000);
 
     zynq_binfo.ram_size = machine->ram_size;
-    zynq_binfo.nb_cpus = 1;
     zynq_binfo.board_id = 0xd32;
     zynq_binfo.loader_start = 0;
     zynq_binfo.board_setup_addr = BOARD_SETUP_ADDR;
diff --git a/hw/arm/xlnx-versal-virt.c b/hw/arm/xlnx-versal-virt.c
index 3f56ae28ee..7c7baff8b7 100644
--- a/hw/arm/xlnx-versal-virt.c
+++ b/hw/arm/xlnx-versal-virt.c
@@ -628,6 +628,9 @@ static void versal_virt_init(MachineState *machine)
      * When loading an OS, we turn on QEMU's PSCI implementation with SMC
      * as the PSCI conduit. When there's no -kernel, we assume the user
      * provides EL3 firmware to handle PSCI.
+     *
+     * Even if the user provides a kernel filename, arm_load_kernel()
+     * may suppress PSCI if it's going to boot that guest code at EL3.
      */
     if (machine->kernel_filename) {
         psci_conduit = QEMU_PSCI_CONDUIT_SMC;
@@ -637,8 +640,6 @@ static void versal_virt_init(MachineState *machine)
                             TYPE_XLNX_VERSAL);
     object_property_set_link(OBJECT(&s->soc), "ddr", OBJECT(machine->ram),
                              &error_abort);
-    object_property_set_int(OBJECT(&s->soc), "psci-conduit", psci_conduit,
-                            &error_abort);
     sysbus_realize(SYS_BUS_DEVICE(&s->soc), &error_fatal);
 
     fdt_create(s);
@@ -679,20 +680,14 @@ static void versal_virt_init(MachineState *machine)
     s->binfo.loader_start = 0x0;
     s->binfo.get_dtb = versal_virt_get_dtb;
     s->binfo.modify_dtb = versal_virt_modify_dtb;
-    if (machine->kernel_filename) {
-        arm_load_kernel(&s->soc.fpd.apu.cpu[0], machine, &s->binfo);
-    } else {
-        AddressSpace *as = arm_boot_address_space(&s->soc.fpd.apu.cpu[0],
-                                                  &s->binfo);
+    s->binfo.psci_conduit = psci_conduit;
+    if (!machine->kernel_filename) {
         /* Some boot-loaders (e.g u-boot) don't like blobs at address 0 (NULL).
          * Offset things by 4K.  */
         s->binfo.loader_start = 0x1000;
         s->binfo.dtb_limit = 0x1000000;
-        if (arm_load_dtb(s->binfo.loader_start,
-                         &s->binfo, s->binfo.dtb_limit, as, machine) < 0) {
-            exit(EXIT_FAILURE);
-        }
     }
+    arm_load_kernel(&s->soc.fpd.apu.cpu[0], machine, &s->binfo);
 
     for (i = 0; i < XLNX_VERSAL_NUM_OSPI_FLASH; i++) {
         BusState *spi_bus;
diff --git a/hw/arm/xlnx-versal.c b/hw/arm/xlnx-versal.c
index ab58bebfd2..2551dfc22d 100644
--- a/hw/arm/xlnx-versal.c
+++ b/hw/arm/xlnx-versal.c
@@ -40,10 +40,8 @@ static void versal_create_apu_cpus(Versal *s)
         object_initialize_child(OBJECT(s), "apu-cpu[*]", &s->fpd.apu.cpu[i],
                                 XLNX_VERSAL_ACPU_TYPE);
         obj = OBJECT(&s->fpd.apu.cpu[i]);
-        object_property_set_int(obj, "psci-conduit", s->cfg.psci_conduit,
-                                &error_abort);
         if (i) {
-            /* Secondary CPUs start in PSCI powered-down state */
+            /* Secondary CPUs start in powered-down state */
             object_property_set_bool(obj, "start-powered-off", true,
                                      &error_abort);
         }
@@ -667,7 +665,6 @@ static void versal_init(Object *obj)
 static Property versal_properties[] = {
     DEFINE_PROP_LINK("ddr", Versal, cfg.mr_ddr, TYPE_MEMORY_REGION,
                      MemoryRegion *),
-    DEFINE_PROP_UINT32("psci-conduit", Versal, cfg.psci_conduit, 0),
     DEFINE_PROP_END_OF_LIST()
 };
 
diff --git a/hw/arm/xlnx-zcu102.c b/hw/arm/xlnx-zcu102.c
index 45eb19ab3b..4c84bb932a 100644
--- a/hw/arm/xlnx-zcu102.c
+++ b/hw/arm/xlnx-zcu102.c
@@ -236,6 +236,7 @@ static void xlnx_zcu102_init(MachineState *machine)
     s->binfo.ram_size = ram_size;
     s->binfo.loader_start = 0;
     s->binfo.modify_dtb = zcu102_modify_dtb;
+    s->binfo.psci_conduit = QEMU_PSCI_CONDUIT_SMC;
     arm_load_kernel(s->soc.boot_cpu_ptr, machine, &s->binfo);
 }
 
diff --git a/hw/arm/xlnx-zynqmp.c b/hw/arm/xlnx-zynqmp.c
index 1c52a575aa..6d0e4116db 100644
--- a/hw/arm/xlnx-zynqmp.c
+++ b/hw/arm/xlnx-zynqmp.c
@@ -50,6 +50,7 @@
 #define LQSPI_ADDR          0xc0000000
 #define QSPI_IRQ            15
 #define QSPI_DMA_ADDR       0xff0f0800
+#define NUM_QSPI_IRQ_LINES  2
 
 #define DP_ADDR             0xfd4a0000
 #define DP_IRQ              113
@@ -215,7 +216,9 @@ static void xlnx_zynqmp_create_rpu(MachineState *ms, XlnxZynqMPState *s,
 
         name = object_get_canonical_path_component(OBJECT(&s->rpu_cpu[i]));
         if (strcmp(name, boot_cpu)) {
-            /* Secondary CPUs start in PSCI powered-down state */
+            /*
+             * Secondary CPUs start in powered-down state.
+             */
             object_property_set_bool(OBJECT(&s->rpu_cpu[i]),
                                      "start-powered-off", true, &error_abort);
         } else {
@@ -362,6 +365,8 @@ static void xlnx_zynqmp_init(Object *obj)
     }
 
     object_initialize_child(obj, "qspi-dma", &s->qspi_dma, TYPE_XLNX_CSU_DMA);
+    object_initialize_child(obj, "qspi-irq-orgate",
+                            &s->qspi_irq_orgate, TYPE_OR_IRQ);
 }
 
 static void xlnx_zynqmp_realize(DeviceState *dev, Error **errp)
@@ -435,12 +440,11 @@ static void xlnx_zynqmp_realize(DeviceState *dev, Error **errp)
     for (i = 0; i < num_apus; i++) {
         const char *name;
 
-        object_property_set_int(OBJECT(&s->apu_cpu[i]), "psci-conduit",
-                                QEMU_PSCI_CONDUIT_SMC, &error_abort);
-
         name = object_get_canonical_path_component(OBJECT(&s->apu_cpu[i]));
         if (strcmp(name, boot_cpu)) {
-            /* Secondary CPUs start in PSCI powered-down state */
+            /*
+             * Secondary CPUs start in powered-down state.
+             */
             object_property_set_bool(OBJECT(&s->apu_cpu[i]),
                                      "start-powered-off", true, &error_abort);
         } else {
@@ -709,6 +713,11 @@ static void xlnx_zynqmp_realize(DeviceState *dev, Error **errp)
                            gic_spi[adma_ch_intr[i]]);
     }
 
+    object_property_set_int(OBJECT(&s->qspi_irq_orgate),
+                            "num-lines", NUM_QSPI_IRQ_LINES, &error_fatal);
+    qdev_realize(DEVICE(&s->qspi_irq_orgate), NULL, &error_fatal);
+    qdev_connect_gpio_out(DEVICE(&s->qspi_irq_orgate), 0, gic_spi[QSPI_IRQ]);
+
     if (!object_property_set_link(OBJECT(&s->qspi_dma), "dma",
                                   OBJECT(system_memory), errp)) {
         return;
@@ -718,7 +727,8 @@ static void xlnx_zynqmp_realize(DeviceState *dev, Error **errp)
     }
 
     sysbus_mmio_map(SYS_BUS_DEVICE(&s->qspi_dma), 0, QSPI_DMA_ADDR);
-    sysbus_connect_irq(SYS_BUS_DEVICE(&s->qspi_dma), 0, gic_spi[QSPI_IRQ]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->qspi_dma), 0,
+                       qdev_get_gpio_in(DEVICE(&s->qspi_irq_orgate), 0));
 
     if (!object_property_set_link(OBJECT(&s->qspi), "stream-connected-dma",
                                   OBJECT(&s->qspi_dma), errp)) {
@@ -729,7 +739,8 @@ static void xlnx_zynqmp_realize(DeviceState *dev, Error **errp)
     }
     sysbus_mmio_map(SYS_BUS_DEVICE(&s->qspi), 0, QSPI_ADDR);
     sysbus_mmio_map(SYS_BUS_DEVICE(&s->qspi), 1, LQSPI_ADDR);
-    sysbus_connect_irq(SYS_BUS_DEVICE(&s->qspi), 0, gic_spi[QSPI_IRQ]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->qspi), 0,
+                       qdev_get_gpio_in(DEVICE(&s->qspi_irq_orgate), 1));
 
     for (i = 0; i < XLNX_ZYNQMP_NUM_QSPI_BUS; i++) {
         g_autofree gchar *bus_name = g_strdup_printf("qspi%d", i);
diff --git a/hw/intc/arm_gicv3_its.c b/hw/intc/arm_gicv3_its.c
index 51d9be4ae6..4f598d3c14 100644
--- a/hw/intc/arm_gicv3_its.c
+++ b/hw/intc/arm_gicv3_its.c
@@ -41,10 +41,26 @@ typedef enum ItsCmdType {
     INTERRUPT = 3,
 } ItsCmdType;
 
-typedef struct {
-    uint32_t iteh;
-    uint64_t itel;
-} IteEntry;
+typedef struct DTEntry {
+    bool valid;
+    unsigned size;
+    uint64_t ittaddr;
+} DTEntry;
+
+typedef struct CTEntry {
+    bool valid;
+    uint32_t rdbase;
+} CTEntry;
+
+typedef struct ITEntry {
+    bool valid;
+    int inttype;
+    uint32_t intid;
+    uint32_t doorbell;
+    uint32_t icid;
+    uint32_t vpeid;
+} ITEntry;
+
 
 /*
  * The ITS spec permits a range of CONSTRAINED UNPREDICTABLE options
@@ -129,91 +145,126 @@ static uint64_t table_entry_addr(GICv3ITSState *s, TableDesc *td,
     return (l2 & ((1ULL << 51) - 1)) + (idx % num_l2_entries) * td->entry_sz;
 }
 
-static bool get_cte(GICv3ITSState *s, uint16_t icid, uint64_t *cte,
-                    MemTxResult *res)
+/*
+ * Read the Collection Table entry at index @icid. On success (including
+ * successfully determining that there is no valid CTE for this index),
+ * we return MEMTX_OK and populate the CTEntry struct @cte accordingly.
+ * If there is an error reading memory then we return the error code.
+ */
+static MemTxResult get_cte(GICv3ITSState *s, uint16_t icid, CTEntry *cte)
 {
     AddressSpace *as = &s->gicv3->dma_as;
-    uint64_t entry_addr = table_entry_addr(s, &s->ct, icid, res);
+    MemTxResult res = MEMTX_OK;
+    uint64_t entry_addr = table_entry_addr(s, &s->ct, icid, &res);
+    uint64_t cteval;
 
     if (entry_addr == -1) {
-        return false; /* not valid */
+        /* No L2 table entry, i.e. no valid CTE, or a memory error */
+        cte->valid = false;
+        return res;
     }
 
-    *cte = address_space_ldq_le(as, entry_addr, MEMTXATTRS_UNSPECIFIED, res);
-    return FIELD_EX64(*cte, CTE, VALID);
+    cteval = address_space_ldq_le(as, entry_addr, MEMTXATTRS_UNSPECIFIED, &res);
+    if (res != MEMTX_OK) {
+        return res;
+    }
+    cte->valid = FIELD_EX64(cteval, CTE, VALID);
+    cte->rdbase = FIELD_EX64(cteval, CTE, RDBASE);
+    return MEMTX_OK;
 }
 
-static bool update_ite(GICv3ITSState *s, uint32_t eventid, uint64_t dte,
-                       IteEntry ite)
+/*
+ * Update the Interrupt Table entry at index @evinted in the table specified
+ * by the dte @dte. Returns true on success, false if there was a memory
+ * access error.
+ */
+static bool update_ite(GICv3ITSState *s, uint32_t eventid, const DTEntry *dte,
+                       const ITEntry *ite)
 {
     AddressSpace *as = &s->gicv3->dma_as;
-    uint64_t itt_addr;
     MemTxResult res = MEMTX_OK;
+    hwaddr iteaddr = dte->ittaddr + eventid * ITS_ITT_ENTRY_SIZE;
+    uint64_t itel = 0;
+    uint32_t iteh = 0;
 
-    itt_addr = FIELD_EX64(dte, DTE, ITTADDR);
-    itt_addr <<= ITTADDR_SHIFT; /* 256 byte aligned */
-
-    address_space_stq_le(as, itt_addr + (eventid * (sizeof(uint64_t) +
-                         sizeof(uint32_t))), ite.itel, MEMTXATTRS_UNSPECIFIED,
-                         &res);
-
-    if (res == MEMTX_OK) {
-        address_space_stl_le(as, itt_addr + (eventid * (sizeof(uint64_t) +
-                             sizeof(uint32_t))) + sizeof(uint32_t), ite.iteh,
-                             MEMTXATTRS_UNSPECIFIED, &res);
+    if (ite->valid) {
+        itel = FIELD_DP64(itel, ITE_L, VALID, 1);
+        itel = FIELD_DP64(itel, ITE_L, INTTYPE, ite->inttype);
+        itel = FIELD_DP64(itel, ITE_L, INTID, ite->intid);
+        itel = FIELD_DP64(itel, ITE_L, ICID, ite->icid);
+        itel = FIELD_DP64(itel, ITE_L, VPEID, ite->vpeid);
+        iteh = FIELD_DP32(iteh, ITE_H, DOORBELL, ite->doorbell);
     }
+
+    address_space_stq_le(as, iteaddr, itel, MEMTXATTRS_UNSPECIFIED, &res);
     if (res != MEMTX_OK) {
         return false;
-    } else {
-        return true;
     }
+    address_space_stl_le(as, iteaddr + 8, iteh, MEMTXATTRS_UNSPECIFIED, &res);
+    return res == MEMTX_OK;
 }
 
-static bool get_ite(GICv3ITSState *s, uint32_t eventid, uint64_t dte,
-                    uint16_t *icid, uint32_t *pIntid, MemTxResult *res)
+/*
+ * Read the Interrupt Table entry at index @eventid from the table specified
+ * by the DTE @dte. On success, we return MEMTX_OK and populate the ITEntry
+ * struct @ite accordingly. If there is an error reading memory then we return
+ * the error code.
+ */
+static MemTxResult get_ite(GICv3ITSState *s, uint32_t eventid,
+                           const DTEntry *dte, ITEntry *ite)
 {
     AddressSpace *as = &s->gicv3->dma_as;
-    uint64_t itt_addr;
-    bool status = false;
-    IteEntry ite = {};
-
-    itt_addr = FIELD_EX64(dte, DTE, ITTADDR);
-    itt_addr <<= ITTADDR_SHIFT; /* 256 byte aligned */
-
-    ite.itel = address_space_ldq_le(as, itt_addr +
-                                    (eventid * (sizeof(uint64_t) +
-                                    sizeof(uint32_t))), MEMTXATTRS_UNSPECIFIED,
-                                    res);
-
-    if (*res == MEMTX_OK) {
-        ite.iteh = address_space_ldl_le(as, itt_addr +
-                                        (eventid * (sizeof(uint64_t) +
-                                        sizeof(uint32_t))) + sizeof(uint32_t),
-                                        MEMTXATTRS_UNSPECIFIED, res);
-
-        if (*res == MEMTX_OK) {
-            if (FIELD_EX64(ite.itel, ITE_L, VALID)) {
-                int inttype = FIELD_EX64(ite.itel, ITE_L, INTTYPE);
-                if (inttype == ITE_INTTYPE_PHYSICAL) {
-                    *pIntid = FIELD_EX64(ite.itel, ITE_L, INTID);
-                    *icid = FIELD_EX32(ite.iteh, ITE_H, ICID);
-                    status = true;
-                }
-            }
-        }
+    MemTxResult res = MEMTX_OK;
+    uint64_t itel;
+    uint32_t iteh;
+    hwaddr iteaddr = dte->ittaddr + eventid * ITS_ITT_ENTRY_SIZE;
+
+    itel = address_space_ldq_le(as, iteaddr, MEMTXATTRS_UNSPECIFIED, &res);
+    if (res != MEMTX_OK) {
+        return res;
+    }
+
+    iteh = address_space_ldl_le(as, iteaddr + 8, MEMTXATTRS_UNSPECIFIED, &res);
+    if (res != MEMTX_OK) {
+        return res;
     }
-    return status;
+
+    ite->valid = FIELD_EX64(itel, ITE_L, VALID);
+    ite->inttype = FIELD_EX64(itel, ITE_L, INTTYPE);
+    ite->intid = FIELD_EX64(itel, ITE_L, INTID);
+    ite->icid = FIELD_EX64(itel, ITE_L, ICID);
+    ite->vpeid = FIELD_EX64(itel, ITE_L, VPEID);
+    ite->doorbell = FIELD_EX64(iteh, ITE_H, DOORBELL);
+    return MEMTX_OK;
 }
 
-static uint64_t get_dte(GICv3ITSState *s, uint32_t devid, MemTxResult *res)
+/*
+ * Read the Device Table entry at index @devid. On success (including
+ * successfully determining that there is no valid DTE for this index),
+ * we return MEMTX_OK and populate the DTEntry struct accordingly.
+ * If there is an error reading memory then we return the error code.
+ */
+static MemTxResult get_dte(GICv3ITSState *s, uint32_t devid, DTEntry *dte)
 {
+    MemTxResult res = MEMTX_OK;
     AddressSpace *as = &s->gicv3->dma_as;
-    uint64_t entry_addr = table_entry_addr(s, &s->dt, devid, res);
+    uint64_t entry_addr = table_entry_addr(s, &s->dt, devid, &res);
+    uint64_t dteval;
 
     if (entry_addr == -1) {
-        return 0; /* a DTE entry with the Valid bit clear */
+        /* No L2 table entry, i.e. no valid DTE, or a memory error */
+        dte->valid = false;
+        return res;
     }
-    return address_space_ldq_le(as, entry_addr, MEMTXATTRS_UNSPECIFIED, res);
+    dteval = address_space_ldq_le(as, entry_addr, MEMTXATTRS_UNSPECIFIED, &res);
+    if (res != MEMTX_OK) {
+        return res;
+    }
+    dte->valid = FIELD_EX64(dteval, DTE, VALID);
+    dte->size = FIELD_EX64(dteval, DTE, SIZE);
+    /* DTE word field stores bits [51:8] of the ITT address */
+    dte->ittaddr = FIELD_EX64(dteval, DTE, ITTADDR) << ITTADDR_SHIFT;
+    return MEMTX_OK;
 }
 
 /*
@@ -224,37 +275,13 @@ static uint64_t get_dte(GICv3ITSState *s, uint32_t devid, MemTxResult *res)
  * 3. handling of ITS CLEAR command
  * 4. handling of ITS DISCARD command
  */
-static ItsCmdResult process_its_cmd(GICv3ITSState *s, uint64_t value,
-                                    uint32_t offset, ItsCmdType cmd)
+static ItsCmdResult do_process_its_cmd(GICv3ITSState *s, uint32_t devid,
+                                       uint32_t eventid, ItsCmdType cmd)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
-    uint32_t devid, eventid;
-    MemTxResult res = MEMTX_OK;
-    bool dte_valid;
-    uint64_t dte = 0;
     uint64_t num_eventids;
-    uint16_t icid = 0;
-    uint32_t pIntid = 0;
-    bool ite_valid = false;
-    uint64_t cte = 0;
-    bool cte_valid = false;
-    uint64_t rdbase;
-
-    if (cmd == NONE) {
-        devid = offset;
-    } else {
-        devid = ((value & DEVID_MASK) >> DEVID_SHIFT);
-
-        offset += NUM_BYTES_IN_DW;
-        value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                     MEMTXATTRS_UNSPECIFIED, &res);
-    }
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-
-    eventid = (value & EVENTID_MASK);
+    DTEntry dte;
+    CTEntry cte;
+    ITEntry ite;
 
     if (devid >= s->dt.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
@@ -263,23 +290,17 @@ static ItsCmdResult process_its_cmd(GICv3ITSState *s, uint64_t value,
         return CMD_CONTINUE;
     }
 
-    dte = get_dte(s, devid, &res);
-
-    if (res != MEMTX_OK) {
+    if (get_dte(s, devid, &dte) != MEMTX_OK) {
         return CMD_STALL;
     }
-    dte_valid = FIELD_EX64(dte, DTE, VALID);
-
-    if (!dte_valid) {
+    if (!dte.valid) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: "
-                      "invalid dte: %"PRIx64" for %d\n",
-                      __func__, dte, devid);
+                      "invalid dte for %d\n", __func__, devid);
         return CMD_CONTINUE;
     }
 
-    num_eventids = 1ULL << (FIELD_EX64(dte, DTE, SIZE) + 1);
-
+    num_eventids = 1ULL << (dte.size + 1);
     if (eventid >= num_eventids) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: eventid %d >= %"
@@ -288,34 +309,31 @@ static ItsCmdResult process_its_cmd(GICv3ITSState *s, uint64_t value,
         return CMD_CONTINUE;
     }
 
-    ite_valid = get_ite(s, eventid, dte, &icid, &pIntid, &res);
-    if (res != MEMTX_OK) {
+    if (get_ite(s, eventid, &dte, &ite) != MEMTX_OK) {
         return CMD_STALL;
     }
 
-    if (!ite_valid) {
+    if (!ite.valid || ite.inttype != ITE_INTTYPE_PHYSICAL) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: invalid ITE\n",
                       __func__);
         return CMD_CONTINUE;
     }
 
-    if (icid >= s->ct.num_entries) {
+    if (ite.icid >= s->ct.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid ICID 0x%x in ITE (table corrupted?)\n",
-                      __func__, icid);
+                      __func__, ite.icid);
         return CMD_CONTINUE;
     }
 
-    cte_valid = get_cte(s, icid, &cte, &res);
-    if (res != MEMTX_OK) {
+    if (get_cte(s, ite.icid, &cte) != MEMTX_OK) {
         return CMD_STALL;
     }
-    if (!cte_valid) {
+    if (!cte.valid) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "%s: invalid command attributes: "
-                      "invalid cte: %"PRIx64"\n",
-                      __func__, cte);
+                      "%s: invalid command attributes: invalid CTE\n",
+                      __func__);
         return CMD_CONTINUE;
     }
 
@@ -323,66 +341,55 @@ static ItsCmdResult process_its_cmd(GICv3ITSState *s, uint64_t value,
      * Current implementation only supports rdbase == procnum
      * Hence rdbase physical address is ignored
      */
-    rdbase = FIELD_EX64(cte, CTE, RDBASE);
-
-    if (rdbase >= s->gicv3->num_cpu) {
+    if (cte.rdbase >= s->gicv3->num_cpu) {
         return CMD_CONTINUE;
     }
 
     if ((cmd == CLEAR) || (cmd == DISCARD)) {
-        gicv3_redist_process_lpi(&s->gicv3->cpu[rdbase], pIntid, 0);
+        gicv3_redist_process_lpi(&s->gicv3->cpu[cte.rdbase], ite.intid, 0);
     } else {
-        gicv3_redist_process_lpi(&s->gicv3->cpu[rdbase], pIntid, 1);
+        gicv3_redist_process_lpi(&s->gicv3->cpu[cte.rdbase], ite.intid, 1);
     }
 
     if (cmd == DISCARD) {
-        IteEntry ite = {};
+        ITEntry ite = {};
         /* remove mapping from interrupt translation table */
-        return update_ite(s, eventid, dte, ite) ? CMD_CONTINUE : CMD_STALL;
+        ite.valid = false;
+        return update_ite(s, eventid, &dte, &ite) ? CMD_CONTINUE : CMD_STALL;
     }
     return CMD_CONTINUE;
 }
+static ItsCmdResult process_its_cmd(GICv3ITSState *s, const uint64_t *cmdpkt,
+                                    ItsCmdType cmd)
+{
+    uint32_t devid, eventid;
+
+    devid = (cmdpkt[0] & DEVID_MASK) >> DEVID_SHIFT;
+    eventid = cmdpkt[1] & EVENTID_MASK;
+    return do_process_its_cmd(s, devid, eventid, cmd);
+}
 
-static ItsCmdResult process_mapti(GICv3ITSState *s, uint64_t value,
-                                  uint32_t offset, bool ignore_pInt)
+static ItsCmdResult process_mapti(GICv3ITSState *s, const uint64_t *cmdpkt,
+                                  bool ignore_pInt)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
     uint32_t devid, eventid;
     uint32_t pIntid = 0;
     uint64_t num_eventids;
     uint32_t num_intids;
-    bool dte_valid;
-    MemTxResult res = MEMTX_OK;
     uint16_t icid = 0;
-    uint64_t dte = 0;
-    IteEntry ite = {};
+    DTEntry dte;
+    ITEntry ite;
 
-    devid = ((value & DEVID_MASK) >> DEVID_SHIFT);
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-
-    eventid = (value & EVENTID_MASK);
+    devid = (cmdpkt[0] & DEVID_MASK) >> DEVID_SHIFT;
+    eventid = cmdpkt[1] & EVENTID_MASK;
 
     if (ignore_pInt) {
         pIntid = eventid;
     } else {
-        pIntid = ((value & pINTID_MASK) >> pINTID_SHIFT);
-    }
-
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
+        pIntid = (cmdpkt[1] & pINTID_MASK) >> pINTID_SHIFT;
     }
 
-    icid = value & ICID_MASK;
+    icid = cmdpkt[2] & ICID_MASK;
 
     if (devid >= s->dt.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
@@ -391,58 +398,63 @@ static ItsCmdResult process_mapti(GICv3ITSState *s, uint64_t value,
         return CMD_CONTINUE;
     }
 
-    dte = get_dte(s, devid, &res);
-
-    if (res != MEMTX_OK) {
+    if (get_dte(s, devid, &dte) != MEMTX_OK) {
         return CMD_STALL;
     }
-    dte_valid = FIELD_EX64(dte, DTE, VALID);
-    num_eventids = 1ULL << (FIELD_EX64(dte, DTE, SIZE) + 1);
+    num_eventids = 1ULL << (dte.size + 1);
     num_intids = 1ULL << (GICD_TYPER_IDBITS + 1);
 
-    if ((icid >= s->ct.num_entries)
-            || !dte_valid || (eventid >= num_eventids) ||
-            (((pIntid < GICV3_LPI_INTID_START) || (pIntid >= num_intids)) &&
-             (pIntid != INTID_SPURIOUS))) {
+    if (icid >= s->ct.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "%s: invalid command attributes "
-                      "icid %d or eventid %d or pIntid %d or"
-                      "unmapped dte %d\n", __func__, icid, eventid,
-                      pIntid, dte_valid);
-        /*
-         * in this implementation, in case of error
-         * we ignore this command and move onto the next
-         * command in the queue
-         */
+                      "%s: invalid ICID 0x%x >= 0x%x\n",
+                      __func__, icid, s->ct.num_entries);
         return CMD_CONTINUE;
     }
 
-    /* add ite entry to interrupt translation table */
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, VALID, dte_valid);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, INTTYPE, ITE_INTTYPE_PHYSICAL);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, INTID, pIntid);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, DOORBELL, INTID_SPURIOUS);
-    ite.iteh = FIELD_DP32(ite.iteh, ITE_H, ICID, icid);
+    if (!dte.valid) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "%s: no valid DTE for devid 0x%x\n", __func__, devid);
+        return CMD_CONTINUE;
+    }
+
+    if (eventid >= num_eventids) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "%s: invalid event ID 0x%x >= 0x%" PRIx64 "\n",
+                      __func__, eventid, num_eventids);
+        return CMD_CONTINUE;
+    }
+
+    if (pIntid < GICV3_LPI_INTID_START || pIntid >= num_intids) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "%s: invalid interrupt ID 0x%x\n", __func__, pIntid);
+        return CMD_CONTINUE;
+    }
 
-    return update_ite(s, eventid, dte, ite) ? CMD_CONTINUE : CMD_STALL;
+    /* add ite entry to interrupt translation table */
+    ite.valid = true;
+    ite.inttype = ITE_INTTYPE_PHYSICAL;
+    ite.intid = pIntid;
+    ite.icid = icid;
+    ite.doorbell = INTID_SPURIOUS;
+    ite.vpeid = 0;
+    return update_ite(s, eventid, &dte, &ite) ? CMD_CONTINUE : CMD_STALL;
 }
 
-static bool update_cte(GICv3ITSState *s, uint16_t icid, bool valid,
-                       uint64_t rdbase)
+/*
+ * Update the Collection Table entry for @icid to @cte. Returns true
+ * on success, false if there was a memory access error.
+ */
+static bool update_cte(GICv3ITSState *s, uint16_t icid, const CTEntry *cte)
 {
     AddressSpace *as = &s->gicv3->dma_as;
     uint64_t entry_addr;
-    uint64_t cte = 0;
+    uint64_t cteval = 0;
     MemTxResult res = MEMTX_OK;
 
-    if (!s->ct.valid) {
-        return true;
-    }
-
-    if (valid) {
+    if (cte->valid) {
         /* add mapping entry to collection table */
-        cte = FIELD_DP64(cte, CTE, VALID, 1);
-        cte = FIELD_DP64(cte, CTE, RDBASE, rdbase);
+        cteval = FIELD_DP64(cteval, CTE, VALID, 1);
+        cteval = FIELD_DP64(cteval, CTE, RDBASE, cte->rdbase);
     }
 
     entry_addr = table_entry_addr(s, &s->ct, icid, &res);
@@ -455,68 +467,53 @@ static bool update_cte(GICv3ITSState *s, uint16_t icid, bool valid,
         return true;
     }
 
-    address_space_stq_le(as, entry_addr, cte, MEMTXATTRS_UNSPECIFIED, &res);
+    address_space_stq_le(as, entry_addr, cteval, MEMTXATTRS_UNSPECIFIED, &res);
     return res == MEMTX_OK;
 }
 
-static ItsCmdResult process_mapc(GICv3ITSState *s, uint32_t offset)
+static ItsCmdResult process_mapc(GICv3ITSState *s, const uint64_t *cmdpkt)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
     uint16_t icid;
-    uint64_t rdbase;
-    bool valid;
-    MemTxResult res = MEMTX_OK;
-    uint64_t value;
-
-    offset += NUM_BYTES_IN_DW;
-    offset += NUM_BYTES_IN_DW;
+    CTEntry cte;
 
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
+    icid = cmdpkt[2] & ICID_MASK;
+    cte.valid = cmdpkt[2] & CMD_FIELD_VALID_MASK;
+    if (cte.valid) {
+        cte.rdbase = (cmdpkt[2] & R_MAPC_RDBASE_MASK) >> R_MAPC_RDBASE_SHIFT;
+        cte.rdbase &= RDBASE_PROCNUM_MASK;
+    } else {
+        cte.rdbase = 0;
     }
 
-    icid = value & ICID_MASK;
-
-    rdbase = (value & R_MAPC_RDBASE_MASK) >> R_MAPC_RDBASE_SHIFT;
-    rdbase &= RDBASE_PROCNUM_MASK;
-
-    valid = (value & CMD_FIELD_VALID_MASK);
-
-    if ((icid >= s->ct.num_entries) || (rdbase >= s->gicv3->num_cpu)) {
+    if (icid >= s->ct.num_entries) {
+        qemu_log_mask(LOG_GUEST_ERROR, "ITS MAPC: invalid ICID 0x%d", icid);
+        return CMD_CONTINUE;
+    }
+    if (cte.valid && cte.rdbase >= s->gicv3->num_cpu) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "ITS MAPC: invalid collection table attributes "
-                      "icid %d rdbase %" PRIu64 "\n",  icid, rdbase);
-        /*
-         * in this implementation, in case of error
-         * we ignore this command and move onto the next
-         * command in the queue
-         */
+                      "ITS MAPC: invalid RDBASE %u ", cte.rdbase);
         return CMD_CONTINUE;
     }
 
-    return update_cte(s, icid, valid, rdbase) ? CMD_CONTINUE : CMD_STALL;
+    return update_cte(s, icid, &cte) ? CMD_CONTINUE : CMD_STALL;
 }
 
-static bool update_dte(GICv3ITSState *s, uint32_t devid, bool valid,
-                       uint8_t size, uint64_t itt_addr)
+/*
+ * Update the Device Table entry for @devid to @dte. Returns true
+ * on success, false if there was a memory access error.
+ */
+static bool update_dte(GICv3ITSState *s, uint32_t devid, const DTEntry *dte)
 {
     AddressSpace *as = &s->gicv3->dma_as;
     uint64_t entry_addr;
-    uint64_t dte = 0;
+    uint64_t dteval = 0;
     MemTxResult res = MEMTX_OK;
 
-    if (s->dt.valid) {
-        if (valid) {
-            /* add mapping entry to device table */
-            dte = FIELD_DP64(dte, DTE, VALID, 1);
-            dte = FIELD_DP64(dte, DTE, SIZE, size);
-            dte = FIELD_DP64(dte, DTE, ITTADDR, itt_addr);
-        }
-    } else {
-        return true;
+    if (dte->valid) {
+        /* add mapping entry to device table */
+        dteval = FIELD_DP64(dteval, DTE, VALID, 1);
+        dteval = FIELD_DP64(dteval, DTE, SIZE, dte->size);
+        dteval = FIELD_DP64(dteval, DTE, ITTADDR, dte->ittaddr);
     }
 
     entry_addr = table_entry_addr(s, &s->dt, devid, &res);
@@ -528,77 +525,43 @@ static bool update_dte(GICv3ITSState *s, uint32_t devid, bool valid,
         /* No L2 table for this index: discard write and continue */
         return true;
     }
-    address_space_stq_le(as, entry_addr, dte, MEMTXATTRS_UNSPECIFIED, &res);
+    address_space_stq_le(as, entry_addr, dteval, MEMTXATTRS_UNSPECIFIED, &res);
     return res == MEMTX_OK;
 }
 
-static ItsCmdResult process_mapd(GICv3ITSState *s, uint64_t value,
-                                 uint32_t offset)
+static ItsCmdResult process_mapd(GICv3ITSState *s, const uint64_t *cmdpkt)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
     uint32_t devid;
-    uint8_t size;
-    uint64_t itt_addr;
-    bool valid;
-    MemTxResult res = MEMTX_OK;
+    DTEntry dte;
 
-    devid = ((value & DEVID_MASK) >> DEVID_SHIFT);
+    devid = (cmdpkt[0] & DEVID_MASK) >> DEVID_SHIFT;
+    dte.size = cmdpkt[1] & SIZE_MASK;
+    dte.ittaddr = (cmdpkt[2] & ITTADDR_MASK) >> ITTADDR_SHIFT;
+    dte.valid = cmdpkt[2] & CMD_FIELD_VALID_MASK;
 
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-
-    size = (value & SIZE_MASK);
-
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
+    if (devid >= s->dt.num_entries) {
+        qemu_log_mask(LOG_GUEST_ERROR,
+                      "ITS MAPD: invalid device ID field 0x%x >= 0x%x\n",
+                      devid, s->dt.num_entries);
+        return CMD_CONTINUE;
     }
 
-    itt_addr = (value & ITTADDR_MASK) >> ITTADDR_SHIFT;
-
-    valid = (value & CMD_FIELD_VALID_MASK);
-
-    if ((devid >= s->dt.num_entries) ||
-        (size > FIELD_EX64(s->typer, GITS_TYPER, IDBITS))) {
+    if (dte.size > FIELD_EX64(s->typer, GITS_TYPER, IDBITS)) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "ITS MAPD: invalid device table attributes "
-                      "devid %d or size %d\n", devid, size);
-        /*
-         * in this implementation, in case of error
-         * we ignore this command and move onto the next
-         * command in the queue
-         */
+                      "ITS MAPD: invalid size %d\n", dte.size);
         return CMD_CONTINUE;
     }
 
-    return update_dte(s, devid, valid, size, itt_addr) ? CMD_CONTINUE : CMD_STALL;
+    return update_dte(s, devid, &dte) ? CMD_CONTINUE : CMD_STALL;
 }
 
-static ItsCmdResult process_movall(GICv3ITSState *s, uint64_t value,
-                                   uint32_t offset)
+static ItsCmdResult process_movall(GICv3ITSState *s, const uint64_t *cmdpkt)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
-    MemTxResult res = MEMTX_OK;
     uint64_t rd1, rd2;
 
-    /* No fields in dwords 0 or 1 */
-    offset += NUM_BYTES_IN_DW;
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
+    rd1 = FIELD_EX64(cmdpkt[2], MOVALL_2, RDBASE1);
+    rd2 = FIELD_EX64(cmdpkt[3], MOVALL_3, RDBASE2);
 
-    rd1 = FIELD_EX64(value, MOVALL_2, RDBASE1);
     if (rd1 >= s->gicv3->num_cpu) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: RDBASE1 %" PRId64
@@ -606,15 +569,6 @@ static ItsCmdResult process_movall(GICv3ITSState *s, uint64_t value,
                       __func__, rd1, s->gicv3->num_cpu);
         return CMD_CONTINUE;
     }
-
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-
-    rd2 = FIELD_EX64(value, MOVALL_3, RDBASE2);
     if (rd2 >= s->gicv3->num_cpu) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: RDBASE2 %" PRId64
@@ -634,37 +588,18 @@ static ItsCmdResult process_movall(GICv3ITSState *s, uint64_t value,
     return CMD_CONTINUE;
 }
 
-static ItsCmdResult process_movi(GICv3ITSState *s, uint64_t value,
-                                 uint32_t offset)
+static ItsCmdResult process_movi(GICv3ITSState *s, const uint64_t *cmdpkt)
 {
-    AddressSpace *as = &s->gicv3->dma_as;
-    MemTxResult res = MEMTX_OK;
-    uint32_t devid, eventid, intid;
-    uint16_t old_icid, new_icid;
-    uint64_t old_cte, new_cte;
-    uint64_t old_rdbase, new_rdbase;
-    uint64_t dte;
-    bool dte_valid, ite_valid, cte_valid;
+    uint32_t devid, eventid;
+    uint16_t new_icid;
     uint64_t num_eventids;
-    IteEntry ite = {};
-
-    devid = FIELD_EX64(value, MOVI_0, DEVICEID);
+    DTEntry dte;
+    CTEntry old_cte, new_cte;
+    ITEntry old_ite;
 
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-    eventid = FIELD_EX64(value, MOVI_1, EVENTID);
-
-    offset += NUM_BYTES_IN_DW;
-    value = address_space_ldq_le(as, s->cq.base_addr + offset,
-                                 MEMTXATTRS_UNSPECIFIED, &res);
-    if (res != MEMTX_OK) {
-        return CMD_STALL;
-    }
-    new_icid = FIELD_EX64(value, MOVI_2, ICID);
+    devid = FIELD_EX64(cmdpkt[0], MOVI_0, DEVICEID);
+    eventid = FIELD_EX64(cmdpkt[1], MOVI_1, EVENTID);
+    new_icid = FIELD_EX64(cmdpkt[2], MOVI_2, ICID);
 
     if (devid >= s->dt.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
@@ -672,21 +607,18 @@ static ItsCmdResult process_movi(GICv3ITSState *s, uint64_t value,
                       __func__, devid, s->dt.num_entries);
         return CMD_CONTINUE;
     }
-    dte = get_dte(s, devid, &res);
-    if (res != MEMTX_OK) {
+    if (get_dte(s, devid, &dte) != MEMTX_OK) {
         return CMD_STALL;
     }
 
-    dte_valid = FIELD_EX64(dte, DTE, VALID);
-    if (!dte_valid) {
+    if (!dte.valid) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: "
-                      "invalid dte: %"PRIx64" for %d\n",
-                      __func__, dte, devid);
+                      "invalid dte for %d\n", __func__, devid);
         return CMD_CONTINUE;
     }
 
-    num_eventids = 1ULL << (FIELD_EX64(dte, DTE, SIZE) + 1);
+    num_eventids = 1ULL << (dte.size + 1);
     if (eventid >= num_eventids) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: eventid %d >= %"
@@ -695,22 +627,21 @@ static ItsCmdResult process_movi(GICv3ITSState *s, uint64_t value,
         return CMD_CONTINUE;
     }
 
-    ite_valid = get_ite(s, eventid, dte, &old_icid, &intid, &res);
-    if (res != MEMTX_OK) {
+    if (get_ite(s, eventid, &dte, &old_ite) != MEMTX_OK) {
         return CMD_STALL;
     }
 
-    if (!ite_valid) {
+    if (!old_ite.valid || old_ite.inttype != ITE_INTTYPE_PHYSICAL) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: invalid ITE\n",
                       __func__);
         return CMD_CONTINUE;
     }
 
-    if (old_icid >= s->ct.num_entries) {
+    if (old_ite.icid >= s->ct.num_entries) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid ICID 0x%x in ITE (table corrupted?)\n",
-                      __func__, old_icid);
+                      __func__, old_ite.icid);
         return CMD_CONTINUE;
     }
 
@@ -721,60 +652,52 @@ static ItsCmdResult process_movi(GICv3ITSState *s, uint64_t value,
         return CMD_CONTINUE;
     }
 
-    cte_valid = get_cte(s, old_icid, &old_cte, &res);
-    if (res != MEMTX_OK) {
+    if (get_cte(s, old_ite.icid, &old_cte) != MEMTX_OK) {
         return CMD_STALL;
     }
-    if (!cte_valid) {
+    if (!old_cte.valid) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: "
-                      "invalid cte: %"PRIx64"\n",
-                      __func__, old_cte);
+                      "invalid CTE for old ICID 0x%x\n",
+                      __func__, old_ite.icid);
         return CMD_CONTINUE;
     }
 
-    cte_valid = get_cte(s, new_icid, &new_cte, &res);
-    if (res != MEMTX_OK) {
+    if (get_cte(s, new_icid, &new_cte) != MEMTX_OK) {
         return CMD_STALL;
     }
-    if (!cte_valid) {
+    if (!new_cte.valid) {
         qemu_log_mask(LOG_GUEST_ERROR,
                       "%s: invalid command attributes: "
-                      "invalid cte: %"PRIx64"\n",
-                      __func__, new_cte);
+                      "invalid CTE for new ICID 0x%x\n",
+                      __func__, new_icid);
         return CMD_CONTINUE;
     }
 
-    old_rdbase = FIELD_EX64(old_cte, CTE, RDBASE);
-    if (old_rdbase >= s->gicv3->num_cpu) {
+    if (old_cte.rdbase >= s->gicv3->num_cpu) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "%s: CTE has invalid rdbase 0x%"PRIx64"\n",
-                      __func__, old_rdbase);
+                      "%s: CTE has invalid rdbase 0x%x\n",
+                      __func__, old_cte.rdbase);
         return CMD_CONTINUE;
     }
 
-    new_rdbase = FIELD_EX64(new_cte, CTE, RDBASE);
-    if (new_rdbase >= s->gicv3->num_cpu) {
+    if (new_cte.rdbase >= s->gicv3->num_cpu) {
         qemu_log_mask(LOG_GUEST_ERROR,
-                      "%s: CTE has invalid rdbase 0x%"PRIx64"\n",
-                      __func__, new_rdbase);
+                      "%s: CTE has invalid rdbase 0x%x\n",
+                      __func__, new_cte.rdbase);
         return CMD_CONTINUE;
     }
 
-    if (old_rdbase != new_rdbase) {
+    if (old_cte.rdbase != new_cte.rdbase) {
         /* Move the LPI from the old redistributor to the new one */
-        gicv3_redist_mov_lpi(&s->gicv3->cpu[old_rdbase],
-                             &s->gicv3->cpu[new_rdbase],
-                             intid);
+        gicv3_redist_mov_lpi(&s->gicv3->cpu[old_cte.rdbase],
+                             &s->gicv3->cpu[new_cte.rdbase],
+                             old_ite.intid);
     }
 
     /* Update the ICID field in the interrupt translation table entry */
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, VALID, 1);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, INTTYPE, ITE_INTTYPE_PHYSICAL);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, INTID, intid);
-    ite.itel = FIELD_DP64(ite.itel, ITE_L, DOORBELL, INTID_SPURIOUS);
-    ite.iteh = FIELD_DP32(ite.iteh, ITE_H, ICID, new_icid);
-    return update_ite(s, eventid, dte, ite) ? CMD_CONTINUE : CMD_STALL;
+    old_ite.icid = new_icid;
+    return update_ite(s, eventid, &dte, &old_ite) ? CMD_CONTINUE : CMD_STALL;
 }
 
 /*
@@ -786,9 +709,7 @@ static void process_cmdq(GICv3ITSState *s)
     uint32_t wr_offset = 0;
     uint32_t rd_offset = 0;
     uint32_t cq_offset = 0;
-    uint64_t data;
     AddressSpace *as = &s->gicv3->dma_as;
-    MemTxResult res = MEMTX_OK;
     uint8_t cmd;
     int i;
 
@@ -816,28 +737,40 @@ static void process_cmdq(GICv3ITSState *s)
 
     while (wr_offset != rd_offset) {
         ItsCmdResult result = CMD_CONTINUE;
+        void *hostmem;
+        hwaddr buflen;
+        uint64_t cmdpkt[GITS_CMDQ_ENTRY_WORDS];
 
         cq_offset = (rd_offset * GITS_CMDQ_ENTRY_SIZE);
-        data = address_space_ldq_le(as, s->cq.base_addr + cq_offset,
-                                    MEMTXATTRS_UNSPECIFIED, &res);
-        if (res != MEMTX_OK) {
+
+        buflen = GITS_CMDQ_ENTRY_SIZE;
+        hostmem = address_space_map(as, s->cq.base_addr + cq_offset,
+                                    &buflen, false, MEMTXATTRS_UNSPECIFIED);
+        if (!hostmem || buflen != GITS_CMDQ_ENTRY_SIZE) {
+            if (hostmem) {
+                address_space_unmap(as, hostmem, buflen, false, 0);
+            }
             s->creadr = FIELD_DP64(s->creadr, GITS_CREADR, STALLED, 1);
             qemu_log_mask(LOG_GUEST_ERROR,
                           "%s: could not read command at 0x%" PRIx64 "\n",
                           __func__, s->cq.base_addr + cq_offset);
             break;
         }
+        for (i = 0; i < ARRAY_SIZE(cmdpkt); i++) {
+            cmdpkt[i] = ldq_le_p(hostmem + i * sizeof(uint64_t));
+        }
+        address_space_unmap(as, hostmem, buflen, false, 0);
 
-        cmd = (data & CMD_MASK);
+        cmd = cmdpkt[0] & CMD_MASK;
 
         trace_gicv3_its_process_command(rd_offset, cmd);
 
         switch (cmd) {
         case GITS_CMD_INT:
-            result = process_its_cmd(s, data, cq_offset, INTERRUPT);
+            result = process_its_cmd(s, cmdpkt, INTERRUPT);
             break;
         case GITS_CMD_CLEAR:
-            result = process_its_cmd(s, data, cq_offset, CLEAR);
+            result = process_its_cmd(s, cmdpkt, CLEAR);
             break;
         case GITS_CMD_SYNC:
             /*
@@ -848,19 +781,19 @@ static void process_cmdq(GICv3ITSState *s)
              */
             break;
         case GITS_CMD_MAPD:
-            result = process_mapd(s, data, cq_offset);
+            result = process_mapd(s, cmdpkt);
             break;
         case GITS_CMD_MAPC:
-            result = process_mapc(s, cq_offset);
+            result = process_mapc(s, cmdpkt);
             break;
         case GITS_CMD_MAPTI:
-            result = process_mapti(s, data, cq_offset, false);
+            result = process_mapti(s, cmdpkt, false);
             break;
         case GITS_CMD_MAPI:
-            result = process_mapti(s, data, cq_offset, true);
+            result = process_mapti(s, cmdpkt, true);
             break;
         case GITS_CMD_DISCARD:
-            result = process_its_cmd(s, data, cq_offset, DISCARD);
+            result = process_its_cmd(s, cmdpkt, DISCARD);
             break;
         case GITS_CMD_INV:
         case GITS_CMD_INVALL:
@@ -875,10 +808,10 @@ static void process_cmdq(GICv3ITSState *s)
             }
             break;
         case GITS_CMD_MOVI:
-            result = process_movi(s, data, cq_offset);
+            result = process_movi(s, cmdpkt);
             break;
         case GITS_CMD_MOVALL:
-            result = process_movall(s, data, cq_offset);
+            result = process_movall(s, cmdpkt);
             break;
         default:
             break;
@@ -969,7 +902,6 @@ static void extract_table_params(GICv3ITSState *s)
         }
 
         memset(td, 0, sizeof(*td));
-        td->valid = FIELD_EX64(value, GITS_BASER, VALID);
         /*
          * If GITS_BASER<n>.Valid is 0 for any <n> then we will not process
          * interrupts. (GITS_TYPER.HCC is 0 for this implementation, so we
@@ -977,8 +909,15 @@ static void extract_table_params(GICv3ITSState *s)
          * for the register corresponding to the Collection table but we
          * still have to process interrupts using non-memory-backed
          * Collection table entries.)
+         * The specification makes it UNPREDICTABLE to enable the ITS without
+         * marking each BASER<n> as valid. We choose to handle these as if
+         * the table was zero-sized, so commands using the table will fail
+         * and interrupts requested via GITS_TRANSLATER writes will be ignored.
+         * This happens automatically by leaving the num_entries field at
+         * zero, which will be caught by the bounds checks we have before
+         * every table lookup anyway.
          */
-        if (!td->valid) {
+        if (!FIELD_EX64(value, GITS_BASER, VALID)) {
             continue;
         }
         td->page_sz = page_sz;
@@ -1004,9 +943,8 @@ static void extract_cmdq_params(GICv3ITSState *s)
     num_pages = FIELD_EX64(value, GITS_CBASER, SIZE) + 1;
 
     memset(&s->cq, 0 , sizeof(s->cq));
-    s->cq.valid = FIELD_EX64(value, GITS_CBASER, VALID);
 
-    if (s->cq.valid) {
+    if (FIELD_EX64(value, GITS_CBASER, VALID)) {
         s->cq.num_entries = (num_pages * GITS_PAGE_SIZE_4K) /
                              GITS_CMDQ_ENTRY_SIZE;
         s->cq.base_addr = FIELD_EX64(value, GITS_CBASER, PHYADDR);
@@ -1032,15 +970,13 @@ static MemTxResult gicv3_its_translation_write(void *opaque, hwaddr offset,
 {
     GICv3ITSState *s = (GICv3ITSState *)opaque;
     bool result = true;
-    uint32_t devid = 0;
 
     trace_gicv3_its_translation_write(offset, data, size, attrs.requester_id);
 
     switch (offset) {
     case GITS_TRANSLATER:
         if (s->ctlr & R_GITS_CTLR_ENABLED_MASK) {
-            devid = attrs.requester_id;
-            result = process_its_cmd(s, data, devid, NONE);
+            result = do_process_its_cmd(s, attrs.requester_id, data, NONE);
         }
         break;
     default:
diff --git a/hw/intc/gicv3_internal.h b/hw/intc/gicv3_internal.h
index b1af26df9f..2bf1baef04 100644
--- a/hw/intc/gicv3_internal.h
+++ b/hw/intc/gicv3_internal.h
@@ -309,8 +309,8 @@ FIELD(GITS_TYPER, CIL, 36, 1)
 #define LPI_CTE_ENABLED          TABLE_ENTRY_VALID_MASK
 #define LPI_PRIORITY_MASK         0xfc
 
-#define GITS_CMDQ_ENTRY_SIZE               32
-#define NUM_BYTES_IN_DW                     8
+#define GITS_CMDQ_ENTRY_WORDS 4
+#define GITS_CMDQ_ENTRY_SIZE  (GITS_CMDQ_ENTRY_WORDS * sizeof(uint64_t))
 
 #define CMD_MASK                  0xff
 
@@ -370,22 +370,23 @@ FIELD(MOVI_2, ICID, 0, 16)
  * 12 bytes Interrupt translation Table Entry size
  * as per Table 5.3 in GICv3 spec
  * ITE Lower 8 Bytes
- *   Bits:    | 49 ... 26 | 25 ... 2 |   1     |   0    |
- *   Values:  |  Doorbell |  IntNum  | IntType |  Valid |
+ *   Bits:    | 63 ... 48 | 47 ... 32 | 31 ... 26 | 25 ... 2 |   1     |  0    |
+ *   Values:  | vPEID     | ICID      | unused    |  IntNum  | IntType | Valid |
  * ITE Higher 4 Bytes
- *   Bits:    | 31 ... 16 | 15 ...0 |
- *   Values:  |  vPEID    |  ICID   |
- * (When Doorbell is unused, as it always is in GICv3, it is 1023)
+ *   Bits:    | 31 ... 25 | 24 ... 0 |
+ *   Values:  | unused    | Doorbell |
+ * (When Doorbell is unused, as it always is for INTYPE_PHYSICAL,
+ * the value of that field in memory cannot be relied upon -- older
+ * versions of QEMU did not correctly write to that memory.)
  */
 #define ITS_ITT_ENTRY_SIZE            0xC
 
 FIELD(ITE_L, VALID, 0, 1)
 FIELD(ITE_L, INTTYPE, 1, 1)
 FIELD(ITE_L, INTID, 2, 24)
-FIELD(ITE_L, DOORBELL, 26, 24)
-
-FIELD(ITE_H, ICID, 0, 16)
-FIELD(ITE_H, VPEID, 16, 16)
+FIELD(ITE_L, ICID, 32, 16)
+FIELD(ITE_L, VPEID, 48, 16)
+FIELD(ITE_H, DOORBELL, 0, 24)
 
 /* Possible values for ITE_L INTTYPE */
 #define ITE_INTTYPE_VIRTUAL 0
diff --git a/hw/sensor/Kconfig b/hw/sensor/Kconfig
index 9c8a049b06..b317f91b7b 100644
--- a/hw/sensor/Kconfig
+++ b/hw/sensor/Kconfig
@@ -21,3 +21,7 @@ config ADM1272
 config MAX34451
     bool
     depends on I2C
+
+config LSM303DLHC_MAG
+    bool
+    depends on I2C
diff --git a/hw/sensor/lsm303dlhc_mag.c b/hw/sensor/lsm303dlhc_mag.c
new file mode 100644
index 0000000000..4c98ddbf20
--- /dev/null
+++ b/hw/sensor/lsm303dlhc_mag.c
@@ -0,0 +1,556 @@
+/*
+ * LSM303DLHC I2C magnetometer.
+ *
+ * Copyright (C) 2021 Linaro Ltd.
+ * Written by Kevin Townsend <kevin.townsend@linaro.org>
+ *
+ * Based on: https://www.st.com/resource/en/datasheet/lsm303dlhc.pdf
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+/*
+ * The I2C address associated with this device is set on the command-line when
+ * initialising the machine, but the following address is standard: 0x1E.
+ *
+ * Get and set functions for 'mag-x', 'mag-y' and 'mag-z' assume that
+ * 1 = 0.001 uT. (NOTE the 1 gauss = 100 uT, so setting a value of 100,000
+ * would be equal to 1 gauss or 100 uT.)
+ *
+ * Get and set functions for 'temperature' assume that 1 = 0.001 C, so 23.6 C
+ * would be equal to 23600.
+ */
+
+#include "qemu/osdep.h"
+#include "hw/i2c/i2c.h"
+#include "migration/vmstate.h"
+#include "qapi/error.h"
+#include "qapi/visitor.h"
+#include "qemu/module.h"
+#include "qemu/log.h"
+#include "qemu/bswap.h"
+
+enum LSM303DLHCMagReg {
+    LSM303DLHC_MAG_REG_CRA          = 0x00,
+    LSM303DLHC_MAG_REG_CRB          = 0x01,
+    LSM303DLHC_MAG_REG_MR           = 0x02,
+    LSM303DLHC_MAG_REG_OUT_X_H      = 0x03,
+    LSM303DLHC_MAG_REG_OUT_X_L      = 0x04,
+    LSM303DLHC_MAG_REG_OUT_Z_H      = 0x05,
+    LSM303DLHC_MAG_REG_OUT_Z_L      = 0x06,
+    LSM303DLHC_MAG_REG_OUT_Y_H      = 0x07,
+    LSM303DLHC_MAG_REG_OUT_Y_L      = 0x08,
+    LSM303DLHC_MAG_REG_SR           = 0x09,
+    LSM303DLHC_MAG_REG_IRA          = 0x0A,
+    LSM303DLHC_MAG_REG_IRB          = 0x0B,
+    LSM303DLHC_MAG_REG_IRC          = 0x0C,
+    LSM303DLHC_MAG_REG_TEMP_OUT_H   = 0x31,
+    LSM303DLHC_MAG_REG_TEMP_OUT_L   = 0x32
+};
+
+typedef struct LSM303DLHCMagState {
+    I2CSlave parent_obj;
+    uint8_t cra;
+    uint8_t crb;
+    uint8_t mr;
+    int16_t x;
+    int16_t z;
+    int16_t y;
+    int16_t x_lock;
+    int16_t z_lock;
+    int16_t y_lock;
+    uint8_t sr;
+    uint8_t ira;
+    uint8_t irb;
+    uint8_t irc;
+    int16_t temperature;
+    int16_t temperature_lock;
+    uint8_t len;
+    uint8_t buf;
+    uint8_t pointer;
+} LSM303DLHCMagState;
+
+#define TYPE_LSM303DLHC_MAG "lsm303dlhc_mag"
+OBJECT_DECLARE_SIMPLE_TYPE(LSM303DLHCMagState, LSM303DLHC_MAG)
+
+/*
+ * Conversion factor from Gauss to sensor values for each GN gain setting,
+ * in units "lsb per Gauss" (see data sheet table 3). There is no documented
+ * behaviour if the GN setting in CRB is incorrectly set to 0b000;
+ * we arbitrarily make it the same as 0b001.
+ */
+uint32_t xy_gain[] = { 1100, 1100, 855, 670, 450, 400, 330, 230 };
+uint32_t z_gain[] = { 980, 980, 760, 600, 400, 355, 295, 205 };
+
+static void lsm303dlhc_mag_get_x(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int gm = extract32(s->crb, 5, 3);
+
+    /* Convert to uT where 1000 = 1 uT. Conversion factor depends on gain. */
+    int64_t value = muldiv64(s->x, 100000, xy_gain[gm]);
+    visit_type_int(v, name, &value, errp);
+}
+
+static void lsm303dlhc_mag_get_y(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int gm = extract32(s->crb, 5, 3);
+
+    /* Convert to uT where 1000 = 1 uT. Conversion factor depends on gain. */
+    int64_t value = muldiv64(s->y, 100000, xy_gain[gm]);
+    visit_type_int(v, name, &value, errp);
+}
+
+static void lsm303dlhc_mag_get_z(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int gm = extract32(s->crb, 5, 3);
+
+    /* Convert to uT where 1000 = 1 uT. Conversion factor depends on gain. */
+    int64_t value = muldiv64(s->z, 100000, z_gain[gm]);
+    visit_type_int(v, name, &value, errp);
+}
+
+static void lsm303dlhc_mag_set_x(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int64_t value;
+    int64_t reg;
+    int gm = extract32(s->crb, 5, 3);
+
+    if (!visit_type_int(v, name, &value, errp)) {
+        return;
+    }
+
+    reg = muldiv64(value, xy_gain[gm], 100000);
+
+    /* Make sure we are within a 12-bit limit. */
+    if (reg > 2047 || reg < -2048) {
+        error_setg(errp, "value %" PRId64 " out of register's range", value);
+        return;
+    }
+
+    s->x = (int16_t)reg;
+}
+
+static void lsm303dlhc_mag_set_y(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int64_t value;
+    int64_t reg;
+    int gm = extract32(s->crb, 5, 3);
+
+    if (!visit_type_int(v, name, &value, errp)) {
+        return;
+    }
+
+    reg = muldiv64(value, xy_gain[gm], 100000);
+
+    /* Make sure we are within a 12-bit limit. */
+    if (reg > 2047 || reg < -2048) {
+        error_setg(errp, "value %" PRId64 " out of register's range", value);
+        return;
+    }
+
+    s->y = (int16_t)reg;
+}
+
+static void lsm303dlhc_mag_set_z(Object *obj, Visitor *v, const char *name,
+                                 void *opaque, Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int64_t value;
+    int64_t reg;
+    int gm = extract32(s->crb, 5, 3);
+
+    if (!visit_type_int(v, name, &value, errp)) {
+        return;
+    }
+
+    reg = muldiv64(value, z_gain[gm], 100000);
+
+    /* Make sure we are within a 12-bit limit. */
+    if (reg > 2047 || reg < -2048) {
+        error_setg(errp, "value %" PRId64 " out of register's range", value);
+        return;
+    }
+
+    s->z = (int16_t)reg;
+}
+
+/*
+ * Get handler for the temperature property.
+ */
+static void lsm303dlhc_mag_get_temperature(Object *obj, Visitor *v,
+                                           const char *name, void *opaque,
+                                           Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int64_t value;
+
+    /* Convert to 1 lsb = 0.125 C to 1 = 0.001 C for 'temperature' property. */
+    value = s->temperature * 125;
+
+    visit_type_int(v, name, &value, errp);
+}
+
+/*
+ * Set handler for the temperature property.
+ */
+static void lsm303dlhc_mag_set_temperature(Object *obj, Visitor *v,
+                                           const char *name, void *opaque,
+                                           Error **errp)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(obj);
+    int64_t value;
+
+    if (!visit_type_int(v, name, &value, errp)) {
+        return;
+    }
+
+    /* Input temperature is in 0.001 C units. Convert to 1 lsb = 0.125 C. */
+    value /= 125;
+
+    if (value > 2047 || value < -2048) {
+        error_setg(errp, "value %" PRId64 " lsb is out of range", value);
+        return;
+    }
+
+    s->temperature = (int16_t)value;
+}
+
+/*
+ * Callback handler whenever a 'I2C_START_RECV' (read) event is received.
+ */
+static void lsm303dlhc_mag_read(LSM303DLHCMagState *s)
+{
+    /*
+     * Set the LOCK bit whenever a new read attempt is made. This will be
+     * cleared in I2C_FINISH. Note that DRDY is always set to 1 in this driver.
+     */
+    s->sr = 0x3;
+
+    /*
+     * Copy the current X/Y/Z and temp. values into the locked registers so
+     * that 'mag-x', 'mag-y', 'mag-z' and 'temperature' can continue to be
+     * updated via QOM, etc., without corrupting the current read event.
+     */
+    s->x_lock = s->x;
+    s->z_lock = s->z;
+    s->y_lock = s->y;
+    s->temperature_lock = s->temperature;
+}
+
+/*
+ * Callback handler whenever a 'I2C_FINISH' event is received.
+ */
+static void lsm303dlhc_mag_finish(LSM303DLHCMagState *s)
+{
+    /*
+     * Clear the LOCK bit when the read attempt terminates.
+     * This bit is initially set in the I2C_START_RECV handler.
+     */
+    s->sr = 0x1;
+}
+
+/*
+ * Callback handler when a device attempts to write to a register.
+ */
+static void lsm303dlhc_mag_write(LSM303DLHCMagState *s)
+{
+    switch (s->pointer) {
+    case LSM303DLHC_MAG_REG_CRA:
+        s->cra = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_CRB:
+        /* Make sure gain is at least 1, falling back to 1 on an error. */
+        if (s->buf >> 5 == 0) {
+            s->buf = 1 << 5;
+        }
+        s->crb = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_MR:
+        s->mr = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_SR:
+        s->sr = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_IRA:
+        s->ira = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_IRB:
+        s->irb = s->buf;
+        break;
+    case LSM303DLHC_MAG_REG_IRC:
+        s->irc = s->buf;
+        break;
+    default:
+        qemu_log_mask(LOG_GUEST_ERROR, "reg is read-only: 0x%02X", s->buf);
+        break;
+    }
+}
+
+/*
+ * Low-level master-to-slave transaction handler.
+ */
+static int lsm303dlhc_mag_send(I2CSlave *i2c, uint8_t data)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(i2c);
+
+    if (s->len == 0) {
+        /* First byte is the reg pointer */
+        s->pointer = data;
+        s->len++;
+    } else if (s->len == 1) {
+        /* Second byte is the new register value. */
+        s->buf = data;
+        lsm303dlhc_mag_write(s);
+    } else {
+        g_assert_not_reached();
+    }
+
+    return 0;
+}
+
+/*
+ * Low-level slave-to-master transaction handler (read attempts).
+ */
+static uint8_t lsm303dlhc_mag_recv(I2CSlave *i2c)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(i2c);
+    uint8_t resp;
+
+    switch (s->pointer) {
+    case LSM303DLHC_MAG_REG_CRA:
+        resp = s->cra;
+        break;
+    case LSM303DLHC_MAG_REG_CRB:
+        resp = s->crb;
+        break;
+    case LSM303DLHC_MAG_REG_MR:
+        resp = s->mr;
+        break;
+    case LSM303DLHC_MAG_REG_OUT_X_H:
+        resp = (uint8_t)(s->x_lock >> 8);
+        break;
+    case LSM303DLHC_MAG_REG_OUT_X_L:
+        resp = (uint8_t)(s->x_lock);
+        break;
+    case LSM303DLHC_MAG_REG_OUT_Z_H:
+        resp = (uint8_t)(s->z_lock >> 8);
+        break;
+    case LSM303DLHC_MAG_REG_OUT_Z_L:
+        resp = (uint8_t)(s->z_lock);
+        break;
+    case LSM303DLHC_MAG_REG_OUT_Y_H:
+        resp = (uint8_t)(s->y_lock >> 8);
+        break;
+    case LSM303DLHC_MAG_REG_OUT_Y_L:
+        resp = (uint8_t)(s->y_lock);
+        break;
+    case LSM303DLHC_MAG_REG_SR:
+        resp = s->sr;
+        break;
+    case LSM303DLHC_MAG_REG_IRA:
+        resp = s->ira;
+        break;
+    case LSM303DLHC_MAG_REG_IRB:
+        resp = s->irb;
+        break;
+    case LSM303DLHC_MAG_REG_IRC:
+        resp = s->irc;
+        break;
+    case LSM303DLHC_MAG_REG_TEMP_OUT_H:
+        /* Check if the temperature sensor is enabled or not (CRA & 0x80). */
+        if (s->cra & 0x80) {
+            resp = (uint8_t)(s->temperature_lock >> 8);
+        } else {
+            resp = 0;
+        }
+        break;
+    case LSM303DLHC_MAG_REG_TEMP_OUT_L:
+        if (s->cra & 0x80) {
+            resp = (uint8_t)(s->temperature_lock & 0xff);
+        } else {
+            resp = 0;
+        }
+        break;
+    default:
+        resp = 0;
+        break;
+    }
+
+    /*
+     * The address pointer on the LSM303DLHC auto-increments whenever a byte
+     * is read, without the master device having to request the next address.
+     *
+     * The auto-increment process has the following logic:
+     *
+     *   - if (s->pointer == 8) then s->pointer = 3
+     *   - else: if (s->pointer == 12) then s->pointer = 0
+     *   - else: s->pointer += 1
+     *
+     * Reading an invalid address return 0.
+     */
+    if (s->pointer == LSM303DLHC_MAG_REG_OUT_Y_L) {
+        s->pointer = LSM303DLHC_MAG_REG_OUT_X_H;
+    } else if (s->pointer == LSM303DLHC_MAG_REG_IRC) {
+        s->pointer = LSM303DLHC_MAG_REG_CRA;
+    } else {
+        s->pointer++;
+    }
+
+    return resp;
+}
+
+/*
+ * Bus state change handler.
+ */
+static int lsm303dlhc_mag_event(I2CSlave *i2c, enum i2c_event event)
+{
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(i2c);
+
+    switch (event) {
+    case I2C_START_SEND:
+        break;
+    case I2C_START_RECV:
+        lsm303dlhc_mag_read(s);
+        break;
+    case I2C_FINISH:
+        lsm303dlhc_mag_finish(s);
+        break;
+    case I2C_NACK:
+        break;
+    }
+
+    s->len = 0;
+    return 0;
+}
+
+/*
+ * Device data description using VMSTATE macros.
+ */
+static const VMStateDescription vmstate_lsm303dlhc_mag = {
+    .name = "LSM303DLHC_MAG",
+    .version_id = 0,
+    .minimum_version_id = 0,
+    .fields = (VMStateField[]) {
+
+        VMSTATE_I2C_SLAVE(parent_obj, LSM303DLHCMagState),
+        VMSTATE_UINT8(len, LSM303DLHCMagState),
+        VMSTATE_UINT8(buf, LSM303DLHCMagState),
+        VMSTATE_UINT8(pointer, LSM303DLHCMagState),
+        VMSTATE_UINT8(cra, LSM303DLHCMagState),
+        VMSTATE_UINT8(crb, LSM303DLHCMagState),
+        VMSTATE_UINT8(mr, LSM303DLHCMagState),
+        VMSTATE_INT16(x, LSM303DLHCMagState),
+        VMSTATE_INT16(z, LSM303DLHCMagState),
+        VMSTATE_INT16(y, LSM303DLHCMagState),
+        VMSTATE_INT16(x_lock, LSM303DLHCMagState),
+        VMSTATE_INT16(z_lock, LSM303DLHCMagState),
+        VMSTATE_INT16(y_lock, LSM303DLHCMagState),
+        VMSTATE_UINT8(sr, LSM303DLHCMagState),
+        VMSTATE_UINT8(ira, LSM303DLHCMagState),
+        VMSTATE_UINT8(irb, LSM303DLHCMagState),
+        VMSTATE_UINT8(irc, LSM303DLHCMagState),
+        VMSTATE_INT16(temperature, LSM303DLHCMagState),
+        VMSTATE_INT16(temperature_lock, LSM303DLHCMagState),
+        VMSTATE_END_OF_LIST()
+    }
+};
+
+/*
+ * Put the device into post-reset default state.
+ */
+static void lsm303dlhc_mag_default_cfg(LSM303DLHCMagState *s)
+{
+    /* Set the device into is default reset state. */
+    s->len = 0;
+    s->pointer = 0;         /* Current register. */
+    s->buf = 0;             /* Shared buffer. */
+    s->cra = 0x10;          /* Temp Enabled = 0, Data Rate = 15.0 Hz. */
+    s->crb = 0x20;          /* Gain = +/- 1.3 Gauss. */
+    s->mr = 0x3;            /* Operating Mode = Sleep. */
+    s->x = 0;
+    s->z = 0;
+    s->y = 0;
+    s->x_lock = 0;
+    s->z_lock = 0;
+    s->y_lock = 0;
+    s->sr = 0x1;            /* DRDY = 1. */
+    s->ira = 0x48;
+    s->irb = 0x34;
+    s->irc = 0x33;
+    s->temperature = 0;     /* Default to 0 degrees C (0/8 lsb = 0 C). */
+    s->temperature_lock = 0;
+}
+
+/*
+ * Callback handler when DeviceState 'reset' is set to true.
+ */
+static void lsm303dlhc_mag_reset(DeviceState *dev)
+{
+    I2CSlave *i2c = I2C_SLAVE(dev);
+    LSM303DLHCMagState *s = LSM303DLHC_MAG(i2c);
+
+    /* Set the device into its default reset state. */
+    lsm303dlhc_mag_default_cfg(s);
+}
+
+/*
+ * Initialisation of any public properties.
+ */
+static void lsm303dlhc_mag_initfn(Object *obj)
+{
+    object_property_add(obj, "mag-x", "int",
+                lsm303dlhc_mag_get_x,
+                lsm303dlhc_mag_set_x, NULL, NULL);
+
+    object_property_add(obj, "mag-y", "int",
+                lsm303dlhc_mag_get_y,
+                lsm303dlhc_mag_set_y, NULL, NULL);
+
+    object_property_add(obj, "mag-z", "int",
+                lsm303dlhc_mag_get_z,
+                lsm303dlhc_mag_set_z, NULL, NULL);
+
+    object_property_add(obj, "temperature", "int",
+                lsm303dlhc_mag_get_temperature,
+                lsm303dlhc_mag_set_temperature, NULL, NULL);
+}
+
+/*
+ * Set the virtual method pointers (bus state change, tx/rx, etc.).
+ */
+static void lsm303dlhc_mag_class_init(ObjectClass *klass, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(klass);
+    I2CSlaveClass *k = I2C_SLAVE_CLASS(klass);
+
+    dc->reset = lsm303dlhc_mag_reset;
+    dc->vmsd = &vmstate_lsm303dlhc_mag;
+    k->event = lsm303dlhc_mag_event;
+    k->recv = lsm303dlhc_mag_recv;
+    k->send = lsm303dlhc_mag_send;
+}
+
+static const TypeInfo lsm303dlhc_mag_info = {
+    .name = TYPE_LSM303DLHC_MAG,
+    .parent = TYPE_I2C_SLAVE,
+    .instance_size = sizeof(LSM303DLHCMagState),
+    .instance_init = lsm303dlhc_mag_initfn,
+    .class_init = lsm303dlhc_mag_class_init,
+};
+
+static void lsm303dlhc_mag_register_types(void)
+{
+    type_register_static(&lsm303dlhc_mag_info);
+}
+
+type_init(lsm303dlhc_mag_register_types)
diff --git a/hw/sensor/meson.build b/hw/sensor/meson.build
index 059c4ca935..d1bba290da 100644
--- a/hw/sensor/meson.build
+++ b/hw/sensor/meson.build
@@ -4,3 +4,4 @@ softmmu_ss.add(when: 'CONFIG_DPS310', if_true: files('dps310.c'))
 softmmu_ss.add(when: 'CONFIG_EMC141X', if_true: files('emc141x.c'))
 softmmu_ss.add(when: 'CONFIG_ADM1272', if_true: files('adm1272.c'))
 softmmu_ss.add(when: 'CONFIG_MAX34451', if_true: files('max34451.c'))
+softmmu_ss.add(when: 'CONFIG_LSM303DLHC_MAG', if_true: files('lsm303dlhc_mag.c'))
diff --git a/hw/timer/armv7m_systick.c b/hw/timer/armv7m_systick.c
index 3bd951dd04..5dfe39afe3 100644
--- a/hw/timer/armv7m_systick.c
+++ b/hw/timer/armv7m_systick.c
@@ -149,6 +149,10 @@ static MemTxResult systick_write(void *opaque, hwaddr addr,
         s->control &= 0xfffffff8;
         s->control |= value & 7;
 
+        if ((oldval ^ value) & SYSTICK_CLKSOURCE) {
+            systick_set_period_from_clock(s);
+        }
+
         if ((oldval ^ value) & SYSTICK_ENABLE) {
             if (value & SYSTICK_ENABLE) {
                 ptimer_run(s->ptimer, 0);
@@ -156,10 +160,6 @@ static MemTxResult systick_write(void *opaque, hwaddr addr,
                 ptimer_stop(s->ptimer);
             }
         }
-
-        if ((oldval ^ value) & SYSTICK_CLKSOURCE) {
-            systick_set_period_from_clock(s);
-        }
         ptimer_transaction_commit(s->ptimer);
         break;
     }
diff --git a/include/hw/arm/boot.h b/include/hw/arm/boot.h
index ce2b48b88b..c7ebae156e 100644
--- a/include/hw/arm/boot.h
+++ b/include/hw/arm/boot.h
@@ -56,7 +56,6 @@ struct arm_boot_info {
     hwaddr smp_loader_start;
     hwaddr smp_bootreg_addr;
     hwaddr gic_cpu_if_addr;
-    int nb_cpus;
     int board_id;
     /* ARM machines that support the ARM Security Extensions use this field to
      * control whether Linux is booted as secure(true) or non-secure(false).
@@ -70,6 +69,9 @@ struct arm_boot_info {
      * boot loader/boot ROM code, and secondary_cpu_reset_hook() should
      * perform any necessary CPU reset handling and set the PC for the
      * secondary CPUs to point at this boot blob.
+     *
+     * These hooks won't be called if secondary CPUs are booting via
+     * emulated PSCI (see psci_conduit below).
      */
     void (*write_secondary_boot)(ARMCPU *cpu,
                                  const struct arm_boot_info *info);
@@ -86,6 +88,16 @@ struct arm_boot_info {
      * the user it should implement this hook.
      */
     void (*modify_dtb)(const struct arm_boot_info *info, void *fdt);
+    /*
+     * If a board wants to use the QEMU emulated-firmware PSCI support,
+     * it should set this to QEMU_PSCI_CONDUIT_HVC or QEMU_PSCI_CONDUIT_SMC
+     * as appropriate. arm_load_kernel() will set the psci-conduit and
+     * start-powered-off properties on the CPUs accordingly.
+     * Note that if the guest image is started at the same exception level
+     * as the conduit specifies calls should go to (eg guest firmware booted
+     * to EL3) then PSCI will not be enabled.
+     */
+    int psci_conduit;
     /* Used internally by arm_boot.c */
     int is_linux;
     hwaddr initrd_start;
diff --git a/include/hw/arm/xlnx-versal.h b/include/hw/arm/xlnx-versal.h
index 1b5ad4de80..0728316ec7 100644
--- a/include/hw/arm/xlnx-versal.h
+++ b/include/hw/arm/xlnx-versal.h
@@ -103,7 +103,6 @@ struct Versal {
 
     struct {
         MemoryRegion *mr_ddr;
-        uint32_t psci_conduit;
     } cfg;
 };
 
diff --git a/include/hw/arm/xlnx-zynqmp.h b/include/hw/arm/xlnx-zynqmp.h
index 062e637fe4..9424f81c37 100644
--- a/include/hw/arm/xlnx-zynqmp.h
+++ b/include/hw/arm/xlnx-zynqmp.h
@@ -38,6 +38,7 @@
 #include "hw/dma/xlnx_csu_dma.h"
 #include "hw/nvram/xlnx-bbram.h"
 #include "hw/nvram/xlnx-zynqmp-efuse.h"
+#include "hw/or-irq.h"
 
 #define TYPE_XLNX_ZYNQMP "xlnx-zynqmp"
 OBJECT_DECLARE_SIMPLE_TYPE(XlnxZynqMPState, XLNX_ZYNQMP)
@@ -122,6 +123,7 @@ struct XlnxZynqMPState {
     XlnxZDMA gdma[XLNX_ZYNQMP_NUM_GDMA_CH];
     XlnxZDMA adma[XLNX_ZYNQMP_NUM_ADMA_CH];
     XlnxCSUDMA qspi_dma;
+    qemu_or_irq qspi_irq_orgate;
 
     char *boot_cpu;
     ARMCPU *boot_cpu_ptr;
diff --git a/include/hw/intc/arm_gicv3_its_common.h b/include/hw/intc/arm_gicv3_its_common.h
index 3e2ad2dff6..0f130494dd 100644
--- a/include/hw/intc/arm_gicv3_its_common.h
+++ b/include/hw/intc/arm_gicv3_its_common.h
@@ -42,7 +42,6 @@
 #define GITS_TRANSLATER  0x0040
 
 typedef struct {
-    bool valid;
     bool indirect;
     uint16_t entry_sz;
     uint32_t page_sz;
@@ -51,7 +50,6 @@ typedef struct {
 } TableDesc;
 
 typedef struct {
-    bool valid;
     uint32_t num_entries;
     uint64_t base_addr;
 } CmdQDesc;
diff --git a/target/arm/cpu.c b/target/arm/cpu.c
index cdbc4cdd01..5a9c02a256 100644
--- a/target/arm/cpu.c
+++ b/target/arm/cpu.c
@@ -1317,6 +1317,11 @@ void arm_cpu_post_init(Object *obj)
                                        OBJ_PROP_FLAG_READWRITE);
     }
 
+    /* Not DEFINE_PROP_UINT32: we want this to be settable after realize */
+    object_property_add_uint32_ptr(obj, "psci-conduit",
+                                   &cpu->psci_conduit,
+                                   OBJ_PROP_FLAG_READWRITE);
+
     qdev_property_add_static(DEVICE(obj), &arm_cpu_cfgend_property);
 
     if (arm_feature(&cpu->env, ARM_FEATURE_GENERIC_TIMER)) {
@@ -1987,7 +1992,6 @@ static ObjectClass *arm_cpu_class_by_name(const char *cpu_model)
 }
 
 static Property arm_cpu_properties[] = {
-    DEFINE_PROP_UINT32("psci-conduit", ARMCPU, psci_conduit, 0),
     DEFINE_PROP_UINT64("midr", ARMCPU, midr, 0),
     DEFINE_PROP_UINT64("mp-affinity", ARMCPU,
                         mp_affinity, ARM64_AFFINITY_INVALID),
diff --git a/target/arm/helper-a64.c b/target/arm/helper-a64.c
index d6a6fd73d9..7cf953b1e6 100644
--- a/target/arm/helper-a64.c
+++ b/target/arm/helper-a64.c
@@ -83,12 +83,14 @@ void HELPER(msr_i_daifset)(CPUARMState *env, uint32_t imm)
 {
     daif_check(env, 0x1e, imm, GETPC());
     env->daif |= (imm << 6) & PSTATE_DAIF;
+    arm_rebuild_hflags(env);
 }
 
 void HELPER(msr_i_daifclear)(CPUARMState *env, uint32_t imm)
 {
     daif_check(env, 0x1f, imm, GETPC());
     env->daif &= ~((imm << 6) & PSTATE_DAIF);
+    arm_rebuild_hflags(env);
 }
 
 /* Convert a softfloat float_relation_ (as returned by
diff --git a/target/arm/helper.c b/target/arm/helper.c
index 6dd241fbef..b5f80988c9 100644
--- a/target/arm/helper.c
+++ b/target/arm/helper.c
@@ -6154,43 +6154,67 @@ int sve_exception_el(CPUARMState *env, int el)
     uint64_t hcr_el2 = arm_hcr_el2_eff(env);
 
     if (el <= 1 && (hcr_el2 & (HCR_E2H | HCR_TGE)) != (HCR_E2H | HCR_TGE)) {
-        bool disabled = false;
-
-        /* The CPACR.ZEN controls traps to EL1:
-         * 0, 2 : trap EL0 and EL1 accesses
-         * 1    : trap only EL0 accesses
-         * 3    : trap no accesses
-         */
-        if (!extract32(env->cp15.cpacr_el1, 16, 1)) {
-            disabled = true;
-        } else if (!extract32(env->cp15.cpacr_el1, 17, 1)) {
-            disabled = el == 0;
-        }
-        if (disabled) {
+        /* Check CPACR.ZEN.  */
+        switch (extract32(env->cp15.cpacr_el1, 16, 2)) {
+        case 1:
+            if (el != 0) {
+                break;
+            }
+            /* fall through */
+        case 0:
+        case 2:
             /* route_to_el2 */
             return hcr_el2 & HCR_TGE ? 2 : 1;
         }
 
         /* Check CPACR.FPEN.  */
-        if (!extract32(env->cp15.cpacr_el1, 20, 1)) {
-            disabled = true;
-        } else if (!extract32(env->cp15.cpacr_el1, 21, 1)) {
-            disabled = el == 0;
-        }
-        if (disabled) {
+        switch (extract32(env->cp15.cpacr_el1, 20, 2)) {
+        case 1:
+            if (el != 0) {
+                break;
+            }
+            /* fall through */
+        case 0:
+        case 2:
             return 0;
         }
     }
 
-    /* CPTR_EL2.  Since TZ and TFP are positive,
-     * they will be zero when EL2 is not present.
+    /*
+     * CPTR_EL2 changes format with HCR_EL2.E2H (regardless of TGE).
      */
-    if (el <= 2 && arm_is_el2_enabled(env)) {
-        if (env->cp15.cptr_el[2] & CPTR_TZ) {
-            return 2;
-        }
-        if (env->cp15.cptr_el[2] & CPTR_TFP) {
-            return 0;
+    if (el <= 2) {
+        if (hcr_el2 & HCR_E2H) {
+            /* Check CPTR_EL2.ZEN.  */
+            switch (extract32(env->cp15.cptr_el[2], 16, 2)) {
+            case 1:
+                if (el != 0 || !(hcr_el2 & HCR_TGE)) {
+                    break;
+                }
+                /* fall through */
+            case 0:
+            case 2:
+                return 2;
+            }
+
+            /* Check CPTR_EL2.FPEN.  */
+            switch (extract32(env->cp15.cptr_el[2], 20, 2)) {
+            case 1:
+                if (el == 2 || !(hcr_el2 & HCR_TGE)) {
+                    break;
+                }
+                /* fall through */
+            case 0:
+            case 2:
+                return 0;
+            }
+        } else if (arm_is_el2_enabled(env)) {
+            if (env->cp15.cptr_el[2] & CPTR_TZ) {
+                return 2;
+            }
+            if (env->cp15.cptr_el[2] & CPTR_TFP) {
+                return 0;
+            }
         }
     }
 
@@ -6225,7 +6249,8 @@ uint32_t sve_zcr_len_for_el(CPUARMState *env, int el)
     ARMCPU *cpu = env_archcpu(env);
     uint32_t zcr_len = cpu->sve_max_vq - 1;
 
-    if (el <= 1) {
+    if (el <= 1 &&
+        (arm_hcr_el2_eff(env) & (HCR_E2H | HCR_TGE)) != (HCR_E2H | HCR_TGE)) {
         zcr_len = MIN(zcr_len, 0xf & (uint32_t)env->vfp.zcr_el[1]);
     }
     if (el <= 2 && arm_feature(env, ARM_FEATURE_EL2)) {
@@ -12913,6 +12938,8 @@ uint32_t HELPER(crc32c)(uint32_t acc, uint32_t val, uint32_t bytes)
 int fp_exception_el(CPUARMState *env, int cur_el)
 {
 #ifndef CONFIG_USER_ONLY
+    uint64_t hcr_el2;
+
     /* CPACR and the CPTR registers don't exist before v6, so FP is
      * always accessible
      */
@@ -12936,13 +12963,15 @@ int fp_exception_el(CPUARMState *env, int cur_el)
         return 0;
     }
 
+    hcr_el2 = arm_hcr_el2_eff(env);
+
     /* The CPACR controls traps to EL1, or PL1 if we're 32 bit:
      * 0, 2 : trap EL0 and EL1/PL1 accesses
      * 1    : trap only EL0 accesses
      * 3    : trap no accesses
      * This register is ignored if E2H+TGE are both set.
      */
-    if ((arm_hcr_el2_eff(env) & (HCR_E2H | HCR_TGE)) != (HCR_E2H | HCR_TGE)) {
+    if ((hcr_el2 & (HCR_E2H | HCR_TGE)) != (HCR_E2H | HCR_TGE)) {
         int fpen = extract32(env->cp15.cpacr_el1, 20, 2);
 
         switch (fpen) {
@@ -12983,19 +13012,32 @@ int fp_exception_el(CPUARMState *env, int cur_el)
         }
     }
 
-    /* For the CPTR registers we don't need to guard with an ARM_FEATURE
-     * check because zero bits in the registers mean "don't trap".
+    /*
+     * CPTR_EL2 is present in v7VE or v8, and changes format
+     * with HCR_EL2.E2H (regardless of TGE).
      */
-
-    /* CPTR_EL2 : present in v7VE or v8 */
-    if (cur_el <= 2 && extract32(env->cp15.cptr_el[2], 10, 1)
-        && arm_is_el2_enabled(env)) {
-        /* Trap FP ops at EL2, NS-EL1 or NS-EL0 to EL2 */
-        return 2;
+    if (cur_el <= 2) {
+        if (hcr_el2 & HCR_E2H) {
+            /* Check CPTR_EL2.FPEN.  */
+            switch (extract32(env->cp15.cptr_el[2], 20, 2)) {
+            case 1:
+                if (cur_el != 0 || !(hcr_el2 & HCR_TGE)) {
+                    break;
+                }
+                /* fall through */
+            case 0:
+            case 2:
+                return 2;
+            }
+        } else if (arm_is_el2_enabled(env)) {
+            if (env->cp15.cptr_el[2] & CPTR_TFP) {
+                return 2;
+            }
+        }
     }
 
     /* CPTR_EL3 : present in v8 */
-    if (extract32(env->cp15.cptr_el[3], 10, 1)) {
+    if (env->cp15.cptr_el[3] & CPTR_TFP) {
         /* Trap all FP ops to EL3 */
         return 3;
     }
diff --git a/target/arm/psci.c b/target/arm/psci.c
index 6709e28013..b279c0b9a4 100644
--- a/target/arm/psci.c
+++ b/target/arm/psci.c
@@ -27,15 +27,13 @@
 
 bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
 {
-    /* Return true if the r0/x0 value indicates a PSCI call and
-     * the exception type matches the configured PSCI conduit. This is
-     * called before the SMC/HVC instruction is executed, to decide whether
-     * we should treat it as a PSCI call or with the architecturally
+    /*
+     * Return true if the exception type matches the configured PSCI conduit.
+     * This is called before the SMC/HVC instruction is executed, to decide
+     * whether we should treat it as a PSCI call or with the architecturally
      * defined behaviour for an SMC or HVC (which might be UNDEF or trap
      * to EL2 or to EL3).
      */
-    CPUARMState *env = &cpu->env;
-    uint64_t param = is_a64(env) ? env->xregs[0] : env->regs[0];
 
     switch (excp_type) {
     case EXCP_HVC:
@@ -52,27 +50,7 @@ bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
         return false;
     }
 
-    switch (param) {
-    case QEMU_PSCI_0_2_FN_PSCI_VERSION:
-    case QEMU_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
-    case QEMU_PSCI_0_2_FN_AFFINITY_INFO:
-    case QEMU_PSCI_0_2_FN64_AFFINITY_INFO:
-    case QEMU_PSCI_0_2_FN_SYSTEM_RESET:
-    case QEMU_PSCI_0_2_FN_SYSTEM_OFF:
-    case QEMU_PSCI_0_1_FN_CPU_ON:
-    case QEMU_PSCI_0_2_FN_CPU_ON:
-    case QEMU_PSCI_0_2_FN64_CPU_ON:
-    case QEMU_PSCI_0_1_FN_CPU_OFF:
-    case QEMU_PSCI_0_2_FN_CPU_OFF:
-    case QEMU_PSCI_0_1_FN_CPU_SUSPEND:
-    case QEMU_PSCI_0_2_FN_CPU_SUSPEND:
-    case QEMU_PSCI_0_2_FN64_CPU_SUSPEND:
-    case QEMU_PSCI_0_1_FN_MIGRATE:
-    case QEMU_PSCI_0_2_FN_MIGRATE:
-        return true;
-    default:
-        return false;
-    }
+    return true;
 }
 
 void arm_handle_psci_call(ARMCPU *cpu)
@@ -194,10 +172,9 @@ void arm_handle_psci_call(ARMCPU *cpu)
         break;
     case QEMU_PSCI_0_1_FN_MIGRATE:
     case QEMU_PSCI_0_2_FN_MIGRATE:
+    default:
         ret = QEMU_PSCI_RET_NOT_SUPPORTED;
         break;
-    default:
-        g_assert_not_reached();
     }
 
 err:
diff --git a/tests/qtest/lsm303dlhc-mag-test.c b/tests/qtest/lsm303dlhc-mag-test.c
new file mode 100644
index 0000000000..0f64e7fc67
--- /dev/null
+++ b/tests/qtest/lsm303dlhc-mag-test.c
@@ -0,0 +1,148 @@
+/*
+ * QTest testcase for the LSM303DLHC I2C magnetometer
+ *
+ * Copyright (C) 2021 Linaro Ltd.
+ * Written by Kevin Townsend <kevin.townsend@linaro.org>
+ *
+ * Based on: https://www.st.com/resource/en/datasheet/lsm303dlhc.pdf
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include "qemu/osdep.h"
+#include "libqtest-single.h"
+#include "libqos/qgraph.h"
+#include "libqos/i2c.h"
+#include "qapi/qmp/qdict.h"
+
+#define LSM303DLHC_MAG_TEST_ID        "lsm303dlhc_mag-test"
+#define LSM303DLHC_MAG_REG_CRA        0x00
+#define LSM303DLHC_MAG_REG_CRB        0x01
+#define LSM303DLHC_MAG_REG_OUT_X_H    0x03
+#define LSM303DLHC_MAG_REG_OUT_Z_H    0x05
+#define LSM303DLHC_MAG_REG_OUT_Y_H    0x07
+#define LSM303DLHC_MAG_REG_IRC        0x0C
+#define LSM303DLHC_MAG_REG_TEMP_OUT_H 0x31
+
+static int qmp_lsm303dlhc_mag_get_property(const char *id, const char *prop)
+{
+    QDict *response;
+    int ret;
+
+    response = qmp("{ 'execute': 'qom-get', 'arguments': { 'path': %s, "
+                   "'property': %s } }", id, prop);
+    g_assert(qdict_haskey(response, "return"));
+    ret = qdict_get_int(response, "return");
+    qobject_unref(response);
+    return ret;
+}
+
+static void qmp_lsm303dlhc_mag_set_property(const char *id, const char *prop,
+                                            int value)
+{
+    QDict *response;
+
+    response = qmp("{ 'execute': 'qom-set', 'arguments': { 'path': %s, "
+                   "'property': %s, 'value': %d } }", id, prop, value);
+    g_assert(qdict_haskey(response, "return"));
+    qobject_unref(response);
+}
+
+static void send_and_receive(void *obj, void *data, QGuestAllocator *alloc)
+{
+    int64_t value;
+    QI2CDevice *i2cdev = (QI2CDevice *)obj;
+
+    /* Check default value for CRB */
+    g_assert_cmphex(i2c_get8(i2cdev, LSM303DLHC_MAG_REG_CRB), ==, 0x20);
+
+    /* Set x to 1.0 gauss and verify the value */
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID, "mag-x", 100000);
+    value = qmp_lsm303dlhc_mag_get_property(
+        LSM303DLHC_MAG_TEST_ID, "mag-x");
+    g_assert_cmpint(value, ==, 100000);
+
+    /* Set y to 1.5 gauss and verify the value */
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID, "mag-y", 150000);
+    value = qmp_lsm303dlhc_mag_get_property(
+        LSM303DLHC_MAG_TEST_ID, "mag-y");
+    g_assert_cmpint(value, ==, 150000);
+
+    /* Set z to 0.5 gauss and verify the value */
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID, "mag-z", 50000);
+    value = qmp_lsm303dlhc_mag_get_property(
+        LSM303DLHC_MAG_TEST_ID, "mag-z");
+    g_assert_cmpint(value, ==, 50000);
+
+    /* Set temperature to 23.6 C and verify the value */
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID,
+        "temperature", 23600);
+    value = qmp_lsm303dlhc_mag_get_property(
+        LSM303DLHC_MAG_TEST_ID, "temperature");
+    /* Should return 23.5 C due to 0.125°C steps. */
+    g_assert_cmpint(value, ==, 23500);
+
+    /* Read raw x axis registers (1 gauss = 1100 at +/-1.3 g gain) */
+    value = i2c_get16(i2cdev, LSM303DLHC_MAG_REG_OUT_X_H);
+    g_assert_cmphex(value, ==, 1100);
+
+    /* Read raw y axis registers (1.5 gauss = 1650 at +/- 1.3 g gain = ) */
+    value = i2c_get16(i2cdev, LSM303DLHC_MAG_REG_OUT_Y_H);
+    g_assert_cmphex(value, ==, 1650);
+
+    /* Read raw z axis registers (0.5 gauss = 490 at +/- 1.3 g gain = ) */
+    value = i2c_get16(i2cdev, LSM303DLHC_MAG_REG_OUT_Z_H);
+    g_assert_cmphex(value, ==, 490);
+
+    /* Read raw temperature registers with temp disabled (CRA = 0x10) */
+    value = i2c_get16(i2cdev, LSM303DLHC_MAG_REG_TEMP_OUT_H);
+    g_assert_cmphex(value, ==, 0);
+
+    /* Enable temperature reads (CRA = 0x90) */
+    i2c_set8(i2cdev, LSM303DLHC_MAG_REG_CRA, 0x90);
+
+    /* Read raw temp registers (23.5 C = 188 at 1 lsb = 0.125 C) */
+    value = i2c_get16(i2cdev, LSM303DLHC_MAG_REG_TEMP_OUT_H);
+    g_assert_cmphex(value, ==, 188);
+}
+
+static void reg_wraparound(void *obj, void *data, QGuestAllocator *alloc)
+{
+    uint8_t value[4];
+    QI2CDevice *i2cdev = (QI2CDevice *)obj;
+
+    /* Set x to 1.0 gauss, and y to 1.5 gauss for known test values */
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID, "mag-x", 100000);
+    qmp_lsm303dlhc_mag_set_property(LSM303DLHC_MAG_TEST_ID, "mag-y", 150000);
+
+    /* Check that requesting 4 bytes starting at Y_H wraps around to X_L */
+    i2c_read_block(i2cdev, LSM303DLHC_MAG_REG_OUT_Y_H, value, 4);
+    /* 1.5 gauss = 1650 lsb = 0x672 */
+    g_assert_cmphex(value[0], ==, 0x06);
+    g_assert_cmphex(value[1], ==, 0x72);
+    /* 1.0 gauss = 1100 lsb = 0x44C */
+    g_assert_cmphex(value[2], ==, 0x04);
+    g_assert_cmphex(value[3], ==, 0x4C);
+
+    /* Check that requesting LSM303DLHC_MAG_REG_IRC wraps around to CRA */
+    i2c_read_block(i2cdev, LSM303DLHC_MAG_REG_IRC, value, 2);
+    /* Default value for IRC = 0x33 */
+    g_assert_cmphex(value[0], ==, 0x33);
+    /* Default value for CRA = 0x10 */
+    g_assert_cmphex(value[1], ==, 0x10);
+}
+
+static void lsm303dlhc_mag_register_nodes(void)
+{
+    QOSGraphEdgeOptions opts = {
+        .extra_device_opts = "id=" LSM303DLHC_MAG_TEST_ID ",address=0x1e"
+    };
+    add_qi2c_address(&opts, &(QI2CAddress) { 0x1E });
+
+    qos_node_create_driver("lsm303dlhc_mag", i2c_device_create);
+    qos_node_consumes("lsm303dlhc_mag", "i2c-bus", &opts);
+
+    qos_add_test("tx-rx", "lsm303dlhc_mag", send_and_receive, NULL);
+    qos_add_test("regwrap", "lsm303dlhc_mag", reg_wraparound, NULL);
+}
+libqos_init(lsm303dlhc_mag_register_nodes);
diff --git a/tests/qtest/meson.build b/tests/qtest/meson.build
index 762f6adcd5..f33d84d19b 100644
--- a/tests/qtest/meson.build
+++ b/tests/qtest/meson.build
@@ -243,6 +243,7 @@ qos_test_ss.add(
   'eepro100-test.c',
   'es1370-test.c',
   'ipoctal232-test.c',
+  'lsm303dlhc-mag-test.c',
   'max34451-test.c',
   'megasas-test.c',
   'ne2000-test.c',