instructions.rs1mod disassemble; 2 3use crate::bus::{BusInterface, MemoryCycle}; 4use crate::{Arm7Tdmi, CpuMode, CpuState, Exception, Registers, StatusRegister}; 5use jgenesis_common::define_bit_enum; 6use jgenesis_common::num::GetBit; 7use jgenesis_proc_macros::{FakeDecode, FakeEncode}; 8use std::array; 9use std::cmp::Ordering; 10use std::ops::Deref; 11 12const ARM_OPCODE_LEN: u32 = 4; 13const THUMB_OPCODE_LEN: u32 = 2; 14 15#[derive(Debug, Clone, Copy)] 16struct ConditionCodes { 17 sign: bool, 18 zero: bool, 19 carry: bool, 20 overflow: bool, 21} 22 23impl ConditionCodes { 24 fn logical(value: u32, shifter_out: bool, overflow: bool) -> Self { 25 Self { sign: value.bit(31), zero: value == 0, carry: shifter_out, overflow } 26 } 27} 28 29#[derive(Debug, Clone, Copy, PartialEq, Eq)] 30enum Condition { 31 Equal = 0x0, // EQ (Z=1) 32 NotEqual = 0x1, // NE (Z=0) 33 CarrySet = 0x2, // CS (C=1) 34 CarryClear = 0x3, // CC (C=0) 35 Minus = 0x4, // MI (N=1) 36 Plus = 0x5, // PL (N=0) 37 OverflowSet = 0x6, // VS (V=1) 38 OverflowClear = 0x7, // VC (V=0) 39 Higher = 0x8, // HI (C=1 and Z=0) 40 LowerOrSame = 0x9, // LS (C=0 or Z=1) 41 GreaterOrEqual = 0xA, // GE (N=V) 42 Less = 0xB, // LT (N!=V) 43 Greater = 0xC, // GT (Z=0 and N=V) 44 LessOrEqual = 0xD, // LE (Z=1 or N!=V) 45 Always = 0xE, // AL (always true) 46 Reserved = 0xF, // Reserved - treat as always false 47} 48 49impl Condition { 50 fn from_bits(bits: u32) -> Self { 51 match bits & 0xF { 52 0x0 => Self::Equal, 53 0x1 => Self::NotEqual, 54 0x2 => Self::CarrySet, 55 0x3 => Self::CarryClear, 56 0x4 => Self::Minus, 57 0x5 => Self::Plus, 58 0x6 => Self::OverflowSet, 59 0x7 => Self::OverflowClear, 60 0x8 => Self::Higher, 61 0x9 => Self::LowerOrSame, 62 0xA => Self::GreaterOrEqual, 63 0xB => Self::Less, 64 0xC => Self::Greater, 65 0xD => Self::LessOrEqual, 66 0xE => Self::Always, 67 0xF => Self::Reserved, 68 _ => unreachable!("value is always <= 0xF"), 69 } 70 } 71 72 fn from_arm_opcode(opcode: u32) -> Self { 73 Self::from_bits(opcode >> 28) 74 } 75 76 fn check(self, cpsr: StatusRegister) -> bool { 77 match self { 78 Self::Equal => cpsr.zero, 79 Self::NotEqual => !cpsr.zero, 80 Self::CarrySet => cpsr.carry, 81 Self::CarryClear => !cpsr.carry, 82 Self::Minus => cpsr.sign, 83 Self::Plus => !cpsr.sign, 84 Self::OverflowSet => cpsr.overflow, 85 Self::OverflowClear => !cpsr.overflow, 86 Self::Higher => cpsr.carry && !cpsr.zero, 87 Self::LowerOrSame => !cpsr.carry || cpsr.zero, 88 Self::GreaterOrEqual => cpsr.sign == cpsr.overflow, 89 Self::Less => cpsr.sign != cpsr.overflow, 90 Self::Greater => !cpsr.zero && cpsr.sign == cpsr.overflow, 91 Self::LessOrEqual => cpsr.zero || cpsr.sign != cpsr.overflow, 92 Self::Always => true, 93 Self::Reserved => false, 94 } 95 } 96 97 fn suffix(self) -> &'static str { 98 match self { 99 Self::Equal => "EQ", 100 Self::NotEqual => "NE", 101 Self::CarrySet => "CS", 102 Self::CarryClear => "CC", 103 Self::Minus => "MI", 104 Self::Plus => "PL", 105 Self::OverflowSet => "VS", 106 Self::OverflowClear => "VC", 107 Self::Higher => "HI", 108 Self::LowerOrSame => "LS", 109 Self::GreaterOrEqual => "GE", 110 Self::Less => "LT", 111 Self::Greater => "GT", 112 Self::LessOrEqual => "LE", 113 Self::Always => "", 114 Self::Reserved => "(invalid)", 115 } 116 } 117} 118 119#[derive(Debug, Clone, Copy, PartialEq, Eq)] 120enum AluOp { 121 And = 0x0, 122 ExclusiveOr = 0x1, 123 Subtract = 0x2, 124 ReverseSubtract = 0x3, 125 Add = 0x4, 126 AddCarry = 0x5, 127 SubtractCarry = 0x6, 128 ReverseSubtractCarry = 0x7, 129 Test = 0x8, 130 TestEqual = 0x9, 131 Compare = 0xA, 132 CompareNegate = 0xB, 133 Or = 0xC, 134 Move = 0xD, 135 BitClear = 0xE, 136 MoveNegate = 0xF, 137} 138 139impl AluOp { 140 fn from_bits(bits: u32) -> Self { 141 match bits & 0xF { 142 0x0 => Self::And, 143 0x1 => Self::ExclusiveOr, 144 0x2 => Self::Subtract, 145 0x3 => Self::ReverseSubtract, 146 0x4 => Self::Add, 147 0x5 => Self::AddCarry, 148 0x6 => Self::SubtractCarry, 149 0x7 => Self::ReverseSubtractCarry, 150 0x8 => Self::Test, 151 0x9 => Self::TestEqual, 152 0xA => Self::Compare, 153 0xB => Self::CompareNegate, 154 0xC => Self::Or, 155 0xD => Self::Move, 156 0xE => Self::BitClear, 157 0xF => Self::MoveNegate, 158 _ => unreachable!("value & 0xF is always <= 0xF"), 159 } 160 } 161 162 // TST, TEQ, CMP, CMN 163 fn is_test(self) -> bool { 164 matches!(self, Self::Test | Self::TestEqual | Self::Compare | Self::CompareNegate) 165 } 166} 167 168#[derive(Debug, Clone, Copy, PartialEq, Eq)] 169enum ShiftType { 170 Left = 0, 171 LogicalRight = 1, 172 ArithmeticRight = 2, 173 RotateRight = 3, 174} 175 176impl ShiftType { 177 fn from_bits(bits: u32) -> Self { 178 match bits & 3 { 179 0 => Self::Left, 180 1 => Self::LogicalRight, 181 2 => Self::ArithmeticRight, 182 3 => Self::RotateRight, 183 _ => unreachable!("value & 3 is always <= 3"), 184 } 185 } 186} 187 188define_bit_enum!(LoadIndexing, [Post, Pre]); 189define_bit_enum!(IndexOp, [Subtract, Add]); 190define_bit_enum!(WriteBack, [No, Yes]); 191 192#[derive(Debug, Clone, Copy, PartialEq, Eq)] 193enum HalfwordLoadType { 194 UnsignedHalfword, 195 SignedByte, 196 SignedHalfword, 197} 198 199impl HalfwordLoadType { 200 fn from_bits(bits: u32) -> Option<Self> { 201 match bits & 3 { 202 0 => None, 203 1 => Some(Self::UnsignedHalfword), 204 2 => Some(Self::SignedByte), 205 3 => Some(Self::SignedHalfword), 206 _ => unreachable!("value & 3 is always <= 3"), 207 } 208 } 209} 210 211define_bit_enum!(LoadSize, [Word, Byte]); 212 213pub type ArmFn<Bus> = fn(&mut Arm7Tdmi<Bus>, u32, &mut Bus); 214 215struct ArmDecode<Bus: BusInterface> { 216 mask: u32, 217 target: u32, 218 op_fn: ArmFn<Bus>, 219} 220 221impl<Bus: BusInterface> ArmDecode<Bus> { 222 fn new(mask: u32, target: u32, op_fn: ArmFn<Bus>) -> Self { 223 Self { mask, target, op_fn } 224 } 225} 226 227type ArmFnArray<Bus> = [ArmFn<Bus>; 4096]; 228 229#[derive(Debug, Clone, FakeEncode, FakeDecode)] 230pub struct ArmOpTable<Bus: BusInterface>(Box<ArmFnArray<Bus>>); 231 232impl<Bus: BusInterface> Default for ArmOpTable<Bus> { 233 fn default() -> Self { 234 Self(Box::new(Arm7Tdmi::<Bus>::new_arm_op_table())) 235 } 236} 237 238impl<Bus: BusInterface> Deref for ArmOpTable<Bus> { 239 type Target = Box<ArmFnArray<Bus>>; 240 241 fn deref(&self) -> &Self::Target { 242 &self.0 243 } 244} 245 246pub type ThumbFn<Bus> = fn(&mut Arm7Tdmi<Bus>, u16, &mut Bus); 247 248struct ThumbDecode<Bus: BusInterface> { 249 mask: u16, 250 target: u16, 251 op_fn: ThumbFn<Bus>, 252} 253 254impl<Bus: BusInterface> ThumbDecode<Bus> { 255 const fn new(mask: u16, target: u16, op_fn: ThumbFn<Bus>) -> Self { 256 Self { mask, target, op_fn } 257 } 258} 259 260type ThumbFnArray<Bus> = [ThumbFn<Bus>; 256]; 261 262#[derive(Debug, Clone, FakeEncode, FakeDecode)] 263pub struct ThumbOpTable<Bus: BusInterface>(Box<ThumbFnArray<Bus>>); 264 265impl<Bus: BusInterface> Default for ThumbOpTable<Bus> { 266 fn default() -> Self { 267 Self(Box::new(Arm7Tdmi::<Bus>::new_thumb_op_table())) 268 } 269} 270 271impl<Bus: BusInterface> Deref for ThumbOpTable<Bus> { 272 type Target = Box<ThumbFnArray<Bus>>; 273 274 fn deref(&self) -> &Self::Target { 275 &self.0 276 } 277} 278 279impl<Bus: BusInterface> Arm7Tdmi<Bus> { 280 fn new_arm_op_table() -> ArmFnArray<Bus> { 281 #[rustfmt::skip] 282 let decode_table = &[ 283 ArmDecode::new(0b1111_1111_1111, 0b0001_0010_0001, Self::arm_bx), 284 ArmDecode::new(0b1111_1100_1111, 0b0000_0000_1001, Self::arm_multiply), 285 ArmDecode::new(0b1111_1000_1111, 0b0000_1000_1001, Self::arm_multiply_long), 286 ArmDecode::new(0b1111_1011_1111, 0b0001_0000_1001, Self::arm_swap), 287 ArmDecode::new(0b1110_0101_1001, 0b0000_0000_1001, Self::arm_load_halfword::<false, false>), 288 ArmDecode::new(0b1110_0101_1001, 0b0000_0100_1001, Self::arm_load_halfword::<false, true>), 289 ArmDecode::new(0b1110_0101_1001, 0b0000_0001_1001, Self::arm_load_halfword::<true, false>), 290 ArmDecode::new(0b1110_0101_1001, 0b0000_0101_1001, Self::arm_load_halfword::<true, true>), 291 ArmDecode::new(0b1111_1011_1111, 0b0001_0000_0000, Self::arm_mrs), 292 ArmDecode::new(0b1111_1011_0000, 0b0001_0010_0000, Self::arm_msr::<false>), 293 ArmDecode::new(0b1111_1011_0000, 0b0011_0010_0000, Self::arm_msr::<true>), 294 ArmDecode::new(0b1110_0000_0000, 0b0000_0000_0000, Self::arm_alu::<false>), 295 ArmDecode::new(0b1110_0000_0000, 0b0010_0000_0000, Self::arm_alu::<true>), 296 ArmDecode::new(0b1110_0001_0000, 0b0100_0000_0000, Self::arm_load_word::<false, false>), 297 ArmDecode::new(0b1110_0001_0000, 0b0100_0001_0000, Self::arm_load_word::<true, false>), 298 ArmDecode::new(0b1110_0001_0000, 0b0110_0000_0000, Self::arm_load_word::<false, true>), 299 ArmDecode::new(0b1110_0001_0000, 0b0110_0001_0000, Self::arm_load_word::<true, true>), 300 ArmDecode::new(0b1111_1001_0000, 0b1000_0000_0000, Self::arm_ldm_stm::<false, false, true>), 301 ArmDecode::new(0b1111_1001_0000, 0b1000_0001_0000, Self::arm_ldm_stm::<true, false, true>), 302 ArmDecode::new(0b1111_1001_0000, 0b1000_1000_0000, Self::arm_ldm_stm::<false, true, true>), 303 ArmDecode::new(0b1111_1001_0000, 0b1000_1001_0000, Self::arm_ldm_stm::<true, true, true>), 304 ArmDecode::new(0b1111_1001_0000, 0b1001_0000_0000, Self::arm_ldm_stm::<false, false, false>), 305 ArmDecode::new(0b1111_1001_0000, 0b1001_0001_0000, Self::arm_ldm_stm::<true, false, false>), 306 ArmDecode::new(0b1111_1001_0000, 0b1001_1000_0000, Self::arm_ldm_stm::<false, true, false>), 307 ArmDecode::new(0b1111_1001_0000, 0b1001_1001_0000, Self::arm_ldm_stm::<true, true, false>), 308 ArmDecode::new(0b1111_0000_0000, 0b1010_0000_0000, Self::arm_branch::<false>), 309 ArmDecode::new(0b1111_0000_0000, 0b1011_0000_0000, Self::arm_branch::<true>), 310 ArmDecode::new(0b1111_0000_0000, 0b1111_0000_0000, Self::arm_swi), 311 ]; 312 313 array::from_fn(|opcode_mask| { 314 let opcode_mask = opcode_mask as u32; 315 for &ArmDecode { mask, target, op_fn } in decode_table { 316 if opcode_mask & mask == target { 317 return op_fn; 318 } 319 } 320 321 |cpu, opcode, bus| { 322 log::error!("Executed undefined ARM opcode: {opcode:08X}"); 323 cpu.handle_exception(Exception::UndefinedInstruction, bus); 324 } 325 }) 326 } 327 328 pub(crate) fn execute_arm_opcode(&mut self, opcode: u32, bus: &mut Bus) { 329 if log::log_enabled!(log::Level::Trace) { 330 log::trace!( 331 "Executing opcode {opcode:08X}, PC+8={:08X}, str={}", 332 self.registers.r[15], 333 disassemble::arm(opcode) 334 ); 335 log::trace!(" R={:08X?}", self.registers.r); 336 } 337 338 let condition = Condition::from_arm_opcode(opcode); 339 if !condition.check(self.registers.cpsr) { 340 return; 341 } 342 343 let opcode_mask = ((opcode >> 16) & 0xFF0) | ((opcode >> 4) & 0xF); 344 self.arm_op_table[opcode_mask as usize](self, opcode, bus); 345 } 346 347 fn new_thumb_op_table() -> ThumbFnArray<Bus> { 348 #[rustfmt::skip] 349 let decode_table = &[ 350 ThumbDecode::new(0xF8, 0x18, Self::thumb_add_sub), 351 ThumbDecode::new(0xE0, 0x00, Self::thumb_move_shifted_register), 352 ThumbDecode::new(0xE0, 0x20, Self::thumb_alu_immediate), 353 ThumbDecode::new(0xFC, 0x40, Self::thumb_alu), 354 ThumbDecode::new(0xFC, 0x44, Self::thumb_high_register_op), 355 ThumbDecode::new(0xF8, 0x48, Self::thumb_pc_relative_load), 356 ThumbDecode::new(0xF2, 0x50, Self::thumb_load_register_offset), 357 ThumbDecode::new(0xF2, 0x52, Self::thumb_load_sign_extended), 358 ThumbDecode::new(0xE0, 0x60, Self::thumb_load_immediate_offset), 359 ThumbDecode::new(0xF0, 0x80, Self::thumb_load_halfword), 360 ThumbDecode::new(0xF0, 0x90, Self::thumb_load_sp_relative), 361 ThumbDecode::new(0xF0, 0xA0, Self::thumb_load_address), 362 ThumbDecode::new(0xFF, 0xB0, Self::thumb_add_offset_sp), 363 ThumbDecode::new(0xF6, 0xB4, Self::thumb_push_pop), 364 ThumbDecode::new(0xF0, 0xC0, Self::thumb_load_multiple), 365 ThumbDecode::new(0xFF, 0xDF, Self::thumb_software_interrupt), 366 ThumbDecode::new(0xF0, 0xD0, Self::thumb_conditional_branch), 367 ThumbDecode::new(0xF8, 0xE0, Self::thumb_unconditional_branch), 368 ThumbDecode::new(0xF0, 0xF0, Self::thumb_long_branch), 369 ]; 370 371 array::from_fn(|opcode_mask| { 372 let opcode_mask = opcode_mask as u16; 373 374 for &ThumbDecode { mask, target, op_fn } in decode_table { 375 if opcode_mask & mask == target { 376 return op_fn; 377 } 378 } 379 380 |cpu, opcode, bus| { 381 log::error!("Executed undefined Thumb opcode: {opcode:04X}"); 382 cpu.handle_exception(Exception::UndefinedInstruction, bus); 383 } 384 }) 385 } 386 387 pub(crate) fn execute_thumb_opcode(&mut self, opcode: u16, bus: &mut Bus) { 388 if log::log_enabled!(log::Level::Trace) { 389 log::trace!( 390 "Executing opcode {opcode:04X}, PC+4={:08X}, str={}", 391 self.registers.r[15], 392 disassemble::thumb(opcode) 393 ); 394 log::trace!(" R={:08X?}", self.registers.r); 395 } 396 397 let opcode_mask = opcode >> 8; 398 self.thumb_op_table[opcode_mask as usize](self, opcode, bus); 399 } 400 401 // B: Branch 402 // BL: Branch and link 403 fn arm_branch<const LINK: bool>(&mut self, opcode: u32, bus: &mut Bus) { 404 // Offset is a 24-bit signed value, shifted left 2 405 let offset = (((opcode & 0xFFFFFF) as i32) << 8) >> 6; 406 self.branch::<LINK, ARM_OPCODE_LEN>(offset, bus); 407 } 408 409 fn branch<const LINK: bool, const OPCODE_LEN: u32>(&mut self, offset: i32, bus: &mut Bus) { 410 if LINK { 411 self.registers.r[14] = self.prev_r15.wrapping_sub(OPCODE_LEN); 412 } 413 414 self.registers.r[15] = self.prev_r15.wrapping_add_signed(offset); 415 self.refill_prefetch(bus); 416 } 417 418 // BX: Branch and exchange ARM/Thumb state 419 fn arm_bx(&mut self, opcode: u32, bus: &mut Bus) { 420 self.branch_exchange(opcode & 0xF, bus); 421 } 422 423 fn branch_exchange(&mut self, rn: u32, bus: &mut Bus) { 424 let new_pc = self.read_register(rn); 425 self.registers.cpsr.state = CpuState::from_bit(new_pc.bit(0)); 426 427 log::trace!("CPU state is now {:?}", self.registers.cpsr.state); 428 429 self.registers.r[15] = new_pc; 430 self.refill_prefetch(bus); 431 } 432 433 // MRS: Transfer PSR contents to register 434 fn arm_mrs(&mut self, opcode: u32, _bus: &mut Bus) { 435 let rd = (opcode >> 12) & 0xF; 436 let spsr = opcode.bit(22); 437 438 if spsr { 439 let spsr = match self.registers.cpsr.mode.spsr(&mut self.registers) { 440 Some(spsr) => *spsr, 441 None => { 442 // SPSR reads in user/system mode access CPSR 443 self.registers.cpsr 444 } 445 }; 446 self.registers.r[rd as usize] = spsr.into(); 447 } else { 448 self.registers.r[rd as usize] = self.registers.cpsr.into(); 449 } 450 } 451 452 // MSR: Transfer register contents to PSR 453 fn arm_msr<const IMMEDIATE: bool>(&mut self, opcode: u32, _bus: &mut Bus) { 454 const CONTROL_MASK: u32 = 0xFF; 455 const FLAGS_MASK: u32 = 0xF << 28; 456 457 let mut operand = if IMMEDIATE { 458 let (immediate, rotation) = arm_parse_rotated_immediate(opcode); 459 immediate.rotate_right(rotation) 460 } else { 461 self.read_register(opcode & 0xF) 462 }; 463 464 let spsr = opcode.bit(22); 465 let psr = if spsr { 466 match self.registers.cpsr.mode.spsr(&mut self.registers) { 467 Some(spsr) => spsr, 468 None => { 469 // SPSR writes in user/system mode do nothing 470 return; 471 } 472 } 473 } else { 474 &mut self.registers.cpsr 475 }; 476 477 let control = opcode.bit(16); 478 let flags = opcode.bit(19); 479 480 if !control { 481 operand = (operand & !CONTROL_MASK) | (u32::from(*psr) & CONTROL_MASK); 482 } 483 if !flags { 484 operand = (operand & !FLAGS_MASK) | (u32::from(*psr) & FLAGS_MASK); 485 } 486 487 if spsr { 488 *psr = operand.into(); 489 } else { 490 self.write_cpsr(operand); 491 } 492 } 493 494 fn arm_swap(&mut self, opcode: u32, bus: &mut Bus) { 495 let rm = opcode & 0xF; 496 let rd = (opcode >> 12) & 0xF; 497 let rn = (opcode >> 16) & 0xF; 498 let size = LoadSize::from_bit(opcode.bit(22)); 499 500 self.swap(rm, rd, rn, size, bus); 501 } 502 503 fn swap(&mut self, rm: u32, rd: u32, rn: u32, size: LoadSize, bus: &mut Bus) { 504 let address = self.registers.r[rn as usize]; 505 506 bus.lock(); 507 508 match size { 509 LoadSize::Word => { 510 let value = bus.read_word(address, MemoryCycle::N).rotate_right(8 * (address & 3)); 511 bus.write_word(address, self.read_register(rm), MemoryCycle::S); 512 self.registers.r[rd as usize] = value; 513 } 514 LoadSize::Byte => { 515 let value = bus.read_byte(address, MemoryCycle::N); 516 bus.write_byte(address, self.read_register(rm) as u8, MemoryCycle::S); 517 self.registers.r[rd as usize] = value.into(); 518 } 519 } 520 521 bus.unlock(); 522 523 bus.internal_cycles(1); 524 } 525 526 // Data processing instructions 527 // AND: And 528 // EOR: Exclusive or 529 // SUB: Subtract 530 // RSB: Reverse subtract 531 // ADD: Add 532 // ADC: Add with carry 533 // SBC: Subtract with carry 534 // RSC: Reverse subtract with carry 535 // TST: Test bits 536 // TEQ: Test equal 537 // CMP: Compare 538 // CMN: Compare negated 539 // ORR: Or 540 // MOV: Move 541 // BIC: Bit clear 542 // MVN: Move negated 543 fn arm_alu<const IMMEDIATE: bool>(&mut self, opcode: u32, bus: &mut Bus) { 544 let alu_op = AluOp::from_bits(opcode >> 21); 545 let set_condition_codes = opcode.bit(20); 546 let rn = (opcode >> 16) & 0xF; 547 let rd = (opcode >> 12) & 0xF; 548 549 if IMMEDIATE { 550 let (immediate, rotation) = arm_parse_rotated_immediate(opcode); 551 self.alu_rotated_immediate( 552 alu_op, 553 rn, 554 rd, 555 set_condition_codes, 556 immediate, 557 rotation, 558 bus, 559 ); 560 } else if opcode.bit(4) { 561 let rm = opcode & 0xF; 562 let rs = (opcode >> 8) & 0xF; 563 let shift_type = ShiftType::from_bits(opcode >> 5); 564 self.alu_register_shift::<ARM_OPCODE_LEN>( 565 alu_op, 566 rn, 567 rd, 568 set_condition_codes, 569 rm, 570 shift_type, 571 rs, 572 bus, 573 ); 574 } else { 575 let rm = opcode & 0xF; 576 let shift_type = ShiftType::from_bits(opcode >> 5); 577 let shift = (opcode >> 7) & 0x1F; 578 self.alu_immediate_shift( 579 alu_op, 580 rn, 581 rd, 582 set_condition_codes, 583 rm, 584 shift_type, 585 shift, 586 bus, 587 ); 588 } 589 } 590 591 #[inline] 592 #[allow(clippy::too_many_arguments)] 593 fn alu_rotated_immediate( 594 &mut self, 595 op: AluOp, 596 rn: u32, 597 rd: u32, 598 set_condition_codes: bool, 599 immediate: u32, 600 rotation: u32, 601 bus: &mut Bus, 602 ) { 603 let (operand2, shifter_out) = if rotation != 0 { 604 (immediate.rotate_right(rotation), immediate.bit((rotation - 1) as u8)) 605 } else { 606 (immediate, self.registers.cpsr.carry) 607 }; 608 609 self.alu(op, self.read_register(rn), rd, set_condition_codes, operand2, shifter_out, bus); 610 } 611 612 #[inline] 613 #[allow(clippy::too_many_arguments)] 614 fn alu_immediate_shift( 615 &mut self, 616 op: AluOp, 617 rn: u32, 618 rd: u32, 619 set_condition_codes: bool, 620 rm: u32, 621 shift_type: ShiftType, 622 shift: u32, 623 bus: &mut Bus, 624 ) { 625 let value = self.read_register(rm); 626 627 let (operand2, shifter_out) = 628 apply_immediate_shift(value, shift_type, shift, self.registers.cpsr.carry); 629 630 self.alu(op, self.read_register(rn), rd, set_condition_codes, operand2, shifter_out, bus); 631 } 632 633 #[inline] 634 #[allow(clippy::too_many_arguments)] 635 fn alu_register_shift<const OPCODE_LEN: u32>( 636 &mut self, 637 op: AluOp, 638 rn: u32, 639 rd: u32, 640 set_condition_codes: bool, 641 rm: u32, 642 shift_type: ShiftType, 643 rs: u32, 644 bus: &mut Bus, 645 ) { 646 let value = self.registers.r[rm as usize]; 647 let shift = self.registers.r[rs as usize] & 0xFF; 648 649 let (operand2, shifter_out) = if shift == 0 { 650 (value, self.registers.cpsr.carry) 651 } else { 652 match shift_type { 653 ShiftType::Left => match shift.cmp(&32) { 654 Ordering::Less => (value << shift, value.bit((32 - shift) as u8)), 655 Ordering::Equal => (0, value.bit(0)), 656 Ordering::Greater => (0, false), 657 }, 658 ShiftType::LogicalRight => match shift.cmp(&32) { 659 Ordering::Less => (value >> shift, value.bit((shift - 1) as u8)), 660 Ordering::Equal => (0, value.bit(31)), 661 Ordering::Greater => (0, false), 662 }, 663 ShiftType::ArithmeticRight => { 664 if shift < 32 { 665 (((value as i32) >> shift) as u32, value.bit((shift - 1) as u8)) 666 } else { 667 (((value as i32) >> 31) as u32, value.bit(31)) 668 } 669 } 670 ShiftType::RotateRight => { 671 let mut shift = shift; 672 while shift > 32 { 673 shift -= 32; 674 } 675 676 if shift == 32 { 677 (value, value.bit(31)) 678 } else { 679 (value.rotate_right(shift), value.bit((shift - 1) as u8)) 680 } 681 } 682 } 683 }; 684 685 let operand1 = self.registers.r[rn as usize]; 686 687 // Register specified shift adds 1I 688 bus.internal_cycles(1); 689 690 self.alu(op, operand1, rd, set_condition_codes, operand2, shifter_out, bus); 691 } 692 693 #[inline] 694 #[allow(clippy::too_many_arguments)] 695 fn alu( 696 &mut self, 697 op: AluOp, 698 operand1: u32, 699 rd: u32, 700 set_condition_codes: bool, 701 operand2: u32, 702 shifter_out: bool, 703 bus: &mut Bus, 704 ) { 705 let (result, codes) = match op { 706 AluOp::And | AluOp::Test => { 707 let result = operand1 & operand2; 708 (result, ConditionCodes::logical(result, shifter_out, self.registers.cpsr.overflow)) 709 } 710 AluOp::ExclusiveOr | AluOp::TestEqual => { 711 let result = operand1 ^ operand2; 712 (result, ConditionCodes::logical(result, shifter_out, self.registers.cpsr.overflow)) 713 } 714 AluOp::Subtract | AluOp::Compare => alu_add(operand1, !operand2, true), 715 AluOp::ReverseSubtract => alu_add(operand2, !operand1, true), 716 AluOp::Add | AluOp::CompareNegate => alu_add(operand1, operand2, false), 717 AluOp::AddCarry => alu_add(operand1, operand2, self.registers.cpsr.carry), 718 AluOp::SubtractCarry => alu_add(operand1, !operand2, self.registers.cpsr.carry), 719 AluOp::ReverseSubtractCarry => alu_add(operand2, !operand1, self.registers.cpsr.carry), 720 AluOp::Or => { 721 let result = operand1 | operand2; 722 (result, ConditionCodes::logical(result, shifter_out, self.registers.cpsr.overflow)) 723 } 724 AluOp::Move => ( 725 operand2, 726 ConditionCodes::logical(operand2, shifter_out, self.registers.cpsr.overflow), 727 ), 728 AluOp::BitClear => { 729 let result = operand1 & !operand2; 730 (result, ConditionCodes::logical(result, shifter_out, self.registers.cpsr.overflow)) 731 } 732 AluOp::MoveNegate => ( 733 !operand2, 734 ConditionCodes::logical(!operand2, shifter_out, self.registers.cpsr.overflow), 735 ), 736 }; 737 738 if set_condition_codes { 739 if rd == 15 { 740 self.spsr_to_cpsr(); 741 } else { 742 self.registers.cpsr.sign = codes.sign; 743 self.registers.cpsr.zero = codes.zero; 744 self.registers.cpsr.carry = codes.carry; 745 self.registers.cpsr.overflow = codes.overflow; 746 } 747 } 748 749 if !op.is_test() { 750 self.registers.r[rd as usize] = result; 751 752 if rd == 15 { 753 self.refill_prefetch(bus); 754 } 755 } 756 } 757 758 fn arm_multiply(&mut self, opcode: u32, bus: &mut Bus) { 759 let rm = opcode & 0xF; 760 let rs = (opcode >> 8) & 0xF; 761 let rn = (opcode >> 12) & 0xF; 762 let rd = (opcode >> 16) & 0xF; 763 let set_condition_codes = opcode.bit(20); 764 let accumulate = opcode.bit(21); 765 766 self.multiply(rm, rs, rn, rd, set_condition_codes, accumulate, bus); 767 } 768 769 #[allow(clippy::too_many_arguments)] 770 fn multiply( 771 &mut self, 772 rm: u32, 773 rs: u32, 774 rn: u32, 775 rd: u32, 776 set_condition_codes: bool, 777 accumulate: bool, 778 bus: &mut Bus, 779 ) { 780 let operand = self.registers.r[rs as usize]; 781 let mut product = self.registers.r[rm as usize].wrapping_mul(operand); 782 if accumulate { 783 product = product.wrapping_add(self.registers.r[rn as usize]); 784 } 785 786 self.registers.r[rd as usize] = product; 787 788 if set_condition_codes { 789 self.registers.cpsr.sign = product.bit(31); 790 self.registers.cpsr.zero = product == 0; 791 // TODO carry bit? 792 } 793 794 let m = if operand & 0xFFFFFF00 == 0 || operand & 0xFFFFFF00 == 0xFFFFFF00 { 795 1 796 } else if operand & 0xFFFF0000 == 0 || operand & 0xFFFF0000 == 0xFFFF0000 { 797 2 798 } else if operand & 0xFF000000 == 0 || operand & 0xFF000000 == 0xFF000000 { 799 3 800 } else { 801 4 802 }; 803 804 let i_cycles = m + u32::from(accumulate); 805 bus.internal_cycles(i_cycles); 806 } 807 808 fn arm_multiply_long(&mut self, opcode: u32, bus: &mut Bus) { 809 let rm = opcode & 0xF; 810 let rs = (opcode >> 8) & 0xF; 811 let rdlo = (opcode >> 12) & 0xF; 812 let rdhi = (opcode >> 16) & 0xF; 813 let set_condition_codes = opcode.bit(20); 814 let accumulate = opcode.bit(21); 815 let signed = opcode.bit(22); 816 817 self.multiply_long(rm, rs, rdlo, rdhi, set_condition_codes, accumulate, signed, bus); 818 } 819 820 #[inline] 821 #[allow(clippy::too_many_arguments)] 822 fn multiply_long( 823 &mut self, 824 rm: u32, 825 rs: u32, 826 rdlo: u32, 827 rdhi: u32, 828 set_condition_codes: bool, 829 accumulate: bool, 830 signed: bool, 831 bus: &mut Bus, 832 ) { 833 let operand = self.registers.r[rs as usize]; 834 let product = if signed { 835 let mut product = 836 i64::from(self.registers.r[rm as usize] as i32) * i64::from(operand as i32); 837 if accumulate { 838 let existing = (i64::from(self.registers.r[rdhi as usize]) << 32) 839 | i64::from(self.registers.r[rdlo as usize]); 840 product = product.wrapping_add(existing); 841 } 842 product as u64 843 } else { 844 let mut product = u64::from(self.registers.r[rm as usize]) * u64::from(operand); 845 if accumulate { 846 let existing = (u64::from(self.registers.r[rdhi as usize]) << 32) 847 | u64::from(self.registers.r[rdlo as usize]); 848 product = product.wrapping_add(existing); 849 } 850 product 851 }; 852 853 self.registers.r[rdlo as usize] = product as u32; 854 self.registers.r[rdhi as usize] = (product >> 32) as u32; 855 856 if set_condition_codes { 857 self.registers.cpsr.sign = product.bit(63); 858 self.registers.cpsr.zero = product == 0; 859 // TODO carry and overflow flags? 860 } 861 862 let m = if operand & 0xFFFFFF00 == 0 || (signed && operand & 0xFFFFFF00 == 0xFFFFFF00) { 863 1 864 } else if operand & 0xFFFF0000 == 0 || (signed && operand & 0xFFFF0000 == 0xFFFF0000) { 865 2 866 } else if operand & 0xFF000000 == 0 || (signed && operand & 0xFF000000 == 0xFF000000) { 867 3 868 } else { 869 4 870 }; 871 872 let i_cycles = 1 + m + u32::from(accumulate); 873 bus.internal_cycles(i_cycles); 874 } 875 876 // LDR: Load word 877 // STR: Store word 878 fn arm_load_word<const LOAD: bool, const REGISTER_OFFSET: bool>( 879 &mut self, 880 opcode: u32, 881 bus: &mut Bus, 882 ) { 883 let indexing = LoadIndexing::from_bit(opcode.bit(24)); 884 let index_op = IndexOp::from_bit(opcode.bit(23)); 885 let size = LoadSize::from_bit(opcode.bit(22)); 886 let write_back = WriteBack::from_bit(opcode.bit(21)); 887 let rn = (opcode >> 16) & 0xF; 888 let rd = (opcode >> 12) & 0xF; 889 890 let offset = if REGISTER_OFFSET { 891 let rm = opcode & 0xF; 892 let value = self.registers.r[rm as usize]; 893 let shift_type = ShiftType::from_bits(opcode >> 5); 894 let shift = (opcode >> 7) & 0x1F; 895 896 let (shifted, _) = 897 apply_immediate_shift(value, shift_type, shift, self.registers.cpsr.carry); 898 shifted 899 } else { 900 opcode & 0xFFF 901 }; 902 903 self.load_word::<LOAD>(rn, rd, offset, size, indexing, index_op, write_back, bus); 904 } 905 906 #[inline] 907 #[allow(clippy::too_many_arguments)] 908 fn load_word<const LOAD: bool>( 909 &mut self, 910 rn: u32, 911 rd: u32, 912 mut offset: u32, 913 size: LoadSize, 914 indexing: LoadIndexing, 915 index_op: IndexOp, 916 write_back: WriteBack, 917 bus: &mut Bus, 918 ) { 919 if index_op == IndexOp::Subtract { 920 offset = (!offset).wrapping_add(1); 921 } 922 923 let mut address = self.read_register(rn); 924 925 if indexing == LoadIndexing::Pre { 926 address = address.wrapping_add(offset); 927 } 928 929 if LOAD { 930 self.registers.r[rd as usize] = match size { 931 LoadSize::Word => { 932 let word = bus.read_word(address, MemoryCycle::N); 933 word.rotate_right(8 * (address & 3)) 934 } 935 LoadSize::Byte => bus.read_byte(address, MemoryCycle::N).into(), 936 }; 937 938 bus.internal_cycles(1); 939 940 if rd == 15 { 941 self.refill_prefetch(bus); 942 } 943 } else { 944 let value = self.registers.r[rd as usize]; 945 match size { 946 LoadSize::Word => bus.write_word(address, value, MemoryCycle::N), 947 LoadSize::Byte => bus.write_byte(address, value as u8, MemoryCycle::N), 948 } 949 950 // Next opcode fetch is N 951 self.fetch_cycle = MemoryCycle::N; 952 } 953 954 // Write back only applies on loads if the base register and destination register are different 955 if !(LOAD && rn == rd) { 956 if indexing == LoadIndexing::Post { 957 self.registers.r[rn as usize] = address.wrapping_add(offset); 958 if rn == 15 { 959 self.refill_prefetch(bus); 960 } 961 } else if write_back == WriteBack::Yes { 962 self.registers.r[rn as usize] = address; 963 if rn == 15 { 964 self.refill_prefetch(bus); 965 } 966 } 967 } 968 } 969 970 // LDRH: Load halfword 971 // STRH: Store halfword 972 // LDRSB: Load sign-extended byte 973 // LDRSH: Load sign-extended halfword 974 fn arm_load_halfword<const LOAD: bool, const IMMEDIATE_OFFSET: bool>( 975 &mut self, 976 opcode: u32, 977 bus: &mut Bus, 978 ) { 979 let indexing = LoadIndexing::from_bit(opcode.bit(24)); 980 let index_op = IndexOp::from_bit(opcode.bit(23)); 981 let write_back = WriteBack::from_bit(opcode.bit(21)); 982 let rn = (opcode >> 16) & 0xF; 983 let rd = (opcode >> 12) & 0xF; 984 985 let offset = if IMMEDIATE_OFFSET { 986 ((opcode >> 4) & 0xF0) | (opcode & 0xF) 987 } else { 988 let rm = opcode & 0xF; 989 self.read_register(rm) 990 }; 991 992 let Some(load_type) = HalfwordLoadType::from_bits(opcode >> 5) else { 993 // TODO this is definitely not correct; how should these opcodes decode? 994 return self.arm_swap(opcode, bus); 995 }; 996 997 self.load_halfword::<LOAD>(rn, rd, offset, load_type, indexing, index_op, write_back, bus); 998 } 999 1000 #[inline] 1001 #[allow(clippy::too_many_arguments)] 1002 fn load_halfword<const LOAD: bool>( 1003 &mut self, 1004 rn: u32, 1005 rd: u32, 1006 mut offset: u32, 1007 load_type: HalfwordLoadType, 1008 indexing: LoadIndexing, 1009 index_op: IndexOp, 1010 write_back: WriteBack, 1011 bus: &mut Bus, 1012 ) { 1013 if index_op == IndexOp::Subtract { 1014 offset = (!offset).wrapping_add(1); 1015 } 1016 1017 let mut address = self.read_register(rn); 1018 if indexing == LoadIndexing::Pre { 1019 address = address.wrapping_add(offset); 1020 } 1021 1022 if LOAD { 1023 let value = match load_type { 1024 HalfwordLoadType::UnsignedHalfword => { 1025 let halfword: u32 = bus.read_halfword(address, MemoryCycle::N).into(); 1026 halfword.rotate_right(8 * (address & 1)) 1027 } 1028 HalfwordLoadType::SignedByte => bus.read_byte(address, MemoryCycle::N) as i8 as u32, 1029 HalfwordLoadType::SignedHalfword => { 1030 if !address.bit(0) { 1031 bus.read_halfword(address, MemoryCycle::N) as i16 as u32 1032 } else { 1033 // Unaligned LDRSH seems to behave the same as LDRSB 1034 bus.read_byte(address, MemoryCycle::N) as i8 as u32 1035 } 1036 } 1037 }; 1038 self.registers.r[rd as usize] = value; 1039 1040 bus.internal_cycles(1); 1041 1042 if rd == 15 { 1043 self.refill_prefetch(bus); 1044 } 1045 } else { 1046 let halfword = self.registers.r[rd as usize] as u16; 1047 bus.write_halfword(address, halfword, MemoryCycle::N); 1048 1049 // Next opcode fetch is N 1050 self.fetch_cycle = MemoryCycle::N; 1051 } 1052 1053 // Write back only applies on loads if the base register and destination register are different 1054 if !(LOAD && rn == rd) { 1055 if indexing == LoadIndexing::Post { 1056 self.registers.r[rn as usize] = address.wrapping_add(offset); 1057 } else if write_back == WriteBack::Yes { 1058 self.registers.r[rn as usize] = address; 1059 } 1060 } 1061 } 1062 1063 fn arm_ldm_stm<const LOAD: bool, const INCREMENT: bool, const AFTER: bool>( 1064 &mut self, 1065 opcode: u32, 1066 bus: &mut Bus, 1067 ) { 1068 let register_bits = opcode & 0xFFFF; 1069 let rn = (opcode >> 16) & 0xF; 1070 let write_back = WriteBack::from_bit(opcode.bit(21)); 1071 let s_bit = opcode.bit(22); 1072 1073 self.load_multiple::<LOAD, INCREMENT, AFTER>(register_bits, rn, write_back, s_bit, bus); 1074 } 1075 1076 #[inline] 1077 fn load_multiple<const LOAD: bool, const INCREMENT: bool, const AFTER: bool>( 1078 &mut self, 1079 mut register_bits: u32, 1080 rn: u32, 1081 write_back: WriteBack, 1082 s_bit: bool, 1083 bus: &mut Bus, 1084 ) { 1085 let mut empty_list = false; 1086 if register_bits == 0 { 1087 // Hardware quirk: empty list loads/stores only R15, and Rb is adjusted by 4 * 16 1088 register_bits = 1 << 15; 1089 empty_list = true; 1090 } 1091 1092 let count = register_bits.count_ones(); 1093 let r15_loaded = register_bits.bit(15); 1094 1095 let base_addr = self.read_register(rn); 1096 let count_for_final_addr = if empty_list { 16 } else { count }; 1097 let final_addr = if INCREMENT { 1098 base_addr.wrapping_add(4 * count_for_final_addr) 1099 } else { 1100 base_addr.wrapping_sub(4 * count_for_final_addr) 1101 }; 1102 1103 let mut address = if INCREMENT { base_addr } else { final_addr }; 1104 1105 let mut first = true; 1106 let mut need_write_back = write_back == WriteBack::Yes; 1107 for r in 0..16 { 1108 if !register_bits.bit(r) { 1109 continue; 1110 } 1111 1112 let memory_cycle = if first { 1113 first = false; 1114 MemoryCycle::N 1115 } else { 1116 MemoryCycle::S 1117 }; 1118 1119 if LOAD && need_write_back { 1120 self.registers.r[rn as usize] = final_addr; 1121 log::trace!(" Wrote back to R{rn}: {:08X}", self.registers.r[rn as usize]); 1122 need_write_back = false; 1123 } 1124 1125 if !(AFTER ^ !INCREMENT) { 1126 address = address.wrapping_add(4); 1127 } 1128 1129 if LOAD { 1130 if r == 15 { 1131 self.registers.r[15] = bus.read_word(address, memory_cycle); 1132 1133 if s_bit { 1134 self.spsr_to_cpsr(); 1135 } 1136 } else if s_bit && !r15_loaded { 1137 let register = get_user_register(&mut self.registers, r.into()); 1138 *register = bus.read_word(address, memory_cycle); 1139 } else { 1140 self.registers.r[r as usize] = bus.read_word(address, memory_cycle); 1141 } 1142 log::trace!(" LDM: Loaded R{r} from {address:08X}"); 1143 } else { 1144 let value = if s_bit { 1145 *get_user_register(&mut self.registers, r.into()) 1146 } else { 1147 self.registers.r[r as usize] 1148 }; 1149 bus.write_word(address, value, memory_cycle); 1150 log::trace!(" STM: Stored R{r} to {address:08X}"); 1151 } 1152 1153 if AFTER ^ !INCREMENT { 1154 address = address.wrapping_add(4); 1155 } 1156 1157 if !LOAD && need_write_back { 1158 self.registers.r[rn as usize] = final_addr; 1159 log::trace!(" Wrote back to R{rn}: {:08X}", self.registers.r[rn as usize]); 1160 need_write_back = false; 1161 } 1162 } 1163 1164 if LOAD { 1165 bus.internal_cycles(1); 1166 1167 if r15_loaded { 1168 self.refill_prefetch(bus); 1169 } 1170 } else { 1171 // Next opcode fetch is N 1172 self.fetch_cycle = MemoryCycle::N; 1173 } 1174 } 1175 1176 fn arm_swi(&mut self, _opcode: u32, bus: &mut Bus) { 1177 self.handle_exception(Exception::SoftwareInterrupt, bus); 1178 } 1179 1180 // Format 1: Move shifted register 1181 fn thumb_move_shifted_register(&mut self, opcode: u16, bus: &mut Bus) { 1182 let rd = opcode & 7; 1183 let rs = (opcode >> 3) & 7; 1184 let shift = (opcode >> 6) & 0x1F; 1185 let shift_type = ShiftType::from_bits((opcode >> 11).into()); 1186 1187 self.alu_immediate_shift( 1188 AluOp::Move, 1189 rd.into(), 1190 rd.into(), 1191 true, 1192 rs.into(), 1193 shift_type, 1194 shift.into(), 1195 bus, 1196 ); 1197 } 1198 1199 // Format 2: Add/subtract 1200 fn thumb_add_sub(&mut self, opcode: u16, bus: &mut Bus) { 1201 let rd = opcode & 7; 1202 let rs = (opcode >> 3) & 7; 1203 let rn = (opcode >> 6) & 7; 1204 let alu_op = if opcode.bit(9) { AluOp::Subtract } else { AluOp::Add }; 1205 1206 let immediate = opcode.bit(10); 1207 if immediate { 1208 self.alu_rotated_immediate(alu_op, rs.into(), rd.into(), true, rn.into(), 0, bus); 1209 } else { 1210 self.alu_immediate_shift( 1211 alu_op, 1212 rs.into(), 1213 rd.into(), 1214 true, 1215 rn.into(), 1216 ShiftType::Left, 1217 0, 1218 bus, 1219 ); 1220 } 1221 } 1222 1223 // Format 3: Move/compare/add/subtract immediate 1224 fn thumb_alu_immediate(&mut self, opcode: u16, bus: &mut Bus) { 1225 let immediate = opcode & 0xFF; 1226 let rd = (opcode >> 8) & 7; 1227 let alu_op = match (opcode >> 11) & 3 { 1228 0 => AluOp::Move, 1229 1 => AluOp::Compare, 1230 2 => AluOp::Add, 1231 3 => AluOp::Subtract, 1232 _ => unreachable!("value & 3 is always <= 3"), 1233 }; 1234 1235 self.alu_rotated_immediate(alu_op, rd.into(), rd.into(), true, immediate.into(), 0, bus); 1236 } 1237 1238 // Format 4: ALU operations 1239 fn thumb_alu(&mut self, opcode: u16, bus: &mut Bus) { 1240 let rd = opcode & 7; 1241 let rs = (opcode >> 3) & 7; 1242 1243 let basic_op = match (opcode >> 6) & 0xF { 1244 0x0 => AluOp::And, 1245 0x1 => AluOp::ExclusiveOr, 1246 op @ (0x2..=0x4 | 0x7) => { 1247 // LSL/LSR/ASR/ROR 1248 let shift_type = match op { 1249 2 => ShiftType::Left, 1250 3 => ShiftType::LogicalRight, 1251 4 => ShiftType::ArithmeticRight, 1252 7 => ShiftType::RotateRight, 1253 _ => unreachable!("nested match expressions"), 1254 }; 1255 1256 return self.alu_register_shift::<THUMB_OPCODE_LEN>( 1257 AluOp::Move, 1258 rd.into(), 1259 rd.into(), 1260 true, 1261 rd.into(), 1262 shift_type, 1263 rs.into(), 1264 bus, 1265 ); 1266 } 1267 0x5 => AluOp::AddCarry, 1268 0x6 => AluOp::SubtractCarry, 1269 0x8 => AluOp::Test, 1270 0x9 => { 1271 // NEG 1272 return self.alu_rotated_immediate( 1273 AluOp::ReverseSubtract, 1274 rs.into(), 1275 rd.into(), 1276 true, 1277 0, 1278 0, 1279 bus, 1280 ); 1281 } 1282 0xA => AluOp::Compare, 1283 0xB => AluOp::CompareNegate, 1284 0xC => AluOp::Or, 1285 0xD => { 1286 // MUL 1287 return self.multiply(rs.into(), rd.into(), rd.into(), rd.into(), true, false, bus); 1288 } 1289 0xE => AluOp::BitClear, 1290 0xF => AluOp::MoveNegate, 1291 _ => unreachable!("value & 0xF is always <= 0xF"), 1292 }; 1293 1294 self.alu_immediate_shift( 1295 basic_op, 1296 rd.into(), 1297 rd.into(), 1298 true, 1299 rs.into(), 1300 ShiftType::Left, 1301 0, 1302 bus, 1303 ); 1304 } 1305 1306 // Format 5: Hi register operations / branch exchange 1307 fn thumb_high_register_op(&mut self, opcode: u16, bus: &mut Bus) { 1308 let mut rd = opcode & 7; 1309 let mut rs = (opcode >> 3) & 7; 1310 1311 if opcode.bit(6) { 1312 rs += 8; 1313 } 1314 1315 if opcode.bit(7) { 1316 rd += 8; 1317 } 1318 1319 let alu_op = match (opcode >> 8) & 3 { 1320 0 => AluOp::Add, 1321 1 => AluOp::Compare, 1322 2 => AluOp::Move, 1323 3 => return self.branch_exchange(rs.into(), bus), 1324 _ => unreachable!("value & 3 is always <= 3"), 1325 }; 1326 1327 let set_condition_codes = alu_op == AluOp::Compare; 1328 1329 self.alu_immediate_shift( 1330 alu_op, 1331 rd.into(), 1332 rd.into(), 1333 set_condition_codes, 1334 rs.into(), 1335 ShiftType::Left, 1336 0, 1337 bus, 1338 ); 1339 } 1340 1341 // Format 6: PC-relative load 1342 fn thumb_pc_relative_load(&mut self, opcode: u16, bus: &mut Bus) { 1343 // Bit 1 of PC is forcibly cleared for PC-relative loads 1344 let mut offset: u32 = ((opcode & 0xFF) << 2).into(); 1345 offset = offset.wrapping_sub(self.prev_r15 & 2); 1346 1347 let rd = (opcode >> 8) & 7; 1348 1349 self.load_word::<true>( 1350 15, 1351 rd.into(), 1352 offset, 1353 LoadSize::Word, 1354 LoadIndexing::Pre, 1355 IndexOp::Add, 1356 WriteBack::No, 1357 bus, 1358 ); 1359 } 1360 1361 // Format 7: Load/store with register offset 1362 fn thumb_load_register_offset(&mut self, opcode: u16, bus: &mut Bus) { 1363 let rd = opcode & 7; 1364 let rb = (opcode >> 3) & 7; 1365 let ro = (opcode >> 6) & 7; 1366 let size = LoadSize::from_bit(opcode.bit(10)); 1367 let load = opcode.bit(11); 1368 1369 let offset = self.read_register(ro.into()); 1370 1371 if load { 1372 self.load_word::<true>( 1373 rb.into(), 1374 rd.into(), 1375 offset, 1376 size, 1377 LoadIndexing::Pre, 1378 IndexOp::Add, 1379 WriteBack::No, 1380 bus, 1381 ); 1382 } else { 1383 self.load_word::<false>( 1384 rb.into(), 1385 rd.into(), 1386 offset, 1387 size, 1388 LoadIndexing::Pre, 1389 IndexOp::Add, 1390 WriteBack::No, 1391 bus, 1392 ); 1393 } 1394 } 1395 1396 // Format 8: Load/store sign-extended byte/halfword 1397 fn thumb_load_sign_extended(&mut self, opcode: u16, bus: &mut Bus) { 1398 let rd = opcode & 7; 1399 let rb = (opcode >> 3) & 7; 1400 let ro = (opcode >> 6) & 7; 1401 1402 let offset = self.read_register(ro.into()); 1403 1404 let sh_bits = (opcode >> 10) & 3; 1405 if sh_bits == 0 { 1406 // STRH 1407 self.load_halfword::<false>( 1408 rb.into(), 1409 rd.into(), 1410 offset, 1411 HalfwordLoadType::UnsignedHalfword, 1412 LoadIndexing::Pre, 1413 IndexOp::Add, 1414 WriteBack::No, 1415 bus, 1416 ); 1417 } else { 1418 let load_type = match sh_bits { 1419 1 => HalfwordLoadType::SignedByte, 1420 2 => HalfwordLoadType::UnsignedHalfword, 1421 3 => HalfwordLoadType::SignedHalfword, 1422 _ => unreachable!(), 1423 }; 1424 1425 self.load_halfword::<true>( 1426 rb.into(), 1427 rd.into(), 1428 offset, 1429 load_type, 1430 LoadIndexing::Pre, 1431 IndexOp::Add, 1432 WriteBack::No, 1433 bus, 1434 ); 1435 } 1436 } 1437 1438 // Format 9: Load/store with immediate offset 1439 fn thumb_load_immediate_offset(&mut self, opcode: u16, bus: &mut Bus) { 1440 let rd = opcode & 7; 1441 let rb = (opcode >> 3) & 7; 1442 let mut offset = (opcode >> 6) & 0x1F; 1443 let load = opcode.bit(11); 1444 let size = LoadSize::from_bit(opcode.bit(12)); 1445 1446 if size == LoadSize::Word { 1447 offset <<= 2; 1448 } 1449 1450 if load { 1451 self.load_word::<true>( 1452 rb.into(), 1453 rd.into(), 1454 offset.into(), 1455 size, 1456 LoadIndexing::Pre, 1457 IndexOp::Add, 1458 WriteBack::No, 1459 bus, 1460 ); 1461 } else { 1462 self.load_word::<false>( 1463 rb.into(), 1464 rd.into(), 1465 offset.into(), 1466 size, 1467 LoadIndexing::Pre, 1468 IndexOp::Add, 1469 WriteBack::No, 1470 bus, 1471 ); 1472 } 1473 } 1474 1475 // Format 10: Load/store halfword 1476 fn thumb_load_halfword(&mut self, opcode: u16, bus: &mut Bus) { 1477 let rd = opcode & 7; 1478 let rb = (opcode >> 3) & 7; 1479 let offset = ((opcode >> 6) & 0x1F) << 1; 1480 let load = opcode.bit(11); 1481 1482 if load { 1483 self.load_halfword::<true>( 1484 rb.into(), 1485 rd.into(), 1486 offset.into(), 1487 HalfwordLoadType::UnsignedHalfword, 1488 LoadIndexing::Pre, 1489 IndexOp::Add, 1490 WriteBack::No, 1491 bus, 1492 ); 1493 } else { 1494 self.load_halfword::<false>( 1495 rb.into(), 1496 rd.into(), 1497 offset.into(), 1498 HalfwordLoadType::UnsignedHalfword, 1499 LoadIndexing::Pre, 1500 IndexOp::Add, 1501 WriteBack::No, 1502 bus, 1503 ); 1504 } 1505 } 1506 1507 // Format 11: SP-relative load/store 1508 fn thumb_load_sp_relative(&mut self, opcode: u16, bus: &mut Bus) { 1509 let offset = (opcode & 0xFF) << 2; 1510 let rd = (opcode >> 8) & 7; 1511 let load = opcode.bit(11); 1512 1513 if load { 1514 self.load_word::<true>( 1515 13, 1516 rd.into(), 1517 offset.into(), 1518 LoadSize::Word, 1519 LoadIndexing::Pre, 1520 IndexOp::Add, 1521 WriteBack::No, 1522 bus, 1523 ); 1524 } else { 1525 self.load_word::<false>( 1526 13, 1527 rd.into(), 1528 offset.into(), 1529 LoadSize::Word, 1530 LoadIndexing::Pre, 1531 IndexOp::Add, 1532 WriteBack::No, 1533 bus, 1534 ); 1535 } 1536 } 1537 1538 // Format 12: Load address 1539 fn thumb_load_address(&mut self, opcode: u16, bus: &mut Bus) { 1540 let mut immediate: u32 = ((opcode & 0xFF) << 2).into(); 1541 let rd = (opcode >> 8) & 7; 1542 1543 // Bit 11 selects between SP (R13) and PC (R15) 1544 let rn = if opcode.bit(11) { 13 } else { 15 }; 1545 1546 // Bit 1 is forced clear when PC is used as Rn 1547 if rn == 15 { 1548 immediate = immediate.wrapping_sub(self.prev_r15 & 2); 1549 } 1550 1551 self.alu_rotated_immediate(AluOp::Add, rn, rd.into(), false, immediate, 0, bus); 1552 } 1553 1554 // Format 13: Add offset to stack pointer 1555 fn thumb_add_offset_sp(&mut self, opcode: u16, bus: &mut Bus) { 1556 let mut offset: u32 = ((opcode & 0x7F) << 2).into(); 1557 if opcode.bit(7) { 1558 offset = (!offset).wrapping_add(1); 1559 } 1560 1561 self.alu_rotated_immediate(AluOp::Add, 13, 13, false, offset, 0, bus); 1562 } 1563 1564 // Format 14: Push/pop registers 1565 fn thumb_push_pop(&mut self, opcode: u16, bus: &mut Bus) { 1566 let lr_pc_bit = opcode.bit(8); 1567 let load = opcode.bit(11); 1568 1569 let mut register_bits = opcode & 0xFF; 1570 if lr_pc_bit { 1571 // Store LR, load PC 1572 register_bits |= 1 << (14 + u32::from(load)); 1573 } 1574 1575 if load { 1576 self.load_multiple::<true, true, true>( 1577 register_bits.into(), 1578 13, 1579 WriteBack::Yes, 1580 false, 1581 bus, 1582 ); 1583 } else { 1584 self.load_multiple::<false, false, false>( 1585 register_bits.into(), 1586 13, 1587 WriteBack::Yes, 1588 false, 1589 bus, 1590 ); 1591 } 1592 } 1593 1594 // Format 15: Multiple load/store 1595 fn thumb_load_multiple(&mut self, opcode: u16, bus: &mut Bus) { 1596 let register_bits = opcode & 0xFF; 1597 let rb = (opcode >> 8) & 7; 1598 let load = opcode.bit(11); 1599 1600 if load { 1601 self.load_multiple::<true, true, true>( 1602 register_bits.into(), 1603 rb.into(), 1604 WriteBack::Yes, 1605 false, 1606 bus, 1607 ); 1608 } else { 1609 self.load_multiple::<false, true, true>( 1610 register_bits.into(), 1611 rb.into(), 1612 WriteBack::Yes, 1613 false, 1614 bus, 1615 ); 1616 } 1617 } 1618 1619 // Format 16: Conditional branch 1620 fn thumb_conditional_branch(&mut self, opcode: u16, bus: &mut Bus) { 1621 let condition = Condition::from_bits((opcode >> 8).into()); 1622 if !condition.check(self.registers.cpsr) { 1623 return; 1624 } 1625 1626 let offset = i32::from(opcode as i8) << 1; 1627 self.branch::<false, THUMB_OPCODE_LEN>(offset, bus); 1628 } 1629 1630 // Format 17: Software interrupt 1631 fn thumb_software_interrupt(&mut self, _opcode: u16, bus: &mut Bus) { 1632 self.handle_exception(Exception::SoftwareInterrupt, bus); 1633 } 1634 1635 // Format 18: Unconditional branch 1636 fn thumb_unconditional_branch(&mut self, opcode: u16, bus: &mut Bus) { 1637 let offset = (i32::from(opcode & 0x7FF) << 21) >> 20; 1638 self.branch::<false, THUMB_OPCODE_LEN>(offset, bus); 1639 } 1640 1641 // Format 19: Long branch with link 1642 // This instruction has no ARM equivalent 1643 fn thumb_long_branch(&mut self, opcode: u16, bus: &mut Bus) { 1644 // Offset is a signed 23-bit value split across two opcodes, with 11 bits in each 1645 // First opcode has H=0 and second opcode has H=1 1646 // It is possible to have an H=1 opcode without an H=0 opcode; Golden Sun: The Lost Age does this 1647 if !opcode.bit(11) { 1648 // First opcode: Write highest 11 bits of jump address to LR 1649 let unsigned_offset = i32::from(opcode & 0x7FF) << 12; 1650 1651 // Clip to signed 23-bit 1652 let offset = (unsigned_offset << 9) >> 9; 1653 1654 self.registers.r[14] = self.prev_r15.wrapping_add_signed(offset); 1655 } else { 1656 // Second opcode: Add lowest 11 bits of jump address to LR, jump, and write return address to LR 1657 let offset_low = u32::from(opcode & 0x7FF) << 1; 1658 let jump_address = self.registers.r[14].wrapping_add(offset_low); 1659 1660 let return_address = self.prev_r15.wrapping_sub(2); 1661 self.registers.r[14] = return_address | 1; 1662 1663 self.registers.r[15] = jump_address; 1664 self.refill_prefetch(bus); 1665 } 1666 } 1667} 1668 1669#[inline] 1670fn arm_parse_rotated_immediate(opcode: u32) -> (u32, u32) { 1671 let immediate = opcode & 0xFF; 1672 let rotation = ((opcode >> 8) & 0xF) << 1; 1673 (immediate, rotation) 1674} 1675 1676fn apply_immediate_shift( 1677 value: u32, 1678 shift_type: ShiftType, 1679 shift: u32, 1680 carry_in: bool, 1681) -> (u32, bool) { 1682 match (shift_type, shift) { 1683 (ShiftType::Left, 0) => (value, carry_in), 1684 (ShiftType::Left, _) => (value << shift, value.bit((32 - shift) as u8)), 1685 (ShiftType::LogicalRight, 0) => (0, value.bit(31)), 1686 (ShiftType::LogicalRight, _) => (value >> shift, value.bit((shift - 1) as u8)), 1687 (ShiftType::ArithmeticRight, 0) => (((value as i32) >> 31) as u32, value.bit(31)), 1688 (ShiftType::ArithmeticRight, _) => { 1689 (((value as i32) >> shift) as u32, value.bit((shift - 1) as u8)) 1690 } 1691 (ShiftType::RotateRight, 0) => { 1692 // RRX: Rotate right through carry 1693 let result = (value >> 1) | (u32::from(carry_in) << 31); 1694 (result, value.bit(0)) 1695 } 1696 (ShiftType::RotateRight, _) => (value.rotate_right(shift), value.bit((shift - 1) as u8)), 1697 } 1698} 1699 1700fn alu_add(operand1: u32, operand2: u32, carry_in: bool) -> (u32, ConditionCodes) { 1701 let carry_in: u32 = carry_in.into(); 1702 1703 let (partial_sum, carry1) = operand1.overflowing_add(operand2); 1704 let (sum, carry2) = partial_sum.overflowing_add(carry_in); 1705 let carry = carry1 || carry2; 1706 1707 let bit_30_carry = (operand1 & 0x7FFFFFFF) + (operand2 & 0x7FFFFFFF) + carry_in >= 0x80000000; 1708 let overflow = bit_30_carry != carry; 1709 1710 (sum, ConditionCodes { sign: sum.bit(31), zero: sum == 0, carry, overflow }) 1711} 1712 1713fn get_user_register(registers: &mut Registers, r: u32) -> &mut u32 { 1714 match (registers.cpsr.mode, r) { 1715 (CpuMode::User | CpuMode::System, _) => &mut registers.r[r as usize], 1716 (CpuMode::Fiq, 8..=12) => &mut registers.other_r_8_12[(r - 8) as usize], 1717 (_, 13) => &mut registers.r13_usr, 1718 (_, 14) => &mut registers.r14_usr, 1719 _ => &mut registers.r[r as usize], 1720 } 1721}