diff --git a/src/arm/cpu/arm/block_data_transfer.zig b/src/arm/cpu/arm/block_data_transfer.zig index 574fd3d..66a7550 100644 --- a/src/arm/cpu/arm/block_data_transfer.zig +++ b/src/arm/cpu/arm/block_data_transfer.zig @@ -12,18 +12,18 @@ pub fn blockDataTransfer(comptime InstrFn: type, comptime P: bool, comptime U: b // U determines whether the LDM/STM transfer is made upwards (U == 1) // or downwards (U == 0). + const base_addr = cpu.r[rn]; + const start_addr: u32 = if (U) blk: { - break :blk cpu.r[rn] + if (P) 4 else 0; + break :blk base_addr + if (P) 4 else 0; } else blk: { - break :blk cpu.r[rn] - (4 * reg_count) + if (!P) 4 else 0; + break :blk base_addr - (4 * reg_count) + if (!P) 4 else 0; }; - // FIXME : why 4 * reg_count? - const new_base_addr: u32 = if (U) blk: { - break :blk cpu.r[rn] + 4 * reg_count; + break :blk base_addr + 4 * reg_count; } else blk: { - break :blk cpu.r[rn] - 4 * reg_count; + break :blk base_addr - 4 * reg_count; }; var address = start_addr; @@ -36,9 +36,9 @@ pub fn blockDataTransfer(comptime InstrFn: type, comptime P: bool, comptime U: b if (rlist == 0) { if (Arm32.arch == .v4t) { const undefined_addr: u32 = if (U) blk: { - break :blk cpu.r[rn] + if (P) 4 else 0; + break :blk base_addr + if (P) 4 else 0; } else blk: { - break :blk cpu.r[rn] - (0x40 - if (!P) 4 else 0); + break :blk base_addr - (0x40 - if (!P) 4 else 0); }; if (L) { @@ -49,41 +49,34 @@ pub fn blockDataTransfer(comptime InstrFn: type, comptime P: bool, comptime U: b } } - cpu.r[rn] = if (U) cpu.r[rn] + 0x40 else cpu.r[rn] - 0x40; + cpu.r[rn] = if (U) base_addr + 0x40 else base_addr - 0x40; return; } - // What happens when W is set and Rn is in the rlist? (STM) - // - // Armv4: Store OLD Base if Rb is FIRST entry in Rlist, otherwise store NEW base - // Armv5: Always store OLD Base - - // FIXME: This absolutely needs revisiting :skull: - - const r15_present = rlist >> 15 & 1 == 1; - var write_to_base = true; - for (first_in_list..16) |idx| { const i: u4 = @intCast(idx); if (rlist >> i & 1 == 1) { - transfer(cpu, r15_present, i, address); - address += 4; - - if (W and !L and write_to_base) { - cpu.r[rn] = new_base_addr; - write_to_base = false; + if (L) { + load(cpu, i, rlist, address); + } else { + store(cpu, rn, i, rlist, address, .{ .old_addr = base_addr, .new_addr = new_base_addr }); } + + address += 4; } } - // What happens when W is set and Rn is in the rlist? (LDM) - // - // ARMv4: No writeback - // ARMv5: writeback if Rn is "the ONLY register" or NOT the LAST register + if (W and !L) + cpu.r[rn] = new_base_addr; if (W and L) { - if (rlist >> rn & 1 == 0) { + // What happens when W is set and Rn is in the rlist? (LDM) + // + // ARMv4: No writeback + // ARMv5: writeback if Rn is "the ONLY register" or NOT the LAST register + + if (rlist >> rn & 1 == 0) { // rn is not in rlist cpu.r[rn] = new_base_addr; return; } @@ -101,35 +94,60 @@ pub fn blockDataTransfer(comptime InstrFn: type, comptime P: bool, comptime U: b } } - fn transfer(cpu: *Arm32, r15_present: bool, i: u5, address: u32) void { - if (L) { - if (S and !r15_present) { - // Always Transfer User mode Registers - cpu.setUserModeRegister(i, cpu.read(u32, address)); - } else { - const value = cpu.read(u32, address); + fn load(cpu: *Arm32, ri: u4, rlist: u16, address: u32) void { + const has_r15 = rlist >> 15 & 1 == 1; - cpu.r[i] = value; - if (i == 0xF) { - const mask: u32 = if (Arm32.arch == .v5te) 1 else 3; - cpu.r[i] &= ~mask; - - if (Arm32.arch == .v5te) cpu.cpsr.t.write(value & 1 == 1); - if (S) cpu.setCpsr(cpu.spsr.raw); // FIXME: before or after the reload? - - cpu.pipe.reload(cpu); - } - } + if (S and !has_r15) { + // Always Transfer User mode Registers + cpu.setUserModeRegister(ri, cpu.read(u32, address)); } else { - if (S) { - // Always Transfer User mode Registers - // This happens regardless if r15 is in the list - const value = cpu.getUserModeRegister(i); - cpu.write(u32, address, value + if (i == 0xF) 4 else @as(u32, 0)); // PC is already 8 ahead to make 12 - } else { - cpu.write(u32, address, cpu.r[i] + if (i == 0xF) 4 else @as(u32, 0)); + const value = cpu.read(u32, address); + cpu.r[ri] = value; + + if (ri == 0xF) { + const mask: u32 = if (Arm32.arch == .v5te) 1 else 3; + cpu.r[ri] &= ~mask; + + if (Arm32.arch == .v5te) cpu.cpsr.t.write(value & 1 == 1); + if (S) cpu.setCpsr(cpu.spsr.raw); // FIXME: before or after the reload? + + cpu.pipe.reload(cpu); } } } + + const BaseAddrs = struct { old_addr: u32, new_addr: u32 }; + + fn store(cpu: *Arm32, rn: u4, ri: u4, rlist: u16, address: u32, base: BaseAddrs) void { + const value = if (S) blk: { + // if S == true: + // Always Transfer User mode Registers + // This happens regardless if r15 is in the list + + break :blk cpu.getUserModeRegister(ri); + } else blk: { + if (ri == rn) { + // What happens when W is set and Rn is in the rlist? (STM) + // + // Armv4: Store OLD Base if Rb is FIRST entry in Rlist, otherwise store NEW base + // Armv5: Always store OLD Base + + if (rlist >> rn & 1 == 0) + break :blk base.new_addr; + + const mask = @as(u16, 1) << rn; + const is_first = @popCount(rlist & (mask - 1)) == 0; + + break :blk switch (Arm32.arch) { + .v4t => if (is_first) base.old_addr else base.new_addr, + .v5te => base.old_addr, + }; + } + + break :blk cpu.r[ri]; + }; + + cpu.write(u32, address, value + if (ri == 0xF) 4 else @as(u32, 0)); + } }.inner; }