use alloc::sync::Arc;
use crate::core::space::{AddressSpace, MemAttrs};
use crate::core::value::{Endian, Width};
use super::cp::{AccessKind, Coprocessor, CpEffect, CpFault, CpOp, CpTransfer, Fault, Mmu};
use super::isa::{
Decoded, DpOp, ExtraOp, Half, HalfMulOp, Index, Insn, Offset, Operand, SatOp, Shift, ShiftType,
};
use super::thumb::{AluOp, HiOp, ImmOp, MemRegOp, MemSize, SmallOperand, Thumb};
use super::{Config, Mode, Regs, psr};
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
pub enum Exception {
Reset,
DataAbort,
Fiq,
Irq,
PrefetchAbort,
Undefined,
Swi,
}
impl Exception {
#[must_use]
pub const fn vector(self) -> u32 {
match self {
Exception::Reset => 0x00,
Exception::Undefined => 0x04,
Exception::Swi => 0x08,
Exception::PrefetchAbort => 0x0c,
Exception::DataAbort => 0x10,
Exception::Irq => 0x18,
Exception::Fiq => 0x1c,
}
}
#[must_use]
pub const fn mode(self) -> Mode {
match self {
Exception::Reset | Exception::Swi => Mode::SUPERVISOR,
Exception::Undefined => Mode::UNDEFINED,
Exception::PrefetchAbort | Exception::DataAbort => Mode::ABORT,
Exception::Irq => Mode::IRQ,
Exception::Fiq => Mode::FIQ,
}
}
#[must_use]
pub const fn masks_fiq(self) -> bool {
matches!(self, Exception::Reset | Exception::Fiq)
}
#[must_use]
pub const fn name(self) -> &'static str {
match self {
Exception::Reset => "reset",
Exception::DataAbort => "data abort",
Exception::Fiq => "fiq",
Exception::Irq => "irq",
Exception::PrefetchAbort => "prefetch abort",
Exception::Undefined => "undefined instruction",
Exception::Swi => "swi",
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct Abort {
kind: AccessKind,
va: u32,
fault: Fault,
}
type Ex<T = ()> = core::result::Result<T, Abort>;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(super) struct State {
pub regs: Regs,
pub cycles: u64,
pub halted: bool,
pub reset_pending: bool,
pub faults: u64,
pub last_fault: u32,
pub last_swi: u32,
pub last_bkpt: u16,
}
impl State {
pub(super) const fn new() -> State {
State {
regs: Regs::new(),
cycles: 0,
halted: false,
reset_pending: true,
faults: 0,
last_fault: 0,
last_swi: 0,
last_bkpt: 0,
}
}
}
pub(super) struct Exec<'a> {
state: &'a mut State,
space: &'a AddressSpace,
mmu: &'a dyn Mmu,
coprocessors: &'a [Option<Arc<dyn Coprocessor>>; 16],
cfg: &'a Config,
attrs: MemAttrs,
insn_addr: u32,
branched: bool,
used: u64,
}
impl<'a> Exec<'a> {
pub(super) fn new(
state: &'a mut State,
space: &'a AddressSpace,
mmu: &'a dyn Mmu,
coprocessors: &'a [Option<Arc<dyn Coprocessor>>; 16],
cfg: &'a Config,
) -> Exec<'a> {
let attrs = MemAttrs::DEFAULT.with_requester(cfg.requester);
Exec {
state,
space,
mmu,
coprocessors,
cfg,
attrs,
insn_addr: 0,
branched: false,
used: 0,
}
}
pub(super) fn step(&mut self, irq: bool, fiq: bool) -> u64 {
if self.state.reset_pending {
self.state.reset_pending = false;
self.state.halted = false;
self.take_exception(Exception::Reset, 0);
return self.used;
}
if self.state.halted {
if !(irq || fiq) {
self.cycle(1);
return self.used;
}
self.state.halted = false;
}
if fiq && !self.flag(psr::F) {
let lr = self.state.regs.r[15].wrapping_add(4);
self.take_exception(Exception::Fiq, lr);
return self.used;
}
if irq && !self.flag(psr::I) {
let lr = self.state.regs.r[15].wrapping_add(4);
self.take_exception(Exception::Irq, lr);
return self.used;
}
if self.flag(psr::T) {
self.step_thumb();
} else {
self.step_arm();
}
self.used
}
fn cycle(&mut self, n: u64) {
self.used += n;
self.state.cycles = self.state.cycles.wrapping_add(n);
}
fn flag(&self, mask: u32) -> bool {
self.state.regs.cpsr & mask != 0
}
fn set_flag(&mut self, mask: u32, on: bool) {
if on {
self.state.regs.cpsr |= mask;
} else {
self.state.regs.cpsr &= !mask;
}
}
fn set_nz(&mut self, value: u32) {
self.set_flag(psr::N, value & 0x8000_0000 != 0);
self.set_flag(psr::Z, value == 0);
}
#[inline]
fn reg(&self, index: u8) -> u32 {
self.state.regs.r[(index & 0xf) as usize]
}
#[inline]
fn reg_plus(&self, index: u8, extra: u32) -> u32 {
let v = self.reg(index);
if index & 0xf == 15 {
v.wrapping_add(extra)
} else {
v
}
}
#[inline]
fn set_reg(&mut self, index: u8, value: u32) {
if index & 0xf == 15 {
self.branch_to(value);
return;
}
self.state.regs.r[(index & 0xf) as usize] = value;
}
fn write_pc_without_flush(&mut self, value: u32) {
let ahead = if self.flag(psr::T) { 4 } else { 8 };
self.state.regs.r[15] = value.wrapping_add(4).wrapping_sub(ahead);
self.branched = true;
}
fn store_value(&self, index: u8) -> u32 {
if index & 0xf == 15 {
let offset = u32::from(self.cfg.store_pc_offset);
let offset = if self.flag(psr::T) {
offset / 2
} else {
offset
};
self.insn_addr.wrapping_add(offset)
} else {
self.reg(index)
}
}
fn branch_to(&mut self, target: u32) {
self.state.regs.r[15] = if self.flag(psr::T) {
target & !1
} else {
target
};
self.branched = true;
self.cycle(2);
}
fn branch_exchange(&mut self, target: u32) {
let thumb = target & 1 != 0;
self.set_flag(psr::T, thumb);
self.state.regs.r[15] = target & !1;
self.branched = true;
self.cycle(2);
}
fn return_from_exception(&mut self, target: u32) {
if let Some(spsr) = self.state.regs.spsr() {
self.state.regs.write_cpsr(spsr);
}
self.state.regs.r[15] = if self.flag(psr::T) {
target & !1
} else {
target
};
self.branched = true;
self.cycle(2);
}
fn vector_base(&self) -> u32 {
if self.cfg.high_vectors || self.mmu.high_vectors() {
0xffff_0000
} else {
0
}
}
fn take_exception(&mut self, kind: Exception, return_address: u32) {
let old_cpsr = self.state.regs.cpsr;
self.state.regs.set_mode(kind.mode());
self.state.regs.set_spsr(old_cpsr);
if kind != Exception::Reset {
self.state.regs.r[14] = return_address;
}
self.state.regs.cpsr |= psr::I;
if kind.masks_fiq() {
self.state.regs.cpsr |= psr::F;
}
self.state.regs.cpsr &= !psr::T;
self.state.regs.r[15] = self.vector_base().wrapping_add(kind.vector());
self.branched = true;
self.cycle(3);
}
fn take_abort(&mut self, abort: Abort) {
self.mmu.report_abort(abort.va, abort.fault, abort.kind);
if abort.kind.is_fetch() {
let lr = self.insn_addr.wrapping_add(4);
self.take_exception(Exception::PrefetchAbort, lr);
} else {
let lr = self.insn_addr.wrapping_add(8);
self.take_exception(Exception::DataAbort, lr);
}
}
fn privileged(&self) -> bool {
self.state.regs.mode().is_privileged()
}
fn translate(&mut self, va: u32, kind: AccessKind, privileged: bool) -> Ex<u32> {
self.mmu
.translate(va, kind, privileged)
.map_err(|fault| Abort { kind, va, fault })
}
fn check_alignment(&self, va: u32, width: Width, kind: AccessKind) -> Ex {
if self.cfg.alignment_faults && !width.is_aligned(u64::from(va)) {
return Err(Abort {
kind,
va,
fault: Fault::ALIGNMENT,
});
}
Ok(())
}
fn to_cpu_order(&self, pa: u32, width: Width, value: u32) -> u32 {
if self.cfg.endian == Endian::Little
|| self.space.endian_at(u64::from(pa)) == self.cfg.endian
{
return value;
}
match width {
Width::U8 => value,
Width::U16 => u32::from((value as u16).swap_bytes()),
_ => value.swap_bytes(),
}
}
fn load(&mut self, va: u32, width: Width, privileged: bool) -> Ex<u32> {
self.check_alignment(va, width, AccessKind::Read)?;
let pa = self.translate(va, AccessKind::Read, privileged)?;
self.cycle(1);
let attrs = self.attrs.with_privileged(privileged);
match self.space.read(u64::from(pa), width, attrs) {
Ok(v) => Ok(self.to_cpu_order(pa, width, v as u32)),
Err(_) => {
self.state.faults = self.state.faults.wrapping_add(1);
self.state.last_fault = va;
Err(Abort {
kind: AccessKind::Read,
va,
fault: Fault::EXTERNAL,
})
}
}
}
fn store(&mut self, va: u32, width: Width, value: u32, privileged: bool) -> Ex {
self.check_alignment(va, width, AccessKind::Write)?;
let pa = self.translate(va, AccessKind::Write, privileged)?;
self.cycle(1);
let attrs = self.attrs.with_privileged(privileged);
let value = self.to_cpu_order(pa, width, value);
match self
.space
.write(u64::from(pa), width, u64::from(value), attrs)
{
Ok(()) => Ok(()),
Err(_) => {
self.state.faults = self.state.faults.wrapping_add(1);
self.state.last_fault = va;
Err(Abort {
kind: AccessKind::Write,
va,
fault: Fault::EXTERNAL,
})
}
}
}
fn load_word_rotated(&mut self, va: u32, privileged: bool) -> Ex<u32> {
if self.cfg.alignment_faults {
self.check_alignment(va, Width::U32, AccessKind::Read)?;
}
let value = self.load(va & !3, Width::U32, privileged)?;
Ok(value.rotate_right((va & 3) * 8))
}
fn load_half_rotated(&mut self, va: u32, privileged: bool) -> Ex<u32> {
if self.cfg.alignment_faults {
self.check_alignment(va, Width::U16, AccessKind::Read)?;
}
let value = self.load(va & !1, Width::U16, privileged)?;
Ok(value.rotate_right((va & 1) * 8))
}
fn load_signed_half(&mut self, va: u32, privileged: bool) -> Ex<u32> {
if self.cfg.alignment_faults {
self.check_alignment(va, Width::U16, AccessKind::Read)?;
}
let value = self.load(va & !1, Width::U16, privileged)?;
Ok(if va & 1 != 0 {
i32::from((value >> 8) as u8 as i8) as u32
} else {
i32::from(value as u16 as i16) as u32
})
}
fn fetch(&mut self, va: u32, width: Width) -> Ex<u32> {
let va = va & !(width.bytes() as u32 - 1);
let pa = self.translate(va, AccessKind::Fetch, self.privileged())?;
self.cycle(1);
match self.space.read(u64::from(pa), width, self.attrs) {
Ok(v) => Ok(self.to_cpu_order(pa, width, v as u32)),
Err(_) => {
self.state.faults = self.state.faults.wrapping_add(1);
self.state.last_fault = va;
Err(Abort {
kind: AccessKind::Fetch,
va,
fault: Fault::EXTERNAL,
})
}
}
}
fn shift_immediate(ty: ShiftType, value: u32, amount: u8, carry_in: bool) -> (u32, bool) {
match (ty, amount) {
(ShiftType::Lsl, 0) => (value, carry_in),
(ShiftType::Lsl, n) => (value << n, value & (1 << (32 - u32::from(n))) != 0),
(ShiftType::Lsr, 0) => (0, value & 0x8000_0000 != 0),
(ShiftType::Lsr, n) => (value >> n, value & (1 << (u32::from(n) - 1)) != 0),
(ShiftType::Asr, 0) => (
if value & 0x8000_0000 != 0 {
u32::MAX
} else {
0
},
value & 0x8000_0000 != 0,
),
(ShiftType::Asr, n) => (
((value as i32) >> n) as u32,
value & (1 << (u32::from(n) - 1)) != 0,
),
(ShiftType::Ror, 0) => ((u32::from(carry_in) << 31) | (value >> 1), value & 1 != 0),
(ShiftType::Ror, n) => (
value.rotate_right(u32::from(n)),
value & (1 << (u32::from(n) - 1)) != 0,
),
}
}
fn shift_register(ty: ShiftType, value: u32, amount: u32, carry_in: bool) -> (u32, bool) {
let amount = amount & 0xff;
if amount == 0 {
return (value, carry_in);
}
match ty {
ShiftType::Lsl => match amount {
1..=31 => (value << amount, value & (1 << (32 - amount)) != 0),
32 => (0, value & 1 != 0),
_ => (0, false),
},
ShiftType::Lsr => match amount {
1..=31 => (value >> amount, value & (1 << (amount - 1)) != 0),
32 => (0, value & 0x8000_0000 != 0),
_ => (0, false),
},
ShiftType::Asr => {
if amount >= 32 {
let sign = value & 0x8000_0000 != 0;
(if sign { u32::MAX } else { 0 }, sign)
} else {
(
((value as i32) >> amount) as u32,
value & (1 << (amount - 1)) != 0,
)
}
}
ShiftType::Ror => {
let low = amount & 31;
if low == 0 {
(value, value & 0x8000_0000 != 0)
} else {
(value.rotate_right(low), value & (1 << (low - 1)) != 0)
}
}
}
}
fn eval_operand(&self, operand: Operand, extra: u32, carry_in: bool) -> (u32, bool) {
match operand {
Operand::Imm { imm8, rotate } => {
let value = u32::from(imm8).rotate_right(u32::from(rotate) * 2);
if rotate == 0 {
(value, carry_in)
} else {
(value, value & 0x8000_0000 != 0)
}
}
Operand::Reg { rm, shift } => {
let value = self.reg_plus(rm, extra);
match shift {
Shift::Imm { ty, amount } => Exec::shift_immediate(ty, value, amount, carry_in),
Shift::Reg { ty, rs } => {
let amount = self.reg(rs);
Exec::shift_register(ty, value, amount, carry_in)
}
}
}
}
}
fn eval_offset(&self, offset: Offset) -> u32 {
match offset {
Offset::Imm(imm) => u32::from(imm),
Offset::Reg { rm, shift } => {
let value = self.reg(rm);
match shift {
Shift::Imm { ty, amount } => {
Exec::shift_immediate(ty, value, amount, self.flag(psr::C)).0
}
Shift::Reg { .. } => value,
}
}
}
}
fn alu(
op: DpOp,
a: u32,
b: u32,
carry_in: bool,
shifter_carry: bool,
v_in: bool,
) -> (u32, bool, bool) {
fn add(a: u32, b: u32, carry: bool) -> (u32, bool, bool) {
let wide = u64::from(a) + u64::from(b) + u64::from(carry);
let result = wide as u32;
let c = wide > u64::from(u32::MAX);
let v = (a ^ result) & (b ^ result) & 0x8000_0000 != 0;
(result, c, v)
}
match op {
DpOp::And | DpOp::Tst => (a & b, shifter_carry, v_in),
DpOp::Eor | DpOp::Teq => (a ^ b, shifter_carry, v_in),
DpOp::Sub | DpOp::Cmp => add(a, !b, true),
DpOp::Rsb => add(!a, b, true),
DpOp::Add | DpOp::Cmn => add(a, b, false),
DpOp::Adc => add(a, b, carry_in),
DpOp::Sbc => add(a, !b, carry_in),
DpOp::Rsc => add(!a, b, carry_in),
DpOp::Orr => (a | b, shifter_carry, v_in),
DpOp::Mov => (b, shifter_carry, v_in),
DpOp::Bic => (a & !b, shifter_carry, v_in),
DpOp::Mvn => (!b, shifter_carry, v_in),
}
}
fn commit_flags(&mut self, result: u32, c: bool, v: bool) {
self.set_nz(result);
self.set_flag(psr::C, c);
self.set_flag(psr::V, v);
}
fn multiply_cycles(rs: u32, signed: bool) -> u64 {
let terminates = |mask: u32, top: u32| {
let high = rs & mask;
high == 0 || (signed && high == mask && rs & top != 0)
};
if terminates(0xffff_ff00, 0x80) {
1
} else if terminates(0xffff_0000, 0x8000) {
2
} else if terminates(0xff00_0000, 0x0080_0000) {
3
} else {
4
}
}
fn step_arm(&mut self) {
let pc = self.state.regs.r[15];
self.insn_addr = pc;
self.branched = false;
self.state.regs.r[15] = pc.wrapping_add(8);
let word = match self.fetch(pc, Width::U32) {
Ok(w) => w,
Err(abort) => {
self.take_abort(abort);
return;
}
};
let decoded = super::isa::decode(word);
let saved = self.state.regs.r;
let outcome = if decoded.passes(self.state.regs.cpsr) {
self.execute_arm(decoded)
} else {
Ok(())
};
if let Err(abort) = outcome {
self.state.regs.r = saved;
self.state.regs.r[15] = pc;
self.take_abort(abort);
return;
}
if !self.branched {
self.state.regs.r[15] = pc.wrapping_add(4);
}
}
#[allow(clippy::too_many_lines)] fn execute_arm(&mut self, decoded: Decoded) -> Ex {
match decoded.insn {
Insn::DataProc {
op,
s,
rd,
rn,
operand,
} => {
let register_shift = operand.is_register_shifted();
if register_shift {
self.cycle(1);
}
let extra = if register_shift { 4 } else { 0 };
let carry_in = self.flag(psr::C);
let (b, shifter_carry) = self.eval_operand(operand, extra, carry_in);
let a = if op.reads_rn() {
self.reg_plus(rn, extra)
} else {
0
};
let (result, c, v) =
Exec::alu(op, a, b, carry_in, shifter_carry, self.flag(psr::V));
if !op.writes_result() {
self.commit_flags(result, c, v);
return Ok(());
}
if rd & 0xf == 15 {
if s && self.state.regs.spsr().is_some() {
self.return_from_exception(result);
} else {
if s {
self.commit_flags(result, c, v);
}
self.branch_to(result);
}
} else {
self.set_reg(rd, result);
if s {
self.commit_flags(result, c, v);
}
}
Ok(())
}
Insn::Mrs { rd, spsr } => {
let value = if spsr {
self.state.regs.spsr().unwrap_or(self.state.regs.cpsr)
} else {
self.state.regs.cpsr
};
if rd & 0xf == 15 {
self.write_pc_without_flush(value);
} else {
self.set_reg(rd, value);
}
Ok(())
}
Insn::Msr {
spsr,
mask,
operand,
} => {
let value = match operand {
Operand::Imm { .. } => operand.immediate().unwrap_or(0),
Operand::Reg { rm, .. } => self.reg(rm),
};
self.write_psr(spsr, mask, value);
Ok(())
}
Insn::Bx { rm } => {
let target = self.reg(rm);
self.branch_exchange(target);
Ok(())
}
Insn::BlxReg { rm } => {
let target = self.reg(rm);
self.state.regs.r[14] = self.insn_addr.wrapping_add(4);
self.branch_exchange(target);
Ok(())
}
Insn::Clz { rd, rm } => {
let value = self.reg(rm);
self.set_reg(rd, value.leading_zeros());
Ok(())
}
Insn::Bkpt { imm } => {
self.state.last_bkpt = imm;
let lr = self.insn_addr.wrapping_add(4);
self.take_exception(Exception::PrefetchAbort, lr);
Ok(())
}
Insn::Branch { link, offset } => {
let target = self.state.regs.r[15].wrapping_add(offset as u32);
if link {
self.state.regs.r[14] = self.insn_addr.wrapping_add(4);
}
self.branch_to(target);
Ok(())
}
Insn::BlxImm { offset } => {
let target = self.state.regs.r[15].wrapping_add(offset as u32);
self.state.regs.r[14] = self.insn_addr.wrapping_add(4);
self.set_flag(psr::T, true);
self.state.regs.r[15] = target & !1;
self.branched = true;
self.cycle(2);
Ok(())
}
Insn::Mul {
accumulate,
s,
rd,
rn,
rm,
rs,
} => {
let a = self.reg_plus(rm, 4);
let b = self.reg_plus(rs, 4);
self.cycle(Exec::multiply_cycles(b, true));
let mut result = a.wrapping_mul(b);
if accumulate {
self.cycle(1);
result = result.wrapping_add(self.reg_plus(rn, 4));
}
if s {
self.set_nz(result);
}
if rd & 0xf == 15 {
self.branch_to(result);
} else {
self.set_reg(rd, result);
}
Ok(())
}
Insn::MulLong {
signed,
accumulate,
s,
rdhi,
rdlo,
rm,
rs,
} => {
let a = self.reg_plus(rm, 4);
let b = self.reg_plus(rs, 4);
self.cycle(Exec::multiply_cycles(b, signed) + 1);
let product = if signed {
(i64::from(a as i32).wrapping_mul(i64::from(b as i32))) as u64
} else {
u64::from(a) * u64::from(b)
};
let result = if accumulate {
self.cycle(1);
let acc = (u64::from(self.reg_plus(rdhi, 4)) << 32)
| u64::from(self.reg_plus(rdlo, 4));
product.wrapping_add(acc)
} else {
product
};
if rdlo & 0xf == 15 {
self.branch_to(result as u32);
} else {
self.set_reg(rdlo, result as u32);
}
if rdhi & 0xf == 15 {
self.branch_to((result >> 32) as u32);
} else {
self.set_reg(rdhi, (result >> 32) as u32);
}
if s {
self.set_flag(psr::N, result & 0x8000_0000_0000_0000 != 0);
self.set_flag(psr::Z, result == 0);
}
Ok(())
}
Insn::Saturating { op, rd, rm, rn } => {
let m = self.reg(rm) as i32;
let n = self.reg(rn) as i32;
let result = match op {
SatOp::QAdd => self.saturating_add(m, n),
SatOp::QSub => self.saturating_sub(m, n),
SatOp::QDAdd => {
let doubled = self.saturating_add(n, n);
self.saturating_add(m, doubled)
}
SatOp::QDSub => {
let doubled = self.saturating_add(n, n);
self.saturating_sub(m, doubled)
}
};
self.set_reg(rd, result as u32);
Ok(())
}
Insn::HalfMul {
op,
rd,
rn,
rm,
rs,
x,
y,
} => {
self.half_multiply(op, rd, rn, rm, rs, x, y);
Ok(())
}
Insn::LoadStore {
load,
byte,
up,
index,
rn,
rd,
offset,
} => self.load_store(load, byte, up, index, rn, rd, offset),
Insn::LoadStoreExtra {
op,
up,
index,
rn,
rd,
offset,
} => self.load_store_extra(op, up, index, rn, rd, offset),
Insn::BlockTransfer {
load,
before,
up,
user,
writeback,
rn,
list,
} => self.block_transfer(load, before, up, user, writeback, rn, list),
Insn::Swap { byte, rd, rn, rm } => {
let addr = self.reg_plus(rn, 4);
let privileged = self.privileged();
let value = if byte {
self.load(addr, Width::U8, privileged)?
} else {
self.load_word_rotated(addr, privileged)?
};
let source = self.reg_plus(rm, 4);
if byte {
self.store(addr, Width::U8, source & 0xff, privileged)?;
} else {
self.store(addr & !3, Width::U32, source, privileged)?;
}
self.cycle(1);
self.set_reg(rd, value);
Ok(())
}
Insn::Swi { imm } => {
self.state.last_swi = imm;
let lr = self.insn_addr.wrapping_add(4);
self.take_exception(Exception::Swi, lr);
Ok(())
}
Insn::Pld { .. } => Ok(()),
Insn::Cdp {
cp,
opc1,
crd,
crn,
crm,
opc2,
..
} => {
let op = CpOp {
cp,
opc1,
crd,
crn,
crm,
opc2,
};
match self.coprocessor(cp).map(|c| c.cdp(op)) {
Some(Ok(effect)) => self.apply_effect(effect),
_ => self.undefined_instruction(),
}
Ok(())
}
Insn::CpReg {
cp,
load,
opc1,
rd,
crn,
crm,
opc2,
..
} => {
self.coprocessor_register(cp, load, opc1, rd, crn, crm, opc2);
Ok(())
}
Insn::CpRegPair {
cp,
load,
opc,
rd,
rn,
crm,
} => {
let Some(coprocessor) = self.coprocessor(cp) else {
self.undefined_instruction();
return Ok(());
};
if load {
match coprocessor.mrrc(cp, opc, crm) {
Ok(value) => {
self.set_reg(rd, value as u32);
self.set_reg(rn, (value >> 32) as u32);
}
Err(CpFault::Undefined) => self.undefined_instruction(),
}
} else {
let value = (u64::from(self.reg(rn)) << 32) | u64::from(self.reg(rd));
match coprocessor.mcrr(cp, opc, crm, value) {
Ok(effect) => self.apply_effect(effect),
Err(CpFault::Undefined) => self.undefined_instruction(),
}
}
Ok(())
}
Insn::CpTransfer {
cp,
load,
long,
crd,
rn,
index,
up,
offset,
..
} => self.coprocessor_transfer(cp, load, long, crd, rn, index, up, offset),
Insn::Undefined => {
self.undefined_instruction();
Ok(())
}
}
}
fn write_psr(&mut self, spsr: bool, mask: u8, value: u32) {
let mut byte_mask = 0u32;
if mask & 0b0001 != 0 {
byte_mask |= 0x0000_00ff;
}
if mask & 0b0010 != 0 {
byte_mask |= 0x0000_ff00;
}
if mask & 0b0100 != 0 {
byte_mask |= 0x00ff_0000;
}
if mask & 0b1000 != 0 {
byte_mask |= 0xff00_0000;
}
if spsr {
if let Some(current) = self.state.regs.spsr() {
self.state
.regs
.set_spsr((current & !byte_mask) | (value & byte_mask));
}
return;
}
if !self.privileged() {
byte_mask &= 0xff00_0000;
}
let new = (self.state.regs.cpsr & !byte_mask) | (value & byte_mask);
self.state.regs.write_cpsr(new);
}
fn saturating_add(&mut self, a: i32, b: i32) -> i32 {
match a.checked_add(b) {
Some(v) => v,
None => {
self.set_flag(psr::Q, true);
if a < 0 { i32::MIN } else { i32::MAX }
}
}
}
fn saturating_sub(&mut self, a: i32, b: i32) -> i32 {
match a.checked_sub(b) {
Some(v) => v,
None => {
self.set_flag(psr::Q, true);
if a < 0 { i32::MIN } else { i32::MAX }
}
}
}
fn half_of(value: u32, half: Half) -> i32 {
match half {
Half::Bottom => i32::from(value as u16 as i16),
Half::Top => i32::from((value >> 16) as u16 as i16),
}
}
#[allow(clippy::too_many_arguments)] fn half_multiply(&mut self, op: HalfMulOp, rd: u8, rn: u8, rm: u8, rs: u8, x: Half, y: Half) {
let m = self.reg(rm);
let s = self.reg(rs);
self.cycle(if op == HalfMulOp::Smlal { 2 } else { 1 });
match op {
HalfMulOp::Smul => {
let product = Exec::half_of(m, x).wrapping_mul(Exec::half_of(s, y));
self.set_reg(rd, product as u32);
}
HalfMulOp::Smla => {
let product = Exec::half_of(m, x).wrapping_mul(Exec::half_of(s, y));
let addend = self.reg(rn) as i32;
if product.checked_add(addend).is_none() {
self.set_flag(psr::Q, true);
}
self.set_reg(rd, product.wrapping_add(addend) as u32);
}
HalfMulOp::Smulw | HalfMulOp::Smlaw => {
let wide = i64::from(m as i32) * i64::from(Exec::half_of(s, y));
let product = (wide >> 16) as i32;
if op == HalfMulOp::Smulw {
self.set_reg(rd, product as u32);
} else {
let addend = self.reg(rn) as i32;
if product.checked_add(addend).is_none() {
self.set_flag(psr::Q, true);
}
self.set_reg(rd, product.wrapping_add(addend) as u32);
}
}
HalfMulOp::Smlal => {
let product = i64::from(Exec::half_of(m, x)) * i64::from(Exec::half_of(s, y));
let acc = ((u64::from(self.reg(rd)) << 32) | u64::from(self.reg(rn))) as i64;
let result = acc.wrapping_add(product) as u64;
self.set_reg(rn, result as u32);
self.set_reg(rd, (result >> 32) as u32);
}
}
}
#[allow(clippy::too_many_arguments)] fn load_store(
&mut self,
load: bool,
byte: bool,
up: bool,
index: Index,
rn: u8,
rd: u8,
offset: Offset,
) -> Ex {
let base = self.reg(rn);
let delta = self.eval_offset(offset);
let adjusted = if up {
base.wrapping_add(delta)
} else {
base.wrapping_sub(delta)
};
let address = match index {
Index::Pre { .. } => adjusted,
Index::Post { .. } => base,
};
let privileged = match index {
Index::Post { unprivileged: true } => false,
_ => self.privileged(),
};
if load {
let value = if byte {
self.load(address, Width::U8, privileged)?
} else {
self.load_word_rotated(address, privileged)?
};
self.cycle(1);
if index.writes_base() {
self.set_reg(rn, adjusted);
}
if rd & 0xf == 15 {
self.branch_exchange(value);
} else {
self.set_reg(rd, value);
}
} else {
let value = self.store_value(rd);
if byte {
self.store(address, Width::U8, value & 0xff, privileged)?;
} else {
self.store(address & !3, Width::U32, value, privileged)?;
}
if index.writes_base() {
self.set_reg(rn, adjusted);
}
}
Ok(())
}
#[allow(clippy::too_many_arguments)] fn load_store_extra(
&mut self,
op: ExtraOp,
up: bool,
index: Index,
rn: u8,
rd: u8,
offset: Offset,
) -> Ex {
let base = self.reg(rn);
let delta = self.eval_offset(offset);
let adjusted = if up {
base.wrapping_add(delta)
} else {
base.wrapping_sub(delta)
};
let address = match index {
Index::Pre { .. } => adjusted,
Index::Post { .. } => base,
};
let privileged = self.privileged();
match op {
ExtraOp::Strh => {
let value = self.store_value(rd);
self.store(address & !1, Width::U16, value & 0xffff, privileged)?;
}
ExtraOp::Ldrh => {
let value = self.load_half_rotated(address, privileged)?;
self.cycle(1);
self.finish_extra_load(index, rn, adjusted, rd, value);
return Ok(());
}
ExtraOp::Ldrsb => {
let value = self.load(address, Width::U8, privileged)?;
self.cycle(1);
let value = i32::from(value as u8 as i8) as u32;
self.finish_extra_load(index, rn, adjusted, rd, value);
return Ok(());
}
ExtraOp::Ldrsh => {
let value = self.load_signed_half(address, privileged)?;
self.cycle(1);
self.finish_extra_load(index, rn, adjusted, rd, value);
return Ok(());
}
ExtraOp::Ldrd => {
if rd & 1 != 0 || rd & 0xf == 14 {
self.undefined_instruction();
return Ok(());
}
let low = self.load(address & !3, Width::U32, privileged)?;
let high = self.load((address & !3).wrapping_add(4), Width::U32, privileged)?;
self.cycle(1);
if index.writes_base() {
self.set_reg(rn, adjusted);
}
self.set_reg(rd, low);
self.set_reg(rd + 1, high);
return Ok(());
}
ExtraOp::Strd => {
if rd & 1 != 0 || rd & 0xf == 14 {
self.undefined_instruction();
return Ok(());
}
let low = self.reg(rd);
let high = self.reg(rd + 1);
self.store(address & !3, Width::U32, low, privileged)?;
self.store((address & !3).wrapping_add(4), Width::U32, high, privileged)?;
}
}
if index.writes_base() {
self.set_reg(rn, adjusted);
}
Ok(())
}
fn finish_extra_load(&mut self, index: Index, rn: u8, adjusted: u32, rd: u8, value: u32) {
if index.writes_base() {
self.set_reg(rn, adjusted);
}
if rd & 0xf == 15 {
self.branch_to(value);
} else {
self.set_reg(rd, value);
}
}
#[allow(clippy::too_many_arguments)] #[allow(clippy::too_many_lines)]
fn block_transfer(
&mut self,
load: bool,
before: bool,
up: bool,
user: bool,
writeback: bool,
rn: u8,
list: u16,
) -> Ex {
let base = self.reg(rn);
let count = list.count_ones();
let (effective_list, span) = if count == 0 {
(0x8000u16, 0x40u32)
} else {
(list, count * 4)
};
let (start, writeback_value) = match (before, up) {
(false, true) => (base, base.wrapping_add(span)),
(true, true) => (base.wrapping_add(4), base.wrapping_add(span)),
(false, false) => (
base.wrapping_sub(span).wrapping_add(4),
base.wrapping_sub(span),
),
(true, false) => (base.wrapping_sub(span), base.wrapping_sub(span)),
};
let loads_pc = load && effective_list & 0x8000 != 0;
let user_bank = user && !loads_pc;
let privileged = self.privileged();
if load && writeback {
self.write_base(rn, writeback_value, user_bank);
}
let mut pc_overwritten = false;
if !load && writeback {
let lowest = effective_list.trailing_zeros();
if effective_list & (1 << (rn & 0xf)) != 0 && u32::from(rn & 0xf) != lowest {
self.write_base(rn, writeback_value, user_bank);
pc_overwritten = rn & 0xf == 15;
}
}
let mut address = start;
for index in 0u8..16 {
if effective_list & (1 << index) == 0 {
continue;
}
let word = address & !3;
if load {
let value = self.load(word, Width::U32, privileged)?;
if user_bank {
self.state.regs.set_reg_in_mode(Mode::USER, index, value);
} else if index == 15 {
if user {
self.return_from_exception(value);
} else {
self.branch_exchange(value);
}
} else {
self.set_reg(index, value);
}
} else {
let value = if index == 15 {
if pc_overwritten {
self.reg(15)
} else {
self.store_value(index)
}
} else if user_bank {
self.state.regs.reg_in_mode(Mode::USER, index)
} else {
self.reg(index)
};
self.store(word, Width::U32, value, privileged)?;
}
address = address.wrapping_add(4);
}
if load {
self.cycle(1);
}
if !load && writeback {
self.write_base(rn, writeback_value, user_bank);
}
Ok(())
}
fn write_base(&mut self, rn: u8, value: u32, user_bank: bool) {
if user_bank && rn & 0xf != 15 {
self.state.regs.set_reg_in_mode(Mode::USER, rn, value);
} else {
self.set_reg(rn, value);
}
}
fn coprocessor(&self, cp: u8) -> Option<&Arc<dyn Coprocessor>> {
self.coprocessors.get((cp & 0xf) as usize)?.as_ref()
}
fn apply_effect(&mut self, effect: CpEffect) {
if effect.halt {
self.state.halted = true;
}
}
fn undefined_instruction(&mut self) {
let lr = self
.insn_addr
.wrapping_add(if self.flag(psr::T) { 2 } else { 4 });
self.take_exception(Exception::Undefined, lr);
}
#[allow(clippy::too_many_arguments)] fn coprocessor_register(
&mut self,
cp: u8,
load: bool,
opc1: u8,
rd: u8,
crn: u8,
crm: u8,
opc2: u8,
) {
let op = CpOp {
cp,
opc1,
crd: 0,
crn,
crm,
opc2,
};
let Some(coprocessor) = self.coprocessor(cp).cloned() else {
self.undefined_instruction();
return;
};
if load {
match coprocessor.mrc(op) {
Ok(value) => {
if rd & 0xf == 15 {
let flags = value & 0xf000_0000;
self.state.regs.cpsr = (self.state.regs.cpsr & 0x0fff_ffff) | flags;
} else {
self.set_reg(rd, value);
}
}
Err(CpFault::Undefined) => self.undefined_instruction(),
}
} else {
let value = self.reg(rd);
match coprocessor.mcr(op, value) {
Ok(effect) => self.apply_effect(effect),
Err(CpFault::Undefined) => self.undefined_instruction(),
}
}
}
#[allow(clippy::too_many_arguments)] fn coprocessor_transfer(
&mut self,
cp: u8,
load: bool,
long: bool,
crd: u8,
rn: u8,
index: Index,
up: bool,
offset: u8,
) -> Ex {
let transfer = CpTransfer {
cp,
crd,
long,
option: offset,
};
let Some(coprocessor) = self.coprocessor(cp).cloned() else {
self.undefined_instruction();
return Ok(());
};
let Ok(words) = coprocessor.transfer_len(transfer) else {
self.undefined_instruction();
return Ok(());
};
let base = self.reg(rn);
let delta = u32::from(offset) * 4;
let adjusted = if up {
base.wrapping_add(delta)
} else {
base.wrapping_sub(delta)
};
let mut address = match index {
Index::Pre { .. } => adjusted,
Index::Post { .. } => base,
};
let privileged = self.privileged();
for word in 0..words {
if load {
let value = self.load(address, Width::U32, privileged)?;
if coprocessor.write_word(transfer, word, value).is_err() {
self.undefined_instruction();
return Ok(());
}
} else {
let Ok(value) = coprocessor.read_word(transfer, word) else {
self.undefined_instruction();
return Ok(());
};
self.store(address, Width::U32, value, privileged)?;
}
address = address.wrapping_add(4);
}
if index.writes_base() {
self.set_reg(rn, adjusted);
}
Ok(())
}
fn step_thumb(&mut self) {
let pc = self.state.regs.r[15];
self.insn_addr = pc;
self.branched = false;
self.state.regs.r[15] = pc.wrapping_add(4);
let half = match self.fetch(pc, Width::U16) {
Ok(w) => w as u16,
Err(abort) => {
self.take_abort(abort);
return;
}
};
let decoded = super::thumb::decode(half);
let saved = self.state.regs.r;
if let Err(abort) = self.execute_thumb(decoded) {
self.state.regs.r = saved;
self.state.regs.r[15] = pc;
self.take_abort(abort);
return;
}
if !self.branched {
self.state.regs.r[15] = pc.wrapping_add(2);
}
}
#[allow(clippy::too_many_lines)] fn execute_thumb(&mut self, insn: Thumb) -> Ex {
let carry_in = self.flag(psr::C);
match insn {
Thumb::ShiftImm { ty, rd, rm, imm } => {
let value = self.reg(rm);
let (result, c) = Exec::shift_immediate(ty, value, imm, carry_in);
self.set_reg(rd, result);
self.set_nz(result);
self.set_flag(psr::C, c);
Ok(())
}
Thumb::AddSub {
sub,
rd,
rn,
operand,
} => {
let a = self.reg(rn);
let b = match operand {
SmallOperand::Reg(r) => self.reg(r),
SmallOperand::Imm(i) => u32::from(i),
};
let op = if sub { DpOp::Sub } else { DpOp::Add };
let (result, c, v) = Exec::alu(op, a, b, carry_in, carry_in, self.flag(psr::V));
self.set_reg(rd, result);
self.commit_flags(result, c, v);
Ok(())
}
Thumb::AluImm { op, rd, imm } => {
let a = self.reg(rd);
let b = u32::from(imm);
let v_in = self.flag(psr::V);
let (result, c, v) = match op {
ImmOp::Mov => Exec::alu(DpOp::Mov, 0, b, carry_in, carry_in, v_in),
ImmOp::Cmp => Exec::alu(DpOp::Cmp, a, b, carry_in, carry_in, v_in),
ImmOp::Add => Exec::alu(DpOp::Add, a, b, carry_in, carry_in, v_in),
ImmOp::Sub => Exec::alu(DpOp::Sub, a, b, carry_in, carry_in, v_in),
};
if op != ImmOp::Cmp {
self.set_reg(rd, result);
}
if op == ImmOp::Mov {
self.set_nz(result);
} else {
self.commit_flags(result, c, v);
}
Ok(())
}
Thumb::Alu { op, rd, rm } => {
self.thumb_alu(op, rd, rm, carry_in);
Ok(())
}
Thumb::HiReg { op, rd, rm } => {
let a = self.reg(rd);
let b = self.reg(rm);
match op {
HiOp::Add => {
let result = a.wrapping_add(b);
if rd & 0xf == 15 {
self.branch_to(result);
} else {
self.set_reg(rd, result);
}
}
HiOp::Cmp => {
let (result, c, v) =
Exec::alu(DpOp::Cmp, a, b, carry_in, carry_in, self.flag(psr::V));
self.commit_flags(result, c, v);
}
HiOp::Mov => {
if rd & 0xf == 15 {
self.branch_to(b);
} else {
self.set_reg(rd, b);
}
}
}
Ok(())
}
Thumb::BranchExchange { link, rm } => {
let target = self.reg(rm);
if link {
self.state.regs.r[14] = self.insn_addr.wrapping_add(2) | 1;
}
self.branch_exchange(target);
Ok(())
}
Thumb::LoadLiteral { rd, imm } => {
let address = (self.state.regs.r[15] & !3).wrapping_add(u32::from(imm) * 4);
let privileged = self.privileged();
let value = self.load(address, Width::U32, privileged)?;
self.cycle(1);
self.set_reg(rd, value);
Ok(())
}
Thumb::MemReg { op, rd, rn, rm } => {
let address = self.reg(rn).wrapping_add(self.reg(rm));
self.thumb_mem_reg(op, rd, address)
}
Thumb::MemImm {
load,
size,
rd,
rn,
imm,
} => {
let address = self.reg(rn).wrapping_add(u32::from(imm) * size.bytes());
self.thumb_mem_sized(load, size, rd, address)
}
Thumb::MemStack { load, rd, imm } => {
let address = self.reg(13).wrapping_add(u32::from(imm) * 4);
self.thumb_mem_sized(load, MemSize::Word, rd, address)
}
Thumb::AddPcSp { sp, rd, imm } => {
let base = if sp {
self.reg(13)
} else {
self.state.regs.r[15] & !3
};
self.set_reg(rd, base.wrapping_add(u32::from(imm) * 4));
Ok(())
}
Thumb::AdjustStack { sub, imm } => {
let delta = u32::from(imm) * 4;
let sp = self.reg(13);
self.set_reg(
13,
if sub {
sp.wrapping_sub(delta)
} else {
sp.wrapping_add(delta)
},
);
Ok(())
}
Thumb::PushPop { load, extra, list } => {
let mut full = u16::from(list);
if extra {
full |= if load { 0x8000 } else { 0x4000 };
}
self.block_transfer(load, !load, load, false, true, 13, full)
}
Thumb::BlockTransfer { load, rn, list } => {
self.block_transfer(load, false, true, false, true, rn, u16::from(list))
}
Thumb::BranchCond { cond, offset } => {
if cond.passes(self.state.regs.cpsr) {
let target = self.state.regs.r[15].wrapping_add(offset as u32);
self.branch_to(target);
}
Ok(())
}
Thumb::Swi { imm } => {
self.state.last_swi = u32::from(imm);
let lr = self.insn_addr.wrapping_add(2);
self.take_exception(Exception::Swi, lr);
Ok(())
}
Thumb::Bkpt { imm } => {
self.state.last_bkpt = u16::from(imm);
let lr = self.insn_addr.wrapping_add(4);
self.take_exception(Exception::PrefetchAbort, lr);
Ok(())
}
Thumb::Branch { offset } => {
let target = self.state.regs.r[15].wrapping_add(offset as u32);
self.branch_to(target);
Ok(())
}
Thumb::BranchLinkPrefix { offset } => {
self.state.regs.r[14] = self.state.regs.r[15].wrapping_add(offset as u32);
Ok(())
}
Thumb::BranchLinkSuffix { exchange, offset } => {
let target = self.state.regs.r[14].wrapping_add(offset);
self.state.regs.r[14] = self.insn_addr.wrapping_add(2) | 1;
if exchange {
self.set_flag(psr::T, false);
self.state.regs.r[15] = target & !3;
} else {
self.state.regs.r[15] = target & !1;
}
self.branched = true;
self.cycle(2);
Ok(())
}
Thumb::Undefined => {
self.undefined_instruction();
Ok(())
}
}
}
fn thumb_alu(&mut self, op: AluOp, rd: u8, rm: u8, carry_in: bool) {
let a = self.reg(rd);
let b = self.reg(rm);
let v_in = self.flag(psr::V);
let dp = |op| Exec::alu(op, a, b, carry_in, carry_in, v_in);
let (result, c, v, writes) = match op {
AluOp::And => (dp(DpOp::And).0, carry_in, v_in, true),
AluOp::Eor => (dp(DpOp::Eor).0, carry_in, v_in, true),
AluOp::Adc => {
let (r, c, v) = dp(DpOp::Adc);
(r, c, v, true)
}
AluOp::Sbc => {
let (r, c, v) = dp(DpOp::Sbc);
(r, c, v, true)
}
AluOp::Tst => (dp(DpOp::And).0, carry_in, v_in, false),
AluOp::Cmp => {
let (r, c, v) = dp(DpOp::Cmp);
(r, c, v, false)
}
AluOp::Cmn => {
let (r, c, v) = dp(DpOp::Cmn);
(r, c, v, false)
}
AluOp::Orr => (dp(DpOp::Orr).0, carry_in, v_in, true),
AluOp::Bic => (dp(DpOp::Bic).0, carry_in, v_in, true),
AluOp::Mvn => (dp(DpOp::Mvn).0, carry_in, v_in, true),
AluOp::Neg => {
let (r, c, v) = Exec::alu(DpOp::Rsb, b, 0, carry_in, carry_in, v_in);
(r, c, v, true)
}
AluOp::Mul => {
self.cycle(Exec::multiply_cycles(a, true));
(a.wrapping_mul(b), carry_in, v_in, true)
}
AluOp::Lsl | AluOp::Lsr | AluOp::Asr | AluOp::Ror => {
self.cycle(1);
let ty = match op {
AluOp::Lsl => ShiftType::Lsl,
AluOp::Lsr => ShiftType::Lsr,
AluOp::Asr => ShiftType::Asr,
_ => ShiftType::Ror,
};
let (r, c) = Exec::shift_register(ty, a, b, carry_in);
(r, c, v_in, true)
}
};
if writes {
self.set_reg(rd, result);
}
self.commit_flags(result, c, v);
}
fn thumb_mem_reg(&mut self, op: MemRegOp, rd: u8, address: u32) -> Ex {
let privileged = self.privileged();
if op.is_load() {
let value = match op {
MemRegOp::Ldr => self.load_word_rotated(address, privileged)?,
MemRegOp::Ldrb => self.load(address, Width::U8, privileged)?,
MemRegOp::Ldrh => self.load_half_rotated(address, privileged)?,
MemRegOp::Ldrsb => {
let v = self.load(address, Width::U8, privileged)?;
i32::from(v as u8 as i8) as u32
}
_ => self.load_signed_half(address, privileged)?,
};
self.cycle(1);
self.set_reg(rd, value);
} else {
let value = self.reg(rd);
match op {
MemRegOp::Strb => self.store(address, Width::U8, value & 0xff, privileged)?,
MemRegOp::Strh => {
self.store(address & !1, Width::U16, value & 0xffff, privileged)?;
}
_ => self.store(address & !3, Width::U32, value, privileged)?,
}
}
Ok(())
}
fn thumb_mem_sized(&mut self, load: bool, size: MemSize, rd: u8, address: u32) -> Ex {
let privileged = self.privileged();
if load {
let value = match size {
MemSize::Byte => self.load(address, Width::U8, privileged)?,
MemSize::Half => self.load_half_rotated(address, privileged)?,
MemSize::Word => self.load_word_rotated(address, privileged)?,
};
self.cycle(1);
self.set_reg(rd, value);
} else {
let value = self.reg(rd);
match size {
MemSize::Byte => self.store(address, Width::U8, value & 0xff, privileged)?,
MemSize::Half => {
self.store(address & !1, Width::U16, value & 0xffff, privileged)?;
}
MemSize::Word => self.store(address & !3, Width::U32, value, privileged)?,
}
}
Ok(())
}
}