diff --git a/src/RP2040.Core/Cpu/InstructionDecoder.cs b/src/RP2040.Core/Cpu/InstructionDecoder.cs index ef99387..8582f84 100644 --- a/src/RP2040.Core/Cpu/InstructionDecoder.cs +++ b/src/RP2040.Core/Cpu/InstructionDecoder.cs @@ -153,6 +153,8 @@ public InstructionDecoder() new OpcodeRule(0xFE00, 0x1E00, &ArithmeticOps.SubsImmediate3), // SUBS (Register) new OpcodeRule(0xFE00, 0x1A00, &ArithmeticOps.SubsRegister), + // LDR (register) + new OpcodeRule(0xFE00, 0x5800, &MemoryOps.LdrRegister), // ================================================================ // GROUP 8: Mask 0xF800 (5 bits significant - Most Generic) // ================================================================ @@ -176,6 +178,12 @@ public InstructionDecoder() new OpcodeRule(0xF800, 0x2000, &BitOps.Movs), // LDMIA (Load Multiple Increment After) new OpcodeRule(0xF800, 0xC800, &MemoryOps.Ldmia), + // LDR (literal) + new OpcodeRule(0xF800, 0x4800, &MemoryOps.LdrLiteral), + // LDR (imm5) + new OpcodeRule(0xF800, 0x6800, &MemoryOps.LdrImmediate), + // LDR (SP, imm8) + new OpcodeRule(0xF800, 0x9800, &MemoryOps.LdrSpRelative), // LSLS (Rd, Rm, imm5) new OpcodeRule(0xF800, 0x0000, &BitOps.LslsImm5), // ================================================================ diff --git a/src/RP2040.Core/Cpu/Instructions/MemoryOps.cs b/src/RP2040.Core/Cpu/Instructions/MemoryOps.cs index de37545..1cadf8c 100644 --- a/src/RP2040.Core/Cpu/Instructions/MemoryOps.cs +++ b/src/RP2040.Core/Cpu/Instructions/MemoryOps.cs @@ -6,6 +6,43 @@ namespace RP2040.Core.Cpu.Instructions; public static unsafe class MemoryOps { + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LdrImmediate(ushort opcode, CortexM0Plus cpu) + { + var rt = opcode & 0x7; + var rn = (opcode >> 3) & 0x7; + var imm5 = (uint)((opcode >> 6) & 0x1F) << 2; + + cpu.Registers[rt] = ReadWordWithCycles(cpu, cpu.Registers[rn] + imm5); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LdrLiteral(ushort opcode, CortexM0Plus cpu) + { + var rt = (opcode >> 8) & 0x7; + var imm8 = (uint)(opcode & 0xFF) << 2; + var nextPc = cpu.Registers.PC + 2; + var addr = (nextPc & 0xFFFFFFFC) + imm8; + cpu.Registers[rt] = ReadWordWithCycles(cpu, addr); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LdrRegister(ushort opcode, CortexM0Plus cpu) + { + var rt = opcode & 0x7; + var rn = (opcode >> 3) & 0x7; + var rm = (opcode >> 6) & 0x7; + cpu.Registers[rt] = ReadWordWithCycles(cpu, cpu.Registers[rn] + cpu.Registers[rm]); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void LdrSpRelative(ushort opcode, CortexM0Plus cpu) + { + var rt = (opcode >> 8) & 0x7; + var imm8 = (uint)(opcode & 0xFF) << 2; + cpu.Registers[rt] = ReadWordWithCycles(cpu, cpu.Registers.SP + imm8); + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Pop(ushort opcode, CortexM0Plus cpu) { @@ -207,4 +244,29 @@ public static void Ldmia(ushort opcode, CortexM0Plus cpu) cpu.Registers[rn] += writeBackOffset; cpu.Cycles += (int)regCount; } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static uint ReadWordWithCycles(CortexM0Plus cpu, uint address) + { + var region = address >> 28; + + switch (region) + { + case <= BusInterconnect.REGION_SRAM: + cpu.Cycles += 1; + break; + case 0x4: // APB/AHB + case 0x5: + cpu.Cycles += 2; + break; + // SIO (Single-cycle IO) + case 0xD: + break; + default: + cpu.Cycles += 1; // Fallback + break; + } + + return cpu.Bus.ReadWord(address); + } } diff --git a/src/RP2040.Core/Helpers/InstructionEmiter.cs b/src/RP2040.Core/Helpers/InstructionEmiter.cs index 75cb1fb..49dbbb1 100644 --- a/src/RP2040.Core/Helpers/InstructionEmiter.cs +++ b/src/RP2040.Core/Helpers/InstructionEmiter.cs @@ -291,6 +291,37 @@ public static ushort Ldmia(uint rn, uint registerList) return (ushort)(0xC800 | (rn & 0x7) << 8 | registerList & 0xFF); } + public static ushort LdrImmediate(uint rt, uint rn, uint imm) + { + var imm5 = imm >> 2; + if (rt > 7 || rn > 7) + throw new ArgumentException(LowRegisterIndexOutOfRange); + return (ushort)(0x6800 | (imm5 & 0x1F) << 6 | (rn & 0x7) << 3 | (rt & 0x7)); + } + + public static ushort LdrLiteral(uint rt, uint imm) + { + var imm8 = imm >> 2; + if (rt > 7) + throw new ArgumentException(LowRegisterIndexOutOfRange); + return (ushort)(0x4800 | (rt & 0x7) << 8 | (imm8 & 0xFF)); + } + + public static ushort LdrRegister(uint rt, uint rn, uint rm) + { + if (rt > 7 || rn > 7 || rm > 7) + throw new ArgumentException(LowRegisterIndexOutOfRange); + return (ushort)(0x5800 | (rm & 0x7) << 6 | (rn & 0x7) << 3 | (rt & 0x7)); + } + + public static ushort LdrSpRelative(uint rt, uint imm) + { + var imm8 = imm >> 2; + if (rt > 7) + throw new ArgumentException(LowRegisterIndexOutOfRange); + return (ushort)(0x9800 | (rt & 0x7) << 8 | (imm8 & 0xFF)); + } + public static ushort LslsImm5(uint rd, uint rm, uint imm5) { if (rd > 7 || rm > 7) diff --git a/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs b/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs index 91cb326..8246866 100644 --- a/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs +++ b/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs @@ -152,6 +152,12 @@ public static TheoryData GetInstructionTestCases() Add("Mrs", (ushort)(InstructionEmiter.Mrs(R0, 5) & 0xFFFF), &SystemOps.Mrs); Add("Msr", (ushort)(InstructionEmiter.Msr(8, R0) & 0xFFFF), &SystemOps.Msr); + // Load Operations + Add("LdrImmediate", InstructionEmiter.LdrImmediate(R0, R1, 4), &MemoryOps.LdrImmediate); + Add("LdrLiteral", InstructionEmiter.LdrLiteral(R2, 0x10), &MemoryOps.LdrLiteral); + Add("LdrRegister", InstructionEmiter.LdrRegister(R3, R4, R5), &MemoryOps.LdrRegister); + Add("LdrSpRelative", InstructionEmiter.LdrSpRelative(R6, 0x20), &MemoryOps.LdrSpRelative); + Add("Ldmia", InstructionEmiter.Ldmia(R0, (1 << R1) | (1 << R2)), &MemoryOps.Ldmia); // Push / Pop diff --git a/tests/RP2040.Core.Tests/Cpu/Instructions/MemoryOpsTests.cs b/tests/RP2040.Core.Tests/Cpu/Instructions/MemoryOpsTests.cs index f164911..3a6f5bb 100644 --- a/tests/RP2040.Core.Tests/Cpu/Instructions/MemoryOpsTests.cs +++ b/tests/RP2040.Core.Tests/Cpu/Instructions/MemoryOpsTests.cs @@ -7,6 +7,207 @@ namespace RP2040.tests.Cpu.Instructions; public abstract class MemoryOpsTests { + public class Ldmia : CpuTestBase + { + [Fact] + public void Should_LoadMultiple_And_WriteBackBaseAddress() + { + // Arrange + var opcode = InstructionEmiter.Ldmia(R0, 1 << R1 | 1 << R2); + Bus.WriteHalfWord(0x20000000, opcode); + const uint baseAddr = 0x20000010; + Cpu.Registers[R0] = baseAddr; + + Bus.WriteWord(baseAddr, 0xF00DF00D); + Bus.WriteWord(baseAddr + 4, 0x4242); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers.PC.Should().Be(0x20000002); + Cpu.Registers[R0].Should().Be(baseAddr + 8); + Cpu.Registers[R1].Should().Be(0xF00DF00D); + Cpu.Registers[R2].Should().Be(0x4242); + } + + [Fact] + public void Should_LoadMultiple_WithoutWriteBack_When_BaseIsLoaded() + { + // Arrange + var opcode = InstructionEmiter.Ldmia(R5, 1 << R5); + Bus.WriteHalfWord(0x20000000, opcode); + const uint baseAddr = 0x20000010; + Cpu.Registers[R5] = baseAddr; + Bus.WriteWord(baseAddr, 0xF00DF00D); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers.PC.Should().Be(0x20000002); + Cpu.Registers[R5].Should().Be(0xF00DF00D); + } + + [Fact] + public void Should_ConsumeCorrectCycles_ForMultipleLoad() + { + // Arrange + var opcode = InstructionEmiter.Ldmia(R0, 1 << R1 | 1 << R2); + Bus.WriteHalfWord(0x20000000, opcode); + Cpu.Registers[R0] = 0x20000010; + + var initialCycles = Cpu.Cycles; + + // Act + Cpu.Step(); + + // Assert + (Cpu.Cycles - initialCycles) + .Should() + .Be(3); + } + } + + public abstract class Ldr : CpuTestBase + { + public class Immediate : CpuTestBase + { + [Fact] + public void Should_LoadValue_From_RnPlusImmediate() + { + // Arrange + var opcode = InstructionEmiter.LdrImmediate(R3, R2, 24); + Bus.WriteHalfWord(0x20000000, opcode); + + Cpu.Registers[R2] = 0x20000100; + Bus.WriteWord(0x20000100 + 24, 0x55); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R3].Should().Be(0x55); + Cpu.Cycles.Should().Be(2, "Ldr to RAM takes 2 cycles"); + } + + [Fact] + public void Should_ConsumeExtraCycles_When_ReadingFromPeripheral() + { + // Arrange + var opcode = InstructionEmiter.LdrImmediate(R0, R1, 0); + Bus.WriteHalfWord(0x20000000, opcode); + Cpu.Registers[R1] = 0x40000000; + + // Act + Cpu.Step(); + + // Assert + Cpu.Cycles.Should().Be(3, "Peripheral access should add wait states"); + } + } + + public class Literal : CpuTestBase + { + [Fact] + public void Should_LoadValue_RelativeToProgramCounter() + { + // Arrange + const uint offset = 148; + var opcode = InstructionEmiter.LdrLiteral(R0, offset); + Bus.WriteHalfWord(0x20000000, opcode); + + var targetAddr = (0x20000004u & 0xFFFFFFFC) + offset; + Bus.WriteWord(targetAddr, 0x42); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R0].Should().Be(0x42); + } + + [Fact] + public void Should_HandleMaxOffset() + { + // Arrange + var opcode = InstructionEmiter.LdrLiteral(R7, 1020); + Bus.WriteHalfWord(0x20000000, opcode); + + var targetAddr = (0x20000004u & 0xFFFFFFFC) + 1020; + Bus.WriteWord(targetAddr, 0xABCDEF); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R7].Should().Be(0xABCDEF); + } + } + + public class Register : CpuTestBase + { + [Fact] + public void Should_LoadValue_From_RnPlusRm() + { + // Arrange + var opcode = InstructionEmiter.LdrRegister(R3, R5, R6); + Bus.WriteHalfWord(0x20000000, opcode); + + Cpu.Registers[R5] = 0x20000000; + Cpu.Registers[R6] = 0x8; + Bus.WriteWord(0x20000008, 0xff554211); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R3].Should().Be(0xff554211); + } + } + + public class SpRelative : CpuTestBase + { + [Fact] + public void Should_LoadValue_RelativeToStackPointer() + { + // Arrange + var opcode = InstructionEmiter.LdrSpRelative(R3, 12); + Bus.WriteHalfWord(0x20000000, opcode); + + Cpu.Registers.SP = 0x20000500; + Bus.WriteWord(0x20000500 + 12, 0xAA); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R3].Should().Be(0xAA); + } + } + + public class Unaligned : CpuTestBase + { + [Fact] + public void Should_HandleUnalignedAccess_WhenUsingUnsafeRead() + { + // Arrange + var opcode = InstructionEmiter.LdrImmediate(R0, R1, 0); + Bus.WriteHalfWord(0x20000000, opcode); + + const uint unalignedAddr = 0x20000101; + Cpu.Registers[R1] = unalignedAddr; + Bus.WriteWord(unalignedAddr, 0xDEADBEEF); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R0].Should().Be(0xDEADBEEF, "Our Bus uses Unsafe.ReadUnaligned"); + } + } + } + public class Pop : CpuTestBase { const uint STACK_BASE = 0x20004000; @@ -211,66 +412,4 @@ public void Should_ConsumeCycles_ProportionalToRegisterCount() .Be(5); } } - - public class Ldmia : CpuTestBase - { - [Fact] - public void Should_LoadMultiple_And_WriteBackBaseAddress() - { - // Arrange - var opcode = InstructionEmiter.Ldmia(R0, 1 << R1 | 1 << R2); - Bus.WriteHalfWord(0x20000000, opcode); - const uint baseAddr = 0x20000010; - Cpu.Registers[R0] = baseAddr; - - Bus.WriteWord(baseAddr, 0xF00DF00D); - Bus.WriteWord(baseAddr + 4, 0x4242); - - // Act - Cpu.Step(); - - // Assert - Cpu.Registers.PC.Should().Be(0x20000002); - Cpu.Registers[R0].Should().Be(baseAddr + 8); - Cpu.Registers[R1].Should().Be(0xF00DF00D); - Cpu.Registers[R2].Should().Be(0x4242); - } - - [Fact] - public void Should_LoadMultiple_WithoutWriteBack_When_BaseIsLoaded() - { - // Arrange - var opcode = InstructionEmiter.Ldmia(R5, 1 << R5); - Bus.WriteHalfWord(0x20000000, opcode); - const uint baseAddr = 0x20000010; - Cpu.Registers[R5] = baseAddr; - Bus.WriteWord(baseAddr, 0xF00DF00D); - - // Act - Cpu.Step(); - - // Assert - Cpu.Registers.PC.Should().Be(0x20000002); - Cpu.Registers[R5].Should().Be(0xF00DF00D); - } - - [Fact] - public void Should_ConsumeCorrectCycles_ForMultipleLoad() - { - // Arrange - var opcode = InstructionEmiter.Ldmia(R0, 1 << R1 | 1 << R2); - Bus.WriteHalfWord(0x20000000, opcode); - Cpu.Registers[R0] = 0x20000010; - - var initialCycles = Cpu.Cycles; - - // Act - Cpu.Step(); - - // Assert - (Cpu.Cycles - initialCycles) - .Should() - .Be(3); - } - } }