diff --git a/src/cpu.zig b/src/cpu.zig index f2ccbd1..6f0c526 100644 --- a/src/cpu.zig +++ b/src/cpu.zig @@ -33,6 +33,17 @@ pub const Arm7tdmi = struct { sched: *Scheduler, bus: *Bus, cpsr: PSR, + spsr: PSR, + + /// Storage for R8_fiq -> R12_fiq and their normal counterparts + /// e.g [r[0 + 8], fiq_r[0 + 8], r[1 + 8], fiq_r[1 + 8]...] + banked_fiq: [2 * 5]u32, + + /// Storage for r13_, r14_ + /// e.g. [r13, r14, r13_svc, r14_svc] + banked_r: [2 * 6]u32, + + banked_spsr: [5]PSR, pub fn init(sched: *Scheduler, bus: *Bus) Self { return .{ @@ -40,9 +51,100 @@ pub const Arm7tdmi = struct { .sched = sched, .bus = bus, .cpsr = .{ .raw = 0x0000_00DF }, + .spsr = .{ .raw = 0x0000_0000 }, + .banked_fiq = [_]u32{0x00} ** 10, + .banked_r = [_]u32{0x00} ** 12, + .banked_spsr = [_]PSR{.{ .raw = 0x0000_0000 }} ** 5, }; } + fn bankedIdx(mode: Mode) usize { + return switch (mode) { + .User, .System => 0, + .Supervisor => 1, + .Abort => 2, + .Undefined => 3, + .IRQ => 4, + .FIQ => 5, + }; + } + + fn spsrIdx(mode: Mode) usize { + return switch (mode) { + .Supervisor => 0, + .Abort => 1, + .Undefined => 2, + .IRQ => 3, + .FIQ => 4, + else => std.debug.panic("{} does not have a SPSR Register", .{mode}), + }; + } + + pub fn hasSPSR(self: *const Self) bool { + return switch (getMode(self.cpsr.mode.read())) { + .System, .User => false, + else => true, + }; + } + + pub fn isPrivileged(self: *const Self) bool { + return switch (getMode(self.cpsr.mode.read())) { + .User => false, + else => true, + }; + } + + pub fn setCpsr(self: *Self, value: u32) void { + if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeMode(@truncate(u5, value & 0x1F)); + self.cpsr.raw = value; + } + + fn changeMode(self: *Self, next_idx: u5) void { + const next = getMode(next_idx); + const now = getMode(self.cpsr.mode.read()); + + // Bank R8 -> r12 + var r: usize = 8; + while (r <= 12) : (r += 1) { + self.banked_fiq[(r - 8) * 2 + if (now == .FIQ) @as(usize, 1) else 0] = self.r[r]; + } + + // Bank r13, r14, SPSR + switch (now) { + .User, .System => { + self.banked_r[bankedIdx(now) * 2 + 0] = self.r[13]; + self.banked_r[bankedIdx(now) * 2 + 1] = self.r[14]; + }, + else => { + self.banked_r[bankedIdx(now) * 2 + 0] = self.r[13]; + self.banked_r[bankedIdx(now) * 2 + 1] = self.r[14]; + self.banked_spsr[spsrIdx(now)] = self.spsr; + }, + } + + // Grab R8 -> R12 + r = 8; + while (r <= 12) : (r += 1) { + self.r[r] = self.banked_fiq[(r - 8) * 2 + if (next == .FIQ) @as(usize, 1) else 0]; + } + + // Grab r13, r14, SPSR + switch (next) { + .User, .System => { + self.r[13] = self.banked_r[bankedIdx(next) * 2 + 0]; + self.r[14] = self.banked_r[bankedIdx(next) * 2 + 1]; + // FIXME: Should we clear out SPSR? + }, + else => { + self.r[13] = self.banked_r[bankedIdx(next) * 2 + 0]; + self.r[14] = self.banked_r[bankedIdx(next) * 2 + 1]; + self.spsr = self.banked_spsr[spsrIdx(next)]; + }, + } + + self.cpsr.mode.write(next_idx); + } + pub fn skipBios(self: *Self) void { self.r[0] = 0x08000000; self.r[1] = 0x000000EA; @@ -112,9 +214,9 @@ pub const Arm7tdmi = struct { const r14 = self.r[14]; const r15 = self.r[15]; - const cpsr = self.cpsr.raw; + const c_psr = self.cpsr.raw; - nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | ", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, cpsr }) catch return; + nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | ", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr }) catch return; nosuspend if (self.cpsr.t.read()) stderr.print("{X:0>4}:\n", .{@truncate(u16, opcode)}) catch return else stderr.print("{X:0>8}:\n", .{opcode}) catch return; } }; @@ -274,6 +376,10 @@ const Mode = enum(u5) { System = 0b11111, }; +pub fn getMode(bits: u5) Mode { + return std.meta.intToEnum(Mode, bits) catch unreachable; +} + fn armUndefined(_: *Arm7tdmi, _: *Bus, opcode: u32) void { const id = armIdx(opcode); std.debug.panic("[CPU:ARM] ID: 0x{X:0>3} 0x{X:0>8} is an illegal opcode", .{ id, opcode }); diff --git a/src/cpu/arm/psr_transfer.zig b/src/cpu/arm/psr_transfer.zig index e669d54..08f7282 100644 --- a/src/cpu/arm/psr_transfer.zig +++ b/src/cpu/arm/psr_transfer.zig @@ -13,32 +13,21 @@ pub fn psrTransfer(comptime I: bool, comptime R: bool, comptime kind: u2) InstrF // MRS const rd = opcode >> 12 & 0xF; - if (R) { - std.debug.panic("[CPU/PSR Transfer] TODO: MRS on SPSR_ is unimplemented", .{}); - } else { - cpu.r[rd] = cpu.cpsr.raw; - } + if (R and !cpu.hasSPSR()) std.log.warn("[CPU/PSR Transfer] Tried to read SPSR from User/System Mode", .{}); + cpu.r[rd] = if (R) cpu.spsr.raw else cpu.cpsr.raw; }, 0b10 => { // MSR const field_mask = @truncate(u4, opcode >> 16 & 0xF); + const rm_idx = opcode & 0xF; + const right = if (I) std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1) else cpu.r[rm_idx]; - if (I) { - const imm = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1); + if (R and !cpu.hasSPSR()) std.log.warn("[CPU/PSR Transfer] Tried to write to SPSR User/System Mode", .{}); - if (R) { - std.debug.panic("[CPU/PSR Transfer] TODO: MSR (flags only) on SPSR_ is unimplemented", .{}); - } else { - cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, imm); - } + if (R) { + if (cpu.isPrivileged()) cpu.spsr.raw = fieldMask(&cpu.spsr, field_mask, right); } else { - const rm_idx = opcode & 0xF; - - if (R) { - std.debug.panic("[CPU/PSR Transfer] TODO: MSR on SPSR_ is unimplemented", .{}); - } else { - cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, cpu.r[rm_idx]); - } + if (cpu.isPrivileged()) cpu.setCpsr(fieldMask(&cpu.cpsr, field_mask, right)); } }, else => std.debug.panic("[CPU/PSR Transfer] Bits 21:220 of {X:0>8} are undefined", .{opcode}),