From 4aa63e318bf3e3aed3a8197b82231524b2ea400c Mon Sep 17 00:00:00 2001 From: Rekai Musuka Date: Wed, 29 Jun 2022 06:33:17 -0300 Subject: [PATCH] feat: implement basic pipeline passes arm.gba, thumb.gb and armwrestler, fails in actual games TODO: run FuzzARM debug specific titles --- src/core/cpu.zig | 82 ++++++++++++++++--- src/core/cpu/arm/block_data_transfer.zig | 18 ++-- src/core/cpu/arm/branch.zig | 13 ++- src/core/cpu/arm/data_processing.zig | 41 ++++------ .../cpu/arm/half_signed_data_transfer.zig | 16 +--- src/core/cpu/arm/single_data_transfer.zig | 25 +++--- src/core/cpu/arm/software_interrupt.zig | 5 +- src/core/cpu/barrel_shifter.zig | 8 +- src/core/cpu/thumb/block_data_transfer.zig | 11 ++- src/core/cpu/thumb/branch.zig | 26 +++--- src/core/cpu/thumb/data_processing.zig | 5 +- src/core/cpu/thumb/data_transfer.zig | 4 +- src/core/cpu/thumb/software_interrupt.zig | 5 +- 13 files changed, 159 insertions(+), 100 deletions(-) diff --git a/src/core/cpu.zig b/src/core/cpu.zig index 9b579f1..d7395a4 100644 --- a/src/core/cpu.zig +++ b/src/core/cpu.zig @@ -125,7 +125,7 @@ pub const thumb = struct { const swi = @import("cpu/thumb/software_interrupt.zig").fmt17; const branch = @import("cpu/thumb/branch.zig"); - /// Determine index into THUMB InstrFn LUT + /// Determine index into THUMB InstrFn LUT fn idx(opcode: u16) u10 { return @truncate(u10, opcode >> 6); } @@ -243,6 +243,7 @@ pub const Arm7tdmi = struct { const Self = @This(); r: [16]u32, + pipe: Pipline, sched: *Scheduler, bus: *Bus, cpsr: PSR, @@ -263,6 +264,7 @@ pub const Arm7tdmi = struct { pub fn init(sched: *Scheduler, bus: *Bus, log_file: ?std.fs.File) Self { return Self{ .r = [_]u32{0x00} ** 16, + .pipe = Pipline.init(), .sched = sched, .bus = bus, .cpsr = .{ .raw = 0x0000_001F }, @@ -322,8 +324,21 @@ pub const Arm7tdmi = struct { return self.bus.io.haltcnt == .Halt; } + pub fn setCpsrNoFlush(self: *Self, value: u32) void { + if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeModeFromIdx(@truncate(u5, value & 0x1F)); + self.cpsr.raw = value; + } + pub fn setCpsr(self: *Self, value: u32) void { if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeModeFromIdx(@truncate(u5, value & 0x1F)); + + const new: PSR = .{ .raw = value }; + if (self.cpsr.t.read() != new.t.read()) { + // If THUMB to ARM or ARM to THUMB, flush pipeline + self.r[15] &= if (new.t.read()) ~@as(u32, 1) else ~@as(u32, 3); + self.pipe.flush(); + } + self.cpsr.raw = value; } @@ -426,19 +441,22 @@ pub const Arm7tdmi = struct { } pub fn step(self: *Self) void { - if (self.cpsr.t.read()) { - const opcode = self.fetch(u16); + if (self.cpsr.t.read()) blk: { + const opcode = @truncate(u16, self.pipe.step(self, u16) orelse break :blk); if (cpu_logging) self.logger.?.mgbaLog(self, opcode); thumb.lut[thumb.idx(opcode)](self, self.bus, opcode); - } else { - const opcode = self.fetch(u32); + } else blk: { + const opcode = self.pipe.step(self, u32) orelse break :blk; if (cpu_logging) self.logger.?.mgbaLog(self, opcode); if (checkCond(self.cpsr, @truncate(u4, opcode >> 28))) { arm.lut[arm.idx(opcode)](self, self.bus, opcode); } } + + if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4); + self.pipe.flushed = false; } pub fn stepDmaTransfer(self: *Self) bool { @@ -482,8 +500,8 @@ pub const Arm7tdmi = struct { if (!self.bus.io.ime or self.cpsr.i.read()) return; // log.debug("An interrupt was Handled!", .{}); - // retAddr.gba says r15 on it's own is off by -04h in both ARM and THUMB mode - const r15 = self.r[15] + 4; + // FIXME: Is the return address ahead? + const r15 = self.r[15]; const cpsr = self.cpsr.raw; self.changeMode(.Irq); @@ -507,8 +525,12 @@ pub const Arm7tdmi = struct { return self.bus.read(T, self.r[15]); } - pub fn fakePC(self: *const Self) u32 { - return self.r[15] + 4; + fn debug_log(self: *const Self, file: *const File, opcode: u32) void { + if (self.binary_log) { + self.skyLog(file) catch unreachable; + } else { + self.mgbaLog(file, opcode) catch unreachable; + } } pub fn panic(self: *const Self, comptime format: []const u8, args: anytype) noreturn { @@ -588,7 +610,7 @@ pub const Arm7tdmi = struct { const r12 = self.r[12]; const r13 = self.r[13]; const r14 = self.r[14]; - const r15 = self.r[15]; + const r15 = self.r[15] -| if (self.cpsr.t.read()) 2 else @as(u32, 4); const c_psr = self.cpsr.raw; @@ -596,7 +618,7 @@ pub const Arm7tdmi = struct { if (self.cpsr.t.read()) { if (opcode >> 11 == 0x1E) { // Instruction 1 of a BL Opcode, print in ARM mode - const other_half = self.bus.dbgRead(u16, self.r[15]); + const other_half = self.bus.debugRead(u16, self.r[15] - 2); const bl_opcode = @as(u32, opcode) << 16 | other_half; log_str = try std.fmt.bufPrint(&buf, arm_fmt, .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr, bl_opcode }); @@ -632,6 +654,44 @@ pub fn checkCond(cpsr: PSR, cond: u4) bool { }; } +const Pipline = struct { + const Self = @This(); + stage: [2]?u32, + flushed: bool, + + fn init() Self { + return .{ + .stage = [_]?u32{null} ** 2, + .flushed = false, + }; + } + + pub fn flush(self: *Self) void { + for (self.stage) |*opcode| opcode.* = null; + self.flushed = true; + } + + pub fn step(self: *Self, cpu: *Arm7tdmi, comptime T: type) ?u32 { + comptime std.debug.assert(T == u32 or T == u16); + + const opcode = self.stage[0]; + + self.stage[0] = self.stage[1]; + self.stage[1] = cpu.bus.read(T, cpu.r[15]); + + return opcode; + } + + fn reload(self: *Self, cpu: *Arm7tdmi, comptime T: type) void { + comptime std.debug.assert(T == u32 or T == u16); + const inc = if (T == u32) 4 else 2; + + self.stage[0] = cpu.bus.read(T, cpu.r[15]); + self.stage[1] = cpu.bus.read(T, cpu.r[15] + inc); + cpu.r[15] += inc * 2; + } +}; + pub const PSR = extern union { mode: Bitfield(u32, 0, 5), t: Bit(u32, 5), diff --git a/src/core/cpu/arm/block_data_transfer.zig b/src/core/cpu/arm/block_data_transfer.zig index 637cc0a..6265e88 100644 --- a/src/core/cpu/arm/block_data_transfer.zig +++ b/src/core/cpu/arm/block_data_transfer.zig @@ -55,8 +55,10 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c if (L) { cpu.r[15] = bus.read(u32, und_addr); + cpu.pipe.flush(); } else { - bus.write(u32, und_addr, cpu.r[15] + 8); + // FIXME: Should r15 on write be +12 ahead? + bus.write(u32, und_addr, cpu.r[15] + 4); } cpu.r[rn] = if (U) cpu.r[rn] + 0x40 else cpu.r[rn] - 0x40; @@ -86,17 +88,23 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c cpu.setUserModeRegister(i, bus.read(u32, address)); } else { const value = bus.read(u32, address); - cpu.r[i] = if (i == 0xF) value & 0xFFFF_FFFC else value; - if (S and i == 0xF) cpu.setCpsr(cpu.spsr.raw); + + cpu.r[i] = value; + if (i == 0xF) { + cpu.r[i] &= ~@as(u32, 3); // Align r15 + cpu.pipe.flush(); + + if (S) cpu.setCpsr(cpu.spsr.raw); + } } } else { if (S) { // Always Transfer User mode Registers // This happens regardless if r15 is in the list const value = cpu.getUserModeRegister(i); - bus.write(u32, address, value + if (i == 0xF) 8 else @as(u32, 0)); // PC is already 4 ahead to make 12 + bus.write(u32, address, value + if (i == 0xF) 4 else @as(u32, 0)); // PC is already 8 ahead to make 12 } else { - bus.write(u32, address, cpu.r[i] + if (i == 0xF) 8 else @as(u32, 0)); + bus.write(u32, address, cpu.r[i] + if (i == 0xF) 4 else @as(u32, 0)); } } } diff --git a/src/core/cpu/arm/branch.zig b/src/core/cpu/arm/branch.zig index 31a48f0..039b3d9 100644 --- a/src/core/cpu/arm/branch.zig +++ b/src/core/cpu/arm/branch.zig @@ -9,14 +9,19 @@ const sext = @import("../../util.zig").sext; pub fn branch(comptime L: bool) InstrFn { return struct { fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void { - if (L) cpu.r[14] = cpu.r[15]; - cpu.r[15] = cpu.fakePC() +% (sext(u32, u24, opcode) << 2); + if (L) cpu.r[14] = cpu.r[15] - 4; + + cpu.r[15] +%= sext(u32, u24, opcode) << 2; + cpu.pipe.flush(); } }.inner; } pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void { const rn = opcode & 0xF; - cpu.cpsr.t.write(cpu.r[rn] & 1 == 1); - cpu.r[15] = cpu.r[rn] & 0xFFFF_FFFE; + + const thumb = cpu.r[rn] & 1 == 1; + cpu.r[15] = cpu.r[rn] & if (thumb) ~@as(u32, 1) else ~@as(u32, 3); + cpu.cpsr.t.write(thumb); + cpu.pipe.flush(); } diff --git a/src/core/cpu/arm/data_processing.zig b/src/core/cpu/arm/data_processing.zig index 2a3b77d..547e753 100644 --- a/src/core/cpu/arm/data_processing.zig +++ b/src/core/cpu/arm/data_processing.zig @@ -13,17 +13,12 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4 const old_carry = @boolToInt(cpu.cpsr.c.read()); // If certain conditions are met, PC is 12 ahead instead of 8 + // TODO: What are these conditions? I can't remember if (!I and opcode >> 4 & 1 == 1) cpu.r[15] += 4; + const op1 = cpu.r[rn]; - const op1 = if (rn == 0xF) cpu.fakePC() else cpu.r[rn]; - - var op2: u32 = undefined; - if (I) { - const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1); - op2 = rotateRight(S, &cpu.cpsr, opcode & 0xFF, amount); - } else { - op2 = execute(S, cpu, opcode); - } + const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1); + const op2 = if (I) rotateRight(S, &cpu.cpsr, opcode & 0xFF, amount) else execute(S, cpu, opcode); // Undo special condition from above if (!I and opcode >> 4 & 1 == 1) cpu.r[15] -= 4; @@ -67,39 +62,31 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4 }, 0x8 => { // TST - if (rd == 0xF) { - undefinedTestBehaviour(cpu); - return; - } + if (rd == 0xF) + return undefinedTestBehaviour(cpu); const result = op1 & op2; setTestOpFlags(S, cpu, opcode, result); }, 0x9 => { // TEQ - if (rd == 0xF) { - undefinedTestBehaviour(cpu); - return; - } + if (rd == 0xF) + return undefinedTestBehaviour(cpu); const result = op1 ^ op2; setTestOpFlags(S, cpu, opcode, result); }, 0xA => { // CMP - if (rd == 0xF) { - undefinedTestBehaviour(cpu); - return; - } + if (rd == 0xF) + return undefinedTestBehaviour(cpu); cmp(cpu, op1, op2); }, 0xB => { // CMN - if (rd == 0xF) { - undefinedTestBehaviour(cpu); - return; - } + if (rd == 0xF) + return undefinedTestBehaviour(cpu); cmn(cpu, op1, op2); }, @@ -127,6 +114,8 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4 setArmLogicOpFlags(S, cpu, rd, result); }, } + + if (rd == 0xF) cpu.pipe.flush(); } }.inner; } @@ -280,5 +269,5 @@ fn setTestOpFlags(comptime S: bool, cpu: *Arm7tdmi, opcode: u32, result: u32) vo fn undefinedTestBehaviour(cpu: *Arm7tdmi) void { @setCold(true); - cpu.setCpsr(cpu.spsr.raw); + cpu.setCpsrNoFlush(cpu.spsr.raw); } diff --git a/src/core/cpu/arm/half_signed_data_transfer.zig b/src/core/cpu/arm/half_signed_data_transfer.zig index d0a4b1d..11a5c14 100644 --- a/src/core/cpu/arm/half_signed_data_transfer.zig +++ b/src/core/cpu/arm/half_signed_data_transfer.zig @@ -15,20 +15,8 @@ pub fn halfAndSignedDataTransfer(comptime P: bool, comptime U: bool, comptime I: const rm = opcode & 0xF; const imm_offset_high = opcode >> 8 & 0xF; - var base: u32 = undefined; - if (rn == 0xF) { - base = cpu.fakePC(); - if (!L) base += 4; - } else { - base = cpu.r[rn]; - } - - var offset: u32 = undefined; - if (I) { - offset = imm_offset_high << 4 | rm; - } else { - offset = cpu.r[rm]; - } + const base = cpu.r[rn] + if (!L and rn == 0xF) 4 else @as(u32, 0); + const offset = if (I) imm_offset_high << 4 | rm else cpu.r[rm]; const modified_base = if (U) base +% offset else base -% offset; var address = if (P) modified_base else base; diff --git a/src/core/cpu/arm/single_data_transfer.zig b/src/core/cpu/arm/single_data_transfer.zig index 794e610..f1d6cca 100644 --- a/src/core/cpu/arm/single_data_transfer.zig +++ b/src/core/cpu/arm/single_data_transfer.zig @@ -14,13 +14,8 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool, const rn = opcode >> 16 & 0xF; const rd = opcode >> 12 & 0xF; - var base: u32 = undefined; - if (rn == 0xF) { - base = cpu.fakePC(); - if (!L) base += 4; // Offset of 12 - } else { - base = cpu.r[rn]; - } + // rn is r15 and L is not set, the PC is 12 ahead + const base = cpu.r[rn] + if (!L and rn == 0xF) 4 else @as(u32, 0); const offset = if (I) shifter.immShift(false, cpu, opcode) else opcode & 0xFFF; @@ -40,18 +35,26 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool, } else { if (B) { // STRB - const value = if (rd == 0xF) cpu.r[rd] + 8 else cpu.r[rd]; + const value = cpu.r[rd] + if (rd == 0xF) 4 else @as(u32, 0); // PC is 12 ahead bus.write(u8, address, @truncate(u8, value)); } else { // STR - const value = if (rd == 0xF) cpu.r[rd] + 8 else cpu.r[rd]; + const value = cpu.r[rd] + if (rd == 0xF) 4 else @as(u32, 0); bus.write(u32, address, value); } } address = modified_base; - if (W and P or !P) cpu.r[rn] = address; - if (L) cpu.r[rd] = result; // This emulates the LDR rd == rn behaviour + if (W and P or !P) { + cpu.r[rn] = address; + if (rn == 0xF) cpu.pipe.flush(); + } + + if (L) { + // This emulates the LDR rd == rn behaviour + cpu.r[rd] = result; + if (rd == 0xF) cpu.pipe.flush(); + } } }.inner; } diff --git a/src/core/cpu/arm/software_interrupt.zig b/src/core/cpu/arm/software_interrupt.zig index 849a6eb..8dd1c0b 100644 --- a/src/core/cpu/arm/software_interrupt.zig +++ b/src/core/cpu/arm/software_interrupt.zig @@ -6,7 +6,7 @@ pub fn armSoftwareInterrupt() InstrFn { return struct { fn inner(cpu: *Arm7tdmi, _: *Bus, _: u32) void { // Copy Values from Current Mode - const r15 = cpu.r[15]; + const ret_addr = cpu.r[15] - 4; const cpsr = cpu.cpsr.raw; // Switch Mode @@ -14,9 +14,10 @@ pub fn armSoftwareInterrupt() InstrFn { cpu.cpsr.t.write(false); // Force ARM Mode cpu.cpsr.i.write(true); // Disable normal interrupts - cpu.r[14] = r15; // Resume Execution + cpu.r[14] = ret_addr; // Resume Execution cpu.spsr.raw = cpsr; // Previous mode CPSR cpu.r[15] = 0x0000_0008; + cpu.pipe.flush(); } }.inner; } diff --git a/src/core/cpu/barrel_shifter.zig b/src/core/cpu/barrel_shifter.zig index 7b2c1b8..fa648b5 100644 --- a/src/core/cpu/barrel_shifter.zig +++ b/src/core/cpu/barrel_shifter.zig @@ -18,11 +18,9 @@ pub fn execute(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 { fn registerShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 { const rs_idx = opcode >> 8 & 0xF; + const rm = cpu.r[opcode & 0xF]; const rs = @truncate(u8, cpu.r[rs_idx]); - const rm_idx = opcode & 0xF; - const rm = if (rm_idx == 0xF) cpu.fakePC() else cpu.r[rm_idx]; - return switch (@truncate(u2, opcode >> 5)) { 0b00 => logicalLeft(S, &cpu.cpsr, rm, rs), 0b01 => logicalRight(S, &cpu.cpsr, rm, rs), @@ -33,9 +31,7 @@ fn registerShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 { pub fn immShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 { const amount = @truncate(u8, opcode >> 7 & 0x1F); - - const rm_idx = opcode & 0xF; - const rm = if (rm_idx == 0xF) cpu.fakePC() else cpu.r[rm_idx]; + const rm = cpu.r[opcode & 0xF]; var result: u32 = undefined; if (amount == 0) { diff --git a/src/core/cpu/thumb/block_data_transfer.zig b/src/core/cpu/thumb/block_data_transfer.zig index c2df7e6..25173cd 100644 --- a/src/core/cpu/thumb/block_data_transfer.zig +++ b/src/core/cpu/thumb/block_data_transfer.zig @@ -33,7 +33,8 @@ pub fn fmt14(comptime L: bool, comptime R: bool) InstrFn { if (R) { if (L) { const value = bus.read(u32, address); - cpu.r[15] = value & 0xFFFF_FFFE; + cpu.r[15] = value & ~@as(u32, 1); + cpu.pipe.flush(); } else { bus.write(u32, address, cpu.r[14]); } @@ -52,7 +53,13 @@ pub fn fmt15(comptime L: bool, comptime rb: u3) InstrFn { const end_address = cpu.r[rb] + 4 * countRlist(opcode); if (opcode & 0xFF == 0) { - if (L) cpu.r[15] = bus.read(u32, address) else bus.write(u32, address, cpu.r[15] + 4); + if (L) { + cpu.r[15] = bus.read(u32, address); + cpu.pipe.flush(); + } else { + bus.write(u32, address, cpu.r[15] + 2); + } + cpu.r[rb] += 0x40; return; } diff --git a/src/core/cpu/thumb/branch.zig b/src/core/cpu/thumb/branch.zig index 015137a..235869d 100644 --- a/src/core/cpu/thumb/branch.zig +++ b/src/core/cpu/thumb/branch.zig @@ -9,16 +9,13 @@ pub fn fmt16(comptime cond: u4) InstrFn { return struct { fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void { // B - const offset = sext(u32, u8, opcode & 0xFF) << 1; + if (cond == 0xE or cond == 0xF) + cpu.panic("[CPU/THUMB.16] Undefined conditional branch with condition {}", .{cond}); - const should_execute = switch (cond) { - 0xE, 0xF => cpu.panic("[CPU/THUMB.16] Undefined conditional branch with condition {}", .{cond}), - else => checkCond(cpu.cpsr, cond), - }; + if (!checkCond(cpu.cpsr, cond)) return; - if (should_execute) { - cpu.r[15] = (cpu.r[15] + 2) +% offset; - } + cpu.r[15] +%= sext(u32, u8, opcode & 0xFF) << 1; + cpu.pipe.flush(); } }.inner; } @@ -27,8 +24,8 @@ pub fn fmt18() InstrFn { return struct { // B but conditional fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void { - const offset = sext(u32, u11, opcode & 0x7FF) << 1; - cpu.r[15] = (cpu.r[15] + 2) +% offset; + cpu.r[15] +%= sext(u32, u11, opcode & 0x7FF) << 1; + cpu.pipe.flush(); } }.inner; } @@ -41,13 +38,16 @@ pub fn fmt19(comptime is_low: bool) InstrFn { if (is_low) { // Instruction 2 - const old_pc = cpu.r[15]; + const next_opcode = cpu.r[15] - 2; cpu.r[15] = cpu.r[14] +% (offset << 1); - cpu.r[14] = old_pc | 1; + cpu.r[14] = next_opcode | 1; + + cpu.pipe.flush(); } else { // Instruction 1 - cpu.r[14] = (cpu.r[15] + 2) +% (sext(u32, u11, offset) << 12); + const lr_offset = sext(u32, u11, offset) << 12; + cpu.r[14] = (cpu.r[15] +% lr_offset) & ~@as(u32, 1); } } }.inner; diff --git a/src/core/cpu/thumb/data_processing.zig b/src/core/cpu/thumb/data_processing.zig index d352147..d449f39 100644 --- a/src/core/cpu/thumb/data_processing.zig +++ b/src/core/cpu/thumb/data_processing.zig @@ -133,10 +133,9 @@ pub fn fmt12(comptime isSP: bool, comptime rd: u3) InstrFn { return struct { fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void { // ADD - const left = if (isSP) cpu.r[13] else (cpu.r[15] + 2) & 0xFFFF_FFFD; + const left = if (isSP) cpu.r[13] else cpu.r[15] & ~@as(u32, 2); const right = (opcode & 0xFF) << 2; - const result = left + right; - cpu.r[rd] = result; + cpu.r[rd] = left + right; } }.inner; } diff --git a/src/core/cpu/thumb/data_transfer.zig b/src/core/cpu/thumb/data_transfer.zig index 481e3e5..c5a3fae 100644 --- a/src/core/cpu/thumb/data_transfer.zig +++ b/src/core/cpu/thumb/data_transfer.zig @@ -11,7 +11,9 @@ pub fn fmt6(comptime rd: u3) InstrFn { fn inner(cpu: *Arm7tdmi, bus: *Bus, opcode: u16) void { // LDR const offset = (opcode & 0xFF) << 2; - cpu.r[rd] = bus.read(u32, (cpu.r[15] + 2 & 0xFFFF_FFFD) + offset); + + // Bit 1 of the PC intentionally ignored + cpu.r[rd] = bus.read(u32, (cpu.r[15] & ~@as(u32, 2)) + offset); } }.inner; } diff --git a/src/core/cpu/thumb/software_interrupt.zig b/src/core/cpu/thumb/software_interrupt.zig index abb69ba..d80957a 100644 --- a/src/core/cpu/thumb/software_interrupt.zig +++ b/src/core/cpu/thumb/software_interrupt.zig @@ -6,7 +6,7 @@ pub fn fmt17() InstrFn { return struct { fn inner(cpu: *Arm7tdmi, _: *Bus, _: u16) void { // Copy Values from Current Mode - const r15 = cpu.r[15]; + const ret_addr = cpu.r[15] - 2; const cpsr = cpu.cpsr.raw; // Switch Mode @@ -14,9 +14,10 @@ pub fn fmt17() InstrFn { cpu.cpsr.t.write(false); // Force ARM Mode cpu.cpsr.i.write(true); // Disable normal interrupts - cpu.r[14] = r15; // Resume Execution + cpu.r[14] = ret_addr; // Resume Execution cpu.spsr.raw = cpsr; // Previous mode CPSR cpu.r[15] = 0x0000_0008; + cpu.pipe.flush(); } }.inner; }