diff --git a/src/RP2040.Core/Cpu/InstructionDecoder.cs b/src/RP2040.Core/Cpu/InstructionDecoder.cs index 832d334..ae2c74b 100644 --- a/src/RP2040.Core/Cpu/InstructionDecoder.cs +++ b/src/RP2040.Core/Cpu/InstructionDecoder.cs @@ -106,6 +106,8 @@ public InstructionDecoder () // ================================================================ // ADD (SP + imm7) new OpcodeRule (0xFF80, 0xB000, &ArithmeticOps.AddSpImmediate7), + // Sub (SP - imm) + new OpcodeRule (0xFF80, 0xB080, &ArithmeticOps.SubSp), // ================================================================ // GROUP 6: Mask 0xFF00 (8 bits significant - Broad Categories) @@ -151,7 +153,11 @@ public InstructionDecoder () new OpcodeRule (0xFE00, 0x1800, &ArithmeticOps.AddsRegister), // ADDS (Rd, Rn, imm3) new OpcodeRule (0xFE00, 0x1C00, &ArithmeticOps.AddsImmediate3), - + // SUBS (imm3) + new OpcodeRule (0xFE00, 0x1E00, &ArithmeticOps.SubsImmediate3), + // SUBS (Register) + new OpcodeRule (0xFE00, 0x1A00, &ArithmeticOps.SubsRegister), + // ================================================================ // GROUP 8: Mask 0xF800 (5 bits significant - Most Generic) // ================================================================ @@ -159,6 +165,8 @@ public InstructionDecoder () new OpcodeRule (0xF800, 0xA800, &ArithmeticOps.AddSpImmediate8), // ADDS (Rd, imm8) new OpcodeRule (0xF800, 0x3000, &ArithmeticOps.AddsImmediate8), + // SUBS (imm8) + new OpcodeRule (0xF800, 0x3800, &ArithmeticOps.SubsImmediate8), // ADR (Rd, imm8) new OpcodeRule (0xF800, 0xA000, &ArithmeticOps.Adr), // ASRS (Rd, Rm, imm5) diff --git a/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs b/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs index d52d5ba..56aad08 100644 --- a/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs +++ b/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs @@ -23,12 +23,9 @@ public static void AddsImmediate8 (ushort opcode, CortexM0Plus cpu) // ADDS Rd, #imm8 (Rd es Source y Destino) var rd = (opcode >> 8) & 0x7; var imm8 = (uint)(opcode & 0xFF); - - // OPTIMIZACIÓN CLAVE: Read-Modify-Write - // Capturamos la dirección de Rd una sola vez. + ref var ptrRd = ref cpu.Registers[rd]; - // Pasamos 'ptrRd' (se lee implícitamente) y asignamos el resultado a 'ptrRd' ptrRd = AddWithFlags (cpu, ptrRd, imm8, carryIn: 0); } @@ -40,7 +37,6 @@ public static void AddsRegister (ushort opcode, CortexM0Plus cpu) var rn = (opcode >> 3) & 0x7; var rm = (opcode >> 6) & 0x7; - // Preparamos puntero de escritura ref var ptrRd = ref cpu.Registers[rd]; var valRn = cpu.Registers[rn]; var valRm = cpu.Registers[rm]; @@ -55,7 +51,6 @@ public static void Adcs (ushort opcode, CortexM0Plus cpu) var rd = opcode & 0x7; var rm = (opcode >> 3) & 0x7; - // Read-Modify-Write optimizado ref var ptrRd = ref cpu.Registers[rd]; var valRm = cpu.Registers[rm]; @@ -64,14 +59,10 @@ public static void Adcs (ushort opcode, CortexM0Plus cpu) ptrRd = AddWithFlags (cpu, ptrRd, valRm, carryIn); } - // --- ADD SP (Sin Flags) --- - [MethodImpl (MethodImplOptions.AggressiveInlining)] public static void AddSpImmediate7 (ushort opcode, CortexM0Plus cpu) { // ADD SP, SP, #imm7 - // Aquí no necesitamos ref porque SP es un campo directo, no un array indexado. - // El acceso a cpu.Registers.SP ya es directísimo. var imm7 = (uint)((opcode & 0x7F) << 2); cpu.Registers.SP += imm7; } @@ -83,7 +74,6 @@ public static void AddSpImmediate8 (ushort opcode, CortexM0Plus cpu) var rd = (opcode >> 8) & 0x7; var imm8 = (uint)((opcode & 0xFF) << 2); - // Escritura en Rd cpu.Registers[rd] = cpu.Registers.SP + imm8; } @@ -134,7 +124,6 @@ public static void Adr (ushort opcode, CortexM0Plus cpu) var basePc = (cpu.Registers.PC + 2) & 0xFFFFFFFC; - // Escritura directa cpu.Registers[rd] = basePc + (imm8 << 2); } @@ -206,7 +195,7 @@ public static void Muls (ushort opcode, CortexM0Plus cpu) [MethodImpl (MethodImplOptions.AggressiveInlining)] public static void SubsImmediate3 (ushort opcode, CortexM0Plus cpu) { - // SUBS Rd, Rn, #imm3 + // SUBS Rd, Rn, #imm3 (Encoding T1) var rd = opcode & 0x7; var rn = (opcode >> 3) & 0x7; var imm3 = (uint)((opcode >> 6) & 0x7); @@ -220,18 +209,39 @@ public static void SubsImmediate3 (ushort opcode, CortexM0Plus cpu) [MethodImpl (MethodImplOptions.AggressiveInlining)] public static void SubsImmediate8 (ushort opcode, CortexM0Plus cpu) { - // SUBS Rd, #imm8 (Rd es Source y Destino) + // SUBS Rd, #imm8 (Encoding T2) var rd = (opcode >> 8) & 0x7; var imm8 = (uint)(opcode & 0xFF); - // OPTIMIZACIÓN: Read-Modify-Write ref var ptrRd = ref cpu.Registers[rd]; ptrRd = SubWithFlags (cpu, ptrRd, imm8); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SubSp(ushort opcode, CortexM0Plus cpu) + { + // SUB (SP minus immediate) + var imm32 = (opcode & 0x7f) << 2; + cpu.Registers.SP -= (ushort) imm32; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SubsRegister(ushort opcode, CortexM0Plus cpu) + { + var rm = (opcode >> 6) & 0x7; + var rn = (opcode >> 3) & 0x7; + var rd = opcode & 0x7; + + ref var ptrRd = ref cpu.Registers[rd]; + var valRn = cpu.Registers[rn]; + var valRm = cpu.Registers[rm]; + + ptrRd = SubWithFlags(cpu, valRn, valRm); + } // ============================================================= - // MATH HELPERS (Sin cambios, reciben valores) + // MATH HELPERS // ============================================================= [MethodImpl (MethodImplOptions.AggressiveInlining)] diff --git a/src/RP2040.Core/Helpers/InstructionEmiter.cs b/src/RP2040.Core/Helpers/InstructionEmiter.cs index 0ee4755..253a0a7 100644 --- a/src/RP2040.Core/Helpers/InstructionEmiter.cs +++ b/src/RP2040.Core/Helpers/InstructionEmiter.cs @@ -54,6 +54,33 @@ public static ushort AddsRegister (uint rd, uint rn, uint rm) return (ushort)(0x1800 | ((rm & 0x07) << 6) | (rn & 0x07) << 3 | (rd & 0x07)); } + public static ushort SubSp (uint imm) + { + if (imm > 508) throw new ArgumentOutOfRangeException(nameof(imm), "Immediate value must be between 0 and 508."); + return (ushort)(0xB080 | ((imm >> 2) & 0x7f)); + } + + public static ushort SubsImm3 (uint rd, uint rn, uint imm3) + { + if (rd > 7 || rn > 7) throw new ArgumentException (LowRegisterIndexOutOfRange); + if (imm3 > 7) throw new ArgumentException("Immediate too large for SUBS"); + + return (ushort)(0x1E00 | ((imm3 & 0x7) << 6) | ((rn & 7) << 3) | (rd & 7)); + } + + public static ushort SubsImm8 (uint rdn, uint imm8) + { + if (rdn > 7) throw new ArgumentException (LowRegisterIndexOutOfRange); + if (imm8 > 255) throw new ArgumentException ("Immediate too large for SUBS"); + return (ushort)(0x3800 | ((rdn & 7) << 8) | (imm8 & 0xff)); + } + + public static ushort SubsReg(uint rd, uint rn, uint rm) + { + if (rd > 7 || rn > 7 || rm > 7) throw new ArgumentException (LowRegisterIndexOutOfRange); + return (ushort)(0x1A00 | ((rm & 0x7) << 6) | ((rn & 7) << 3) | (rd & 7)); + } + public static ushort Adr (uint rd, uint imm8) { if (rd > 7) throw new ArgumentException (LowRegisterIndexOutOfRange); diff --git a/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs b/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs index 757045b..04783a0 100644 --- a/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs +++ b/tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs @@ -52,7 +52,11 @@ public static TheoryData GetInstructionTestCases () Add ("AddsImm8", InstructionEmiter.AddsImm8 (R1, 1), &ArithmeticOps.AddsImmediate8); Add ("AddsRegister", InstructionEmiter.AddsRegister (R1, R2, R7), &ArithmeticOps.AddsRegister); Add ("Adr", InstructionEmiter.Adr (R4, 0x50), &ArithmeticOps.Adr); - + Add("SubsRegister", InstructionEmiter.SubsReg(R1, R2, R4), &ArithmeticOps.SubsRegister); + Add("SubsImm3", InstructionEmiter.SubsImm3(R1, R2, 3), &ArithmeticOps.SubsImmediate3); + Add("SubsImm8", InstructionEmiter.SubsImm8(R1, 0x10), &ArithmeticOps.SubsImmediate8); + Add("SubSp", InstructionEmiter.SubSp(0x10), &ArithmeticOps.SubSp); + // Special Cases for AddHighRegister Add ("AddHighReg (Reg)", InstructionEmiter.AddHighRegisters (R1, R2), &ArithmeticOps.AddHighToReg); Add ("AddHighReg (Sp)", InstructionEmiter.AddHighRegisters (SP, R2), &ArithmeticOps.AddHighToSp); diff --git a/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs b/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs index cbef629..7d4770d 100644 --- a/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs +++ b/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs @@ -683,4 +683,145 @@ public void Should_SetNegativeFlag_When_ResultIsInterpretedAsSignedNegative () Cpu.Registers.Z.Should ().BeFalse (); } } + + public class Sub : CpuTestBase + { + [Fact] + public void Should_Execute_SubSpInstruction() + { + // Arrange + var opcode = InstructionEmiter.SubSp(0x10); + Cpu.Registers.SP = 0x10000040; + Bus.WriteHalfWord (0x20000000, opcode); + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers.SP.Should ().Be (0x10000030); + } + } + + public class Subs : CpuTestBase + { + [Fact] + public void Should_Execute_SubsR1_With_Overflow () + { + // Arrange + var opcode = InstructionEmiter.SubsImm8 (R1, 1); + Cpu.Registers[R1] = (uint)(-0x80000000 & uint.MaxValue); + Bus.WriteHalfWord (0x20000000, opcode); + + // Act + Cpu.Step (); + + // Assert + Cpu.Registers[R1].Should ().Be (0x7fffffff); + Cpu.Registers.N.Should ().BeFalse (); + Cpu.Registers.Z.Should ().BeFalse (); + Cpu.Registers.C.Should ().BeTrue (); + Cpu.Registers.V.Should ().BeTrue (); + } + + [Fact] + public void Should_Execute_SubsR5R3 () + { + // Arrange + var opcode = InstructionEmiter.SubsImm3 (R5, R3, 5); + Bus.WriteHalfWord (0x20000000, opcode); + Cpu.Registers.R3 = 0; + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers[R5].Should ().Be ((uint)(-5 & uint.MaxValue)); + Cpu.Registers.N.Should ().BeTrue(); + Cpu.Registers.Z.Should ().BeFalse (); + Cpu.Registers.C.Should ().BeFalse (); + Cpu.Registers.V.Should ().BeFalse (); + } + + [Fact] + public void Should_Execute_SubsR5R3R2() + { + // Arrange + var opcode = InstructionEmiter.SubsReg (R5, R3, R2); + Bus.WriteHalfWord (0x20000000, opcode); + Cpu.Registers.R3 = 6; + Cpu.Registers.R2 = 5; + + // Act + Cpu.Step(); + + // Assert + Cpu.Registers.R5.Should ().Be (1); + Cpu.Registers.N.Should ().BeFalse (); + Cpu.Registers.Z.Should ().BeFalse (); + Cpu.Registers.C.Should ().BeTrue (); + Cpu.Registers.V.Should ().BeFalse (); + } + + [Fact] + public void Should_Execute_SubsR3R3R2() + { + // Arrange + var opcode = InstructionEmiter.SubsReg (R3, R3, R2); + Bus.WriteHalfWord (0x20000000, opcode); + Cpu.Registers.R2 = 8; + Cpu.Registers.R3 = 0xffffffff; + + // Act + Cpu.Step (); + + // Arrange + Cpu.Registers.R3.Should ().Be (0xfffffff7); + Cpu.Registers.N.Should ().BeTrue (); + Cpu.Registers.Z.Should ().BeFalse (); + Cpu.Registers.C.Should ().BeTrue (); + Cpu.Registers.V.Should ().BeFalse (); + } + + [Fact] + public void Should_Execute_SubsR5R3R2_And_Set_NVFlags () + { + // Arrange + var opcode = InstructionEmiter.SubsReg (R5, R3, R2); + Bus.WriteHalfWord (0x20000000, opcode); + Cpu.Registers.R3 = 0; + Cpu.Registers.R2 = 0x80000000; + + // Act + Cpu.Step (); + + // Assert + Cpu.Registers.R5.Should ().Be (0x80000000); + Cpu.Registers.N.Should ().BeTrue (); + Cpu.Registers.Z.Should ().BeFalse (); + Cpu.Registers.C.Should ().BeFalse (); + Cpu.Registers.V.Should ().BeTrue (); + } + + [Fact] + public void Should_Execute_SubsR5R3R2_And_Set_ZCFlags () + { + // Arrange + var opcode = InstructionEmiter.SubsReg (R3, R3, R2); + Bus.WriteHalfWord (0x20000000, opcode); + Cpu.Registers.R3 = 0x80000000; + Cpu.Registers.R2 = 0x80000000; + + // Act + Cpu.Step (); + + // Assert + Cpu.Registers.R5.Should ().Be (0); + Cpu.Registers.N.Should ().BeFalse (); + Cpu.Registers.Z.Should ().BeTrue (); + Cpu.Registers.C.Should ().BeTrue (); + Cpu.Registers.V.Should ().BeFalse (); + } + } + + }