Set thumb bit in cpsr when block linking

Author: Grarak

src/core/graphics/gpu_renderer.rs

@@ -150,7 +150,7 @@ impl GpuRenderer {
             gl::BindFramebuffer(gl::FRAMEBUFFER, 0);
             gl::Viewport(0, 0, PRESENTER_SCREEN_WIDTH as _, PRESENTER_SCREEN_HEIGHT as _);
 
-            let fps = fps.load(Ordering::Relaxed);
+            let fps = fps.load(Ordering::Relaxed) as u32;
             let per = fps * 100 / 60;
 
             let last_time_saved = *last_save_time.lock().unwrap();

src/core/thread_regs.rs

@@ -365,10 +365,8 @@ impl ThreadRegs {
         self.cpu.check_for_interrupt(cycle_manager);
     }
 
-    pub fn set_thumb(&mut self, enable: bool) {
-        let mut cpsr = Cpsr::from(self.cpsr);
-        cpsr.set_thumb(enable);
-        self.cpsr = u32::from(cpsr);
+    pub fn set_thumb(&mut self, thumb: bool) {
+        self.cpsr = (self.cpsr & !(1 << 5)) | ((thumb as u32) << 5);
     }
 
     pub fn is_thumb(&self) -> bool {

src/jit/assembler/arm/alu_assembler.rs

@@ -563,6 +563,33 @@ impl Bfc {
     }
 }
 
+#[bitsize(32)]
+#[derive(FromBits)]
+pub struct Bfi {
+    rn: u4,
+    id: u3,
+    lsb: u5,
+    rd: u4,
+    msb: u5,
+    id2: u7,
+    cond: u4,
+}
+
+impl Bfi {
+    #[inline]
+    pub fn create(rd: Reg, rn: Reg, lsb: u8, width: u8, cond: Cond) -> u32 {
+        u32::from(Bfi::new(
+            u4::new(rn as u8),
+            u3::new(0b001),
+            u5::new(lsb),
+            u4::new(rd as u8),
+            u5::new(lsb + width - 1),
+            u7::new(0b0111110),
+            u4::new(cond as u8),
+        ))
+    }
+}
+
 // TODO Add const asserts once const features has been added back to rust
 // https://github.com/rust-lang/rust/issues/110395
 //const_assert_eq!(lookup_opcode(Self::add(0, 0, 0, 0)).0 as u8, And as u8);

src/jit/assembler/block_asm.rs

@@ -318,6 +318,14 @@ impl<'a> BlockAsm<'a> {
         self.insert_inst(BlockInst::Bfc { operand: operand.into(), lsb, width })
     }
 
+    pub fn bfi(&mut self, op0: impl Into<BlockReg>, op1: impl Into<BlockReg>, lsb: u8, width: u8) {
+        self.insert_inst(BlockInst::Bfi {
+            operands: [op0.into(), op1.into()],
+            lsb,
+            width,
+        })
+    }
+
     pub fn muls_guest_thumb_pc_aligned(&mut self, op0: impl Into<BlockReg>, op1: impl Into<BlockReg>, op2: impl Into<BlockOperandShift>) {
         self.insert_inst(BlockInst::Mul {
             operands: [op0.into().into(), op1.into().into(), op2.into()],

src/jit/assembler/block_inst.rs

@@ -1,4 +1,4 @@
-use crate::jit::assembler::arm::alu_assembler::{AluImm, AluReg, AluShiftImm, Bfc, MulReg};
+use crate::jit::assembler::arm::alu_assembler::{AluImm, AluReg, AluShiftImm, Bfc, Bfi, MulReg};
 use crate::jit::assembler::arm::branch_assembler::Bx;
 use crate::jit::assembler::arm::transfer_assembler::{LdmStm, LdrStrImm, LdrStrImmSBHD, LdrStrReg, LdrStrRegSBHD, Mrs, Msr};
 use crate::jit::assembler::arm::Bkpt;
@@ -124,6 +124,7 @@ impl BlockInst {
                 BlockSystemRegOp::Msr => (block_reg_set!(operand.try_as_reg()), block_reg_set!()),
             },
             BlockInst::Bfc { operand, .. } => (block_reg_set!(Some(*operand)), block_reg_set!(Some(*operand))),
+            BlockInst::Bfi { operands, .. } => (block_reg_set!(Some(operands[0]), Some(operands[1])), block_reg_set!(Some(operands[0]))),
 
             BlockInst::SaveContext { .. } => (block_reg_set!(), block_reg_set!()),
             BlockInst::SaveReg {
@@ -250,6 +251,10 @@ impl BlockInst {
                 }
             }
             BlockInst::Bfc { operand, .. } => Self::replace_reg(operand, old, new),
+            BlockInst::Bfi { operands, .. } => {
+                Self::replace_reg(&mut operands[0], old, new);
+                Self::replace_reg(&mut operands[1], old, new);
+            }
             BlockInst::SaveContext { .. } => {
                 unreachable!()
             }
@@ -296,6 +301,7 @@ impl BlockInst {
                 }
             }
             BlockInst::Bfc { operand, .. } => Self::replace_reg(operand, old, new),
+            BlockInst::Bfi { operands, .. } => Self::replace_reg(&mut operands[0], old, new),
             BlockInst::SaveContext { tmp_guest_cpsr_reg, .. } => Self::replace_reg(tmp_guest_cpsr_reg, old, new),
             BlockInst::SaveReg {
                 guest_reg,
@@ -338,6 +344,10 @@ impl BlockInst {
             BlockInst::TransferMultiple { operand, .. } => Self::replace_reg(operand, old, new),
             BlockInst::SystemReg { operand, .. } => Self::replace_operand(operand, old, new),
             BlockInst::Bfc { operand, .. } => Self::replace_reg(operand, old, new),
+            BlockInst::Bfi { operands, .. } => {
+                Self::replace_reg(&mut operands[0], old, new);
+                Self::replace_reg(&mut operands[1], old, new);
+            }
 
             BlockInst::SaveContext { .. } => {
                 unreachable!()
@@ -518,6 +528,7 @@ impl BlockInst {
                 BlockSystemRegOp::Msr => opcodes.push(Msr::cpsr_flags(operand.as_reg().as_fixed(), Cond::AL)),
             },
             BlockInst::Bfc { operand, lsb, width } => opcodes.push(Bfc::create(operand.as_fixed(), *lsb, *width, Cond::AL)),
+            BlockInst::Bfi { operands, lsb, width } => opcodes.push(Bfi::create(operands[0].as_fixed(), operands[1].as_fixed(), *lsb, *width, Cond::AL)),
             BlockInst::Mul { operands, set_cond, .. } => match operands[2].operand {
                 BlockOperand::Reg(reg) => opcodes.push(MulReg::mul(
                     operands[0].as_reg().as_fixed(),
@@ -686,6 +697,11 @@ pub enum BlockInst {
         lsb: u8,
         width: u8,
     },
+    Bfi {
+        operands: [BlockReg; 2],
+        lsb: u8,
+        width: u8,
+    },
     Mul {
         operands: [BlockOperandShift; 3],
         set_cond: BlockAluSetCond,
@@ -789,6 +805,7 @@ impl Debug for BlockInst {
             }
             BlockInst::SystemReg { op, operand } => write!(f, "{op:?} {operand:?}"),
             BlockInst::Bfc { operand, lsb, width } => write!(f, "Bfc {operand:?}, {lsb}, {width}"),
+            BlockInst::Bfi { operands, lsb, width } => write!(f, "Bfi {:?}, {:?}, {lsb}, {width}", operands[0], operands[1]),
             BlockInst::Mul { operands, set_cond, thumb_pc_aligned } => write!(f, "Mul{set_cond:?} {operands:?}, align pc: {thumb_pc_aligned}"),
             BlockInst::Label { label, guest_pc } => {
                 let guest_pc = match guest_pc {

src/jit/emitter/emit_branch.rs

@@ -184,17 +184,27 @@ impl<'a, const CPU: CpuType> JitAsm<'a, CPU> {
                     asm.emit_return_stack_write(block_asm, runtime_data_addr_reg);
                 }
 
+                let cpsr_reg = block_asm.new_reg();
+                let addr_mask_reg = block_asm.new_reg();
+
+                // Set thumb bit
+                block_asm.transfer_read(cpsr_reg, block_asm.thread_regs_addr_reg, Reg::CPSR as u32 * 4, false, MemoryAmount::Word);
+                block_asm.bfi(cpsr_reg, target_pc_reg, 5, 1);
+                block_asm.transfer_write(cpsr_reg, block_asm.thread_regs_addr_reg, Reg::CPSR as u32 * 4, false, MemoryAmount::Word);
+
+                block_asm.free_reg(cpsr_reg);
+
                 let target_addr_reg = block_asm.new_reg();
-                let pc_mask_reg = block_asm.new_reg();
 
                 // Align pc to !1 or !3
-                block_asm.mvn(pc_mask_reg, 1);
-                block_asm.tst(target_pc_reg, 1);
-                block_asm.start_cond_block(Cond::EQ);
-                block_asm.mvn(pc_mask_reg, 3);
-                block_asm.end_cond_block();
+                // let thumb = (target_pc & 1) == 1;
+                // let addr_mask = !(1 | ((!thumb as u32) << 1));
+                // let target_addr = target_pc & addr_mask;
+                block_asm.mvn(addr_mask_reg, 3);
+                block_asm.orr(addr_mask_reg, addr_mask_reg, (target_pc_reg.into(), ShiftType::Lsl, BlockOperand::from(1)));
+                block_asm.and(target_addr_reg, target_pc_reg, addr_mask_reg);
 
-                block_asm.and(target_addr_reg, target_pc_reg, pc_mask_reg);
+                block_asm.free_reg(addr_mask_reg);
 
                 let map_ptr = get_jit!(asm.emu).jit_memory_map.get_map_ptr::<CPU>();
 
@@ -228,7 +238,6 @@ impl<'a, const CPU: CpuType> JitAsm<'a, CPU> {
                 block_asm.free_reg(map_entry_base_ptr_reg);
                 block_asm.free_reg(map_index_reg);
                 block_asm.free_reg(map_ptr_reg);
-                block_asm.free_reg(pc_mask_reg);
                 block_asm.free_reg(target_addr_reg);
             },
             |asm, block_asm| {

src/jit/emitter/thumb/emit_branch_thumb.rs

@@ -36,14 +36,11 @@ impl<'a, const CPU: CpuType> JitAsm<'a, CPU> {
         block_asm.end_cond_block();
     }
 
-    pub fn emit_bl_setup_thumb(&mut self) {
-        let next_inst_info = &self.jit_buf.insts[self.jit_buf.current_index + 1];
-        assert!(next_inst_info.op == Op::BlOffT || next_inst_info.op == Op::BlxOffT)
-    }
-
     pub fn emit_bl_thumb(&mut self, block_asm: &mut BlockAsm) {
         let previous_inst_info = &self.jit_buf.insts[self.jit_buf.current_index - 1];
-        assert_eq!(previous_inst_info.op, Op::BlSetupT);
+        if previous_inst_info.op != Op::BlSetupT {
+            return;
+        }
 
         let relative_pc = *previous_inst_info.operands()[0].as_imm().unwrap() as i32 + 4;
         let mut target_pc = (self.jit_buf.current_pc as i32 - 2 + relative_pc) as u32;

src/jit/emitter/thumb/emit_thumb.rs

@@ -49,7 +49,7 @@ impl<'a, const CPU: CpuType> JitAsm<'a, CPU> {
             Op::BT | Op::BeqT | Op::BneT | Op::BcsT | Op::BccT | Op::BmiT | Op::BplT | Op::BvsT | Op::BvcT | Op::BhiT | Op::BlsT | Op::BgeT | Op::BltT | Op::BgtT | Op::BleT => {
                 self.emit_b_thumb(block_asm)
             }
-            Op::BlSetupT => self.emit_bl_setup_thumb(),
+            Op::BlSetupT => {}
             Op::BlOffT | Op::BlxOffT => self.emit_bl_thumb(block_asm),
             Op::BxRegT => self.emit_bx_thumb(block_asm),
             Op::BlxRegT => self.emit_blx_thumb(block_asm),

src/jit/jit_asm.rs

@@ -238,7 +238,6 @@ fn emit_code_block_internal<const CPU: CpuType, const THUMB: bool>(store_host_sp
     jit_entry(store_host_sp);
 }
 
-#[inline]
 fn execute_internal<const CPU: CpuType>(guest_pc: u32) -> u16 {
     let asm = unsafe { get_jit_asm_ptr::<CPU>().as_mut().unwrap_unchecked() };
 
@@ -248,7 +247,8 @@ fn execute_internal<const CPU: CpuType>(guest_pc: u32) -> u16 {
     let jit_entry = {
         get_regs_mut!(asm.emu, CPU).set_thumb(thumb);
 
-        let guest_pc = if thumb { guest_pc & !1 } else { guest_pc & !3 };
+        let guest_pc_mask = !(1 | ((!thumb as u32) << 1));
+        let guest_pc = guest_pc & guest_pc_mask;
         let jit_entry = get_jit!(asm.emu).get_jit_start_addr::<CPU>(guest_pc);
         let jit_entry: extern "C" fn(bool) = unsafe { mem::transmute(jit_entry) };
 
@@ -313,7 +313,6 @@ impl<'a, const CPU: CpuType> JitAsm<'a, CPU> {
         }
     }
 
-    #[inline(always)]
     pub fn execute(&mut self) -> u16 {
         let entry = get_regs!(self.emu, CPU).pc;
         execute_internal::<CPU>(entry)

src/jit/jit_memory.rs

@@ -8,7 +8,6 @@ use crate::utils::{HeapMem, HeapMemU32};
 use crate::{utils, DEBUG_LOG};
 use lazy_static::lazy_static;
 use paste::paste;
-use std::hint::assert_unchecked;
 use std::intrinsics::unlikely;
 use std::marker::ConstParamTy;
 use std::{ptr, slice};
@@ -225,11 +224,7 @@ impl JitMemory {
     }
 
     pub fn get_jit_start_addr<const CPU: CpuType>(&self, guest_pc: u32) -> *const extern "C" fn(bool) {
-        let ptr = self.jit_memory_map.get_jit_entry::<CPU>(guest_pc);
-        unsafe {
-            assert_unchecked(!ptr.is_null());
-            (*ptr).0
-        }
+        unsafe { (*self.jit_memory_map.get_jit_entry::<CPU>(guest_pc)).0 }
     }
 
     pub fn invalidate_block<const REGION: JitRegion>(&mut self, guest_addr: u32, size: usize) {