hv: Restore CYC_OVRD WFI mode and handle FIQ disable

Put the WFI mode back how macOS expects it on boot, since it won't set
it itself. Then, also trap attempts by macOS to disable FIQs. This
happens in the sleep/core off path. It is not sufficient to make all
that work properly, but it's a start.

Signed-off-by: Hector Martin <[email protected]>

Author: marcan

src/hv.c

@@ -80,6 +80,9 @@ void hv_init(void)
     // Compute tick interval
     hv_tick_interval = mrs(CNTFRQ_EL0) / HV_TICK_RATE;
 
+    // Set deep WFI back to defaults
+    reg_mask(SYS_IMP_APL_CYC_OVRD, CYC_OVRD_WFI_MODE_MASK, CYC_OVRD_WFI_MODE(0));
+
     sysop("dsb ishst");
     sysop("tlbi alle1is");
     sysop("dsb ish");
@@ -169,6 +172,8 @@ static void hv_init_secondary(struct hv_secondary_info_t *info)
     msr(SYS_IMP_APL_SPRR_CONFIG_EL1, info->sprr_config);
     msr(SYS_IMP_APL_GXF_CONFIG_EL1, info->gxf_config);
 
+    reg_mask(SYS_IMP_APL_CYC_OVRD, CYC_OVRD_WFI_MODE_MASK, CYC_OVRD_WFI_MODE(0));
+
     if (gxf_enabled())
         gl2_call(hv_set_gxf_vbar, 0, 0, 0, 0);
 

src/hv_exc.c

@@ -270,9 +270,11 @@ static bool hv_handle_msr(struct exc_info *ctx, u64 iss)
             if (is_read) {
                 regs[rt] = mrs(SYS_IMP_APL_CYC_OVRD);
             } else {
-                msr(SYS_IMP_APL_CYC_OVRD, regs[rt] & ~CYC_OVRD_DISABLE_WFI_RET);
-                if (regs[rt] & CYC_OVRD_DISABLE_WFI_RET)
-                    printf("msr(SYS_IMP_APL_CYC_OVRD, 0x%08lx): Filtered WFI RET disable\n",
+                msr(SYS_IMP_APL_CYC_OVRD,
+                    regs[rt] & ~(CYC_OVRD_DISABLE_WFI_RET | CYC_OVRD_FIQ_MODE_MASK));
+                if (regs[rt] & (CYC_OVRD_DISABLE_WFI_RET | CYC_OVRD_FIQ_MODE_MASK))
+                    printf("msr(SYS_IMP_APL_CYC_OVRD, 0x%08lx): Filtered WFI RET disable and/or "
+                           "FIQ mode\n",
                            regs[rt]);
             }
             return true;