diff --git a/src/cpu.zig b/src/cpu.zig index f21b994..5c9e1db 100644 --- a/src/cpu.zig +++ b/src/cpu.zig @@ -18,6 +18,7 @@ const branchAndExchange = @import("cpu/arm/branch.zig").branchAndExchange; // THUMB Instruction Groups const format3 = @import("cpu/thumb/format3.zig").format3; +const format5 = @import("cpu/thumb/format5.zig").format5; pub const ArmInstrFn = fn (*Arm7tdmi, *Bus, u32) void; pub const ThumbInstrFn = fn (*Arm7tdmi, *Bus, u16) void; @@ -160,6 +161,14 @@ fn thumbPopulate() [0x400]ThumbInstrFn { lut[i] = format3(op, rd); } + + if (i >> 4 & 0x3F == 0b010001) { + const op = i >> 2 & 0x3; + const h1 = i >> 1 & 1; + const h2 = i & 1; + + lut[i] = format5(op, h1, h2); + } } return lut; diff --git a/src/cpu/thumb/format5.zig b/src/cpu/thumb/format5.zig new file mode 100644 index 0000000..51c4e7f --- /dev/null +++ b/src/cpu/thumb/format5.zig @@ -0,0 +1,35 @@ +const std = @import("std"); + +const Bus = @import("../../Bus.zig"); +const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi; +const InstrFn = @import("../../cpu.zig").ThumbInstrFn; + +pub fn format5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn { + return struct { + fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void { + const src = @as(u4, h2) << 3 | (opcode >> 3 & 0x7); + const dst = @as(u4, h1) << 3 | (opcode & 0x7); + + switch (op) { + 0b01 => { + // CMP + const left = cpu.r[dst]; + const right = cpu.r[src]; + const result = left -% right; + + cpu.cpsr.n.write(result >> 31 & 1 == 1); + cpu.cpsr.z.write(result == 0); + cpu.cpsr.c.write(right <= left); + cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1); + }, + 0b10 => cpu.r[dst] = cpu.r[src], // MOV + 0b11 => { + // BX + cpu.cpsr.t.write(cpu.r[src] & 1 == 1); + cpu.r[15] = cpu.r[src] & 0xFFFF_FFFE; + }, + else => std.debug.panic("[CPU] Op #{} is invalid for THUMB Format 5", .{op}), + } + } + }.inner; +}