target/arm: Implement M-profile lazy FP state preservation

The M-profile architecture floating point system supports
lazy FP state preservation, where FP registers are not
pushed to the stack when an exception occurs but are instead
only saved if and when the first FP instruction in the exception
handler is executed. Implement this in QEMU, corresponding
to the check of LSPACT in the pseudocode ExecuteFPCheck().

Backports commit e33cf0f8d8c9998a7616684f9d6aa0d181b88803 from qemu

qemu/aarch64.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_aarch64
 #define helper_v7m_blxns helper_v7m_blxns_aarch64
 #define helper_v7m_bxns helper_v7m_bxns_aarch64
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_aarch64
 #define helper_v7m_mrs helper_v7m_mrs_aarch64
 #define helper_v7m_msr helper_v7m_msr_aarch64
 #define helper_v7m_tt helper_v7m_tt_aarch64

qemu/aarch64eb.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_aarch64eb
 #define helper_v7m_blxns helper_v7m_blxns_aarch64eb
 #define helper_v7m_bxns helper_v7m_bxns_aarch64eb
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_aarch64eb
 #define helper_v7m_mrs helper_v7m_mrs_aarch64eb
 #define helper_v7m_msr helper_v7m_msr_aarch64eb
 #define helper_v7m_tt helper_v7m_tt_aarch64eb

qemu/arm.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_arm
 #define helper_v7m_blxns helper_v7m_blxns_arm
 #define helper_v7m_bxns helper_v7m_bxns_arm
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_arm
 #define helper_v7m_mrs helper_v7m_mrs_arm
 #define helper_v7m_msr helper_v7m_msr_arm
 #define helper_v7m_tt helper_v7m_tt_arm

qemu/armeb.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_armeb
 #define helper_v7m_blxns helper_v7m_blxns_armeb
 #define helper_v7m_bxns helper_v7m_bxns_armeb
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_armeb
 #define helper_v7m_mrs helper_v7m_mrs_armeb
 #define helper_v7m_msr helper_v7m_msr_armeb
 #define helper_v7m_tt helper_v7m_tt_armeb

qemu/header_gen.py

@@ -1751,6 +1751,7 @@ symbols = (
     'helper_uxtb16',
     'helper_v7m_blxns',
     'helper_v7m_bxns',
+    'helper_v7m_preserve_fp_state',
     'helper_v7m_mrs',
     'helper_v7m_msr',
     'helper_v7m_tt',

qemu/m68k.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_m68k
 #define helper_v7m_blxns helper_v7m_blxns_m68k
 #define helper_v7m_bxns helper_v7m_bxns_m68k
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_m68k
 #define helper_v7m_mrs helper_v7m_mrs_m68k
 #define helper_v7m_msr helper_v7m_msr_m68k
 #define helper_v7m_tt helper_v7m_tt_m68k

qemu/mips.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_mips
 #define helper_v7m_blxns helper_v7m_blxns_mips
 #define helper_v7m_bxns helper_v7m_bxns_mips
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_mips
 #define helper_v7m_mrs helper_v7m_mrs_mips
 #define helper_v7m_msr helper_v7m_msr_mips
 #define helper_v7m_tt helper_v7m_tt_mips

qemu/mips64.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_mips64
 #define helper_v7m_blxns helper_v7m_blxns_mips64
 #define helper_v7m_bxns helper_v7m_bxns_mips64
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_mips64
 #define helper_v7m_mrs helper_v7m_mrs_mips64
 #define helper_v7m_msr helper_v7m_msr_mips64
 #define helper_v7m_tt helper_v7m_tt_mips64

qemu/mips64el.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_mips64el
 #define helper_v7m_blxns helper_v7m_blxns_mips64el
 #define helper_v7m_bxns helper_v7m_bxns_mips64el
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_mips64el
 #define helper_v7m_mrs helper_v7m_mrs_mips64el
 #define helper_v7m_msr helper_v7m_msr_mips64el
 #define helper_v7m_tt helper_v7m_tt_mips64el

qemu/mipsel.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_mipsel
 #define helper_v7m_blxns helper_v7m_blxns_mipsel
 #define helper_v7m_bxns helper_v7m_bxns_mipsel
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_mipsel
 #define helper_v7m_mrs helper_v7m_mrs_mipsel
 #define helper_v7m_msr helper_v7m_msr_mipsel
 #define helper_v7m_tt helper_v7m_tt_mipsel

qemu/powerpc.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_powerpc
 #define helper_v7m_blxns helper_v7m_blxns_powerpc
 #define helper_v7m_bxns helper_v7m_bxns_powerpc
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_powerpc
 #define helper_v7m_mrs helper_v7m_mrs_powerpc
 #define helper_v7m_msr helper_v7m_msr_powerpc
 #define helper_v7m_tt helper_v7m_tt_powerpc

qemu/riscv32.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_riscv32
 #define helper_v7m_blxns helper_v7m_blxns_riscv32
 #define helper_v7m_bxns helper_v7m_bxns_riscv32
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_riscv32
 #define helper_v7m_mrs helper_v7m_mrs_riscv32
 #define helper_v7m_msr helper_v7m_msr_riscv32
 #define helper_v7m_tt helper_v7m_tt_riscv32

qemu/riscv64.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_riscv64
 #define helper_v7m_blxns helper_v7m_blxns_riscv64
 #define helper_v7m_bxns helper_v7m_bxns_riscv64
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_riscv64
 #define helper_v7m_mrs helper_v7m_mrs_riscv64
 #define helper_v7m_msr helper_v7m_msr_riscv64
 #define helper_v7m_tt helper_v7m_tt_riscv64

qemu/sparc.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_sparc
 #define helper_v7m_blxns helper_v7m_blxns_sparc
 #define helper_v7m_bxns helper_v7m_bxns_sparc
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_sparc
 #define helper_v7m_mrs helper_v7m_mrs_sparc
 #define helper_v7m_msr helper_v7m_msr_sparc
 #define helper_v7m_tt helper_v7m_tt_sparc

qemu/sparc64.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_sparc64
 #define helper_v7m_blxns helper_v7m_blxns_sparc64
 #define helper_v7m_bxns helper_v7m_bxns_sparc64
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_sparc64
 #define helper_v7m_mrs helper_v7m_mrs_sparc64
 #define helper_v7m_msr helper_v7m_msr_sparc64
 #define helper_v7m_tt helper_v7m_tt_sparc64

qemu/target/arm/cpu.h

@@ -59,6 +59,7 @@
 #define EXCP_NOCP           17   /* v7M NOCP UsageFault */
 #define EXCP_INVSTATE       18   /* v7M INVSTATE UsageFault */
 #define EXCP_STKOF          19   /* v8M STKOF UsageFault */
+#define EXCP_LAZYFP         20   /* v7M fault during lazy FP stacking */
 /* NB: add new EXCP_ defines to the array in arm_log_exception() too */
 
 #define ARMV7M_EXCP_RESET   1
@@ -3132,6 +3133,8 @@ FIELD(TBFLAG_A32, NS, 6, 1)
 FIELD(TBFLAG_A32, VFPEN, 7, 1)
 FIELD(TBFLAG_A32, CONDEXEC, 8, 8)
 FIELD(TBFLAG_A32, SCTLR_B, 16, 1)
+/* For M profile only, set if FPCCR.LSPACT is set */
+FIELD(TBFLAG_A32, LSPACT, 18, 1)
 /* For M profile only, set if we must create a new FP context */
 FIELD(TBFLAG_A32, NEW_FP_CTXT_NEEDED, 19, 1)
 /* For M profile only, set if FPCCR.S does not match current security state */

qemu/target/arm/helper.c

@@ -7180,6 +7180,12 @@ void HELPER(v7m_blxns)(CPUARMState *env, uint32_t dest)
     g_assert_not_reached();
 }
 
+void HELPER(v7m_preserve_fp_state)(CPUARMState *env)
+{
+    /* translate.c should never generate calls here in user-only mode */
+    g_assert_not_reached();
+}
+
 uint32_t HELPER(v7m_tt)(CPUARMState *env, uint32_t addr, uint32_t op)
 {
     /* The TT instructions can be used by unprivileged code, but in
@@ -7541,6 +7547,99 @@ pend_fault:
     return false;
 }
 
+void HELPER(v7m_preserve_fp_state)(CPUARMState *env)
+{
+    /*
+     * Preserve FP state (because LSPACT was set and we are about
+     * to execute an FP instruction). This corresponds to the
+     * PreserveFPState() pseudocode.
+     * We may throw an exception if the stacking fails.
+     */
+    ARMCPU *cpu = arm_env_get_cpu(env);
+    bool is_secure = env->v7m.fpccr[M_REG_S] & R_V7M_FPCCR_S_MASK;
+    bool negpri = !(env->v7m.fpccr[M_REG_S] & R_V7M_FPCCR_HFRDY_MASK);
+    bool is_priv = !(env->v7m.fpccr[is_secure] & R_V7M_FPCCR_USER_MASK);
+    bool splimviol = env->v7m.fpccr[is_secure] & R_V7M_FPCCR_SPLIMVIOL_MASK;
+    uint32_t fpcar = env->v7m.fpcar[is_secure];
+    bool stacked_ok = true;
+    bool ts = is_secure && (env->v7m.fpccr[M_REG_S] & R_V7M_FPCCR_TS_MASK);
+    bool take_exception;
+
+    /* Take the iothread lock as we are going to touch the NVIC */
+    // Unicorn: commented out
+    //qemu_mutex_lock_iothread();
+
+    /* Check the background context had access to the FPU */
+    if (!v7m_cpacr_pass(env, is_secure, is_priv)) {
+        armv7m_nvic_set_pending_lazyfp(env->nvic, ARMV7M_EXCP_USAGE, is_secure);
+        env->v7m.cfsr[is_secure] |= R_V7M_CFSR_NOCP_MASK;
+        stacked_ok = false;
+    } else if (!is_secure && !extract32(env->v7m.nsacr, 10, 1)) {
+        armv7m_nvic_set_pending_lazyfp(env->nvic, ARMV7M_EXCP_USAGE, M_REG_S);
+        env->v7m.cfsr[M_REG_S] |= R_V7M_CFSR_NOCP_MASK;
+        stacked_ok = false;
+    }
+
+    if (!splimviol && stacked_ok) {
+        /* We only stack if the stack limit wasn't violated */
+        int i;
+        ARMMMUIdx mmu_idx;
+
+        mmu_idx = arm_v7m_mmu_idx_all(env, is_secure, is_priv, negpri);
+        for (i = 0; i < (ts ? 32 : 16); i += 2) {
+            uint64_t dn = *aa32_vfp_dreg(env, i / 2);
+            uint32_t faddr = fpcar + 4 * i;
+            uint32_t slo = extract64(dn, 0, 32);
+            uint32_t shi = extract64(dn, 32, 32);
+
+            if (i >= 16) {
+                faddr += 8; /* skip the slot for the FPSCR */
+            }
+            stacked_ok = stacked_ok &&
+                v7m_stack_write(cpu, faddr, slo, mmu_idx, STACK_LAZYFP) &&
+                v7m_stack_write(cpu, faddr + 4, shi, mmu_idx, STACK_LAZYFP);
+        }
+
+        stacked_ok = stacked_ok &&
+            v7m_stack_write(cpu, fpcar + 0x40,
+                            vfp_get_fpscr(env), mmu_idx, STACK_LAZYFP);
+    }
+
+    /*
+     * We definitely pended an exception, but it's possible that it
+     * might not be able to be taken now. If its priority permits us
+     * to take it now, then we must not update the LSPACT or FP regs,
+     * but instead jump out to take the exception immediately.
+     * If it's just pending and won't be taken until the current
+     * handler exits, then we do update LSPACT and the FP regs.
+     */
+    take_exception = !stacked_ok &&
+        armv7m_nvic_can_take_pending_exception(env->nvic);
+
+    // Unicorn: commented out
+    //qemu_mutex_unlock_iothread();
+
+    if (take_exception) {
+        raise_exception_ra(env, EXCP_LAZYFP, 0, 1, GETPC());
+    }
+
+    env->v7m.fpccr[is_secure] &= ~R_V7M_FPCCR_LSPACT_MASK;
+
+    if (ts) {
+        /* Clear s0 to s31 and the FPSCR */
+        int i;
+
+        for (i = 0; i < 32; i += 2) {
+            *aa32_vfp_dreg(env, i / 2) = 0;
+        }
+        vfp_set_fpscr(env, 0);
+    }
+    /*
+     * Otherwise s0 to s15 and FPSCR are UNKNOWN; we choose to leave them
+     * unchanged.
+     */
+}
+
 /* Write to v7M CONTROL.SPSEL bit for the specified security bank.
  * This may change the current stack pointer between Main and Process
  * stack pointers if it is done for the CONTROL register for the current
@@ -8878,6 +8977,7 @@ static void arm_log_exception(int idx)
             [EXCP_NOCP] = "v7M NOCP UsageFault",
             [EXCP_INVSTATE] = "v7M INVSTATE UsageFault",
             [EXCP_STKOF] = "v8M STKOF UsageFault",
+            [EXCP_LAZYFP] = "v7M exception during lazy FP stacking",
         };
 
         if (idx >= 0 && idx < ARRAY_SIZE(excnames)) {
@@ -9180,6 +9280,12 @@ void arm_v7m_cpu_do_interrupt(CPUState *cs)
             return;
         }
         break;
+    case EXCP_LAZYFP:
+        /*
+         * We already pended the specific exception in the NVIC in the
+         * v7m_preserve_fp_state() helper function.
+         */
+        break;
     default:
         cpu_abort(cs, "Unhandled exception 0x%x\n", cs->exception_index);
         return; /* Never happens.  Keep compiler happy.  */
@@ -13276,6 +13382,14 @@ void cpu_get_tb_cpu_state(CPUARMState *env, target_ulong *pc,
         flags = FIELD_DP32(flags, TBFLAG_A32, NEW_FP_CTXT_NEEDED, 1);
     }
 
+    if (arm_feature(env, ARM_FEATURE_M)) {
+        bool is_secure = env->v7m.fpccr[M_REG_S] & R_V7M_FPCCR_S_MASK;
+
+        if (env->v7m.fpccr[is_secure] & R_V7M_FPCCR_LSPACT_MASK) {
+            flags = FIELD_DP32(flags, TBFLAG_A32, LSPACT, 1);
+        }
+    }
+
     *pflags = flags;
     *cs_base = 0;
 }

qemu/target/arm/helper.h

@@ -71,6 +71,8 @@ DEF_HELPER_2(v7m_blxns, void, env, i32)
 
 DEF_HELPER_3(v7m_tt, i32, env, i32, i32)
 
+DEF_HELPER_1(v7m_preserve_fp_state, void, env)
+
 DEF_HELPER_2(v8m_stackcheck, void, env, i32)
 
 DEF_HELPER_4(access_check_cp_reg, void, env, ptr, i32, i32)

qemu/target/arm/translate.c

@@ -3531,6 +3531,29 @@ static int disas_vfp_insn(DisasContext *s, uint32_t insn)
     if (arm_dc_feature(s, ARM_FEATURE_M)) {
         /* Handle M-profile lazy FP state mechanics */
 
+        /* Trigger lazy-state preservation if necessary */
+        if (s->v7m_lspact) {
+            /*
+             * Lazy state saving affects external memory and also the NVIC,
+             * so we must mark it as an IO operation for icount.
+             */
+            if (tb_cflags(s->base.tb) & CF_USE_ICOUNT) {
+                // Unicorn: commented out
+                //gen_io_start();
+            }
+            gen_helper_v7m_preserve_fp_state(tcg_ctx, tcg_ctx->cpu_env);
+            if (tb_cflags(s->base.tb) & CF_USE_ICOUNT) {
+                // Unicorn: commented out
+                //gen_io_end();
+            }
+            /*
+             * If the preserve_fp_state helper doesn't throw an exception
+             * then it will clear LSPACT; we don't need to repeat this for
+             * any further FP insns in this TB.
+             */
+            s->v7m_lspact = false;
+        }
+
         /* Update ownership of FP context: set FPCCR.S to match current state */
         if (s->v8m_fpccr_s_wrong) {
             TCGv_i32 tmp;
@@ -13567,6 +13590,9 @@ static void arm_tr_init_disas_context(DisasContextBase *dcbase, CPUState *cs)
         regime_is_secure(env, dc->mmu_idx);
     dc->v8m_stackcheck = FIELD_EX32(tb_flags, TBFLAG_A32, STACKCHECK);
     dc->v8m_fpccr_s_wrong = FIELD_EX32(tb_flags, TBFLAG_A32, FPCCR_S_WRONG);
+    dc->v7m_new_fp_ctxt_needed =
+        FIELD_EX32(tb_flags, TBFLAG_A32, NEW_FP_CTXT_NEEDED);
+    dc->v7m_lspact = FIELD_EX32(tb_flags, TBFLAG_A32, LSPACT);
     dc->cp_regs = cpu->cp_regs;
     dc->features = env->features;
 

qemu/target/arm/translate.h

@@ -41,6 +41,7 @@ typedef struct DisasContext {
     bool v8m_stackcheck; /* true if we need to perform v8M stack limit checks */
     bool v8m_fpccr_s_wrong; /* true if v8M FPCCR.S != v8m_secure */
     bool v7m_new_fp_ctxt_needed; /* ASPEN set but no active FP context */
+    bool v7m_lspact; /* FPCCR.LSPACT set */
     /* Immediate value in AArch32 SVC insn; must be set if is_jmp == DISAS_SWI
      * so that top level loop can generate correct syndrome information.
      */

qemu/x86_64.h

@@ -1745,6 +1745,7 @@
 #define helper_uxtb16 helper_uxtb16_x86_64
 #define helper_v7m_blxns helper_v7m_blxns_x86_64
 #define helper_v7m_bxns helper_v7m_bxns_x86_64
+#define helper_v7m_preserve_fp_state helper_v7m_preserve_fp_state_x86_64
 #define helper_v7m_mrs helper_v7m_mrs_x86_64
 #define helper_v7m_msr helper_v7m_msr_x86_64
 #define helper_v7m_tt helper_v7m_tt_x86_64