From cfb7a688bd311d7641cfce4ccd960ca6f507fe6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergio=20Dom=C3=ADnguez?= Date: Sun, 1 Feb 2026 17:27:00 -0600 Subject: [PATCH 1/4] Implements SUB and SUBS instructions. Adds support for SUB (subtract) and SUBS (subtract with flags) instructions, including variants for immediate values and register operands. This expands the instruction set and enhances the arithmetic capabilities of the CPU. --- src/RP2040.Core/Cpu/InstructionDecoder.cs | 10 +- .../Cpu/Instructions/ArithmeticOps.cs | 21 +++ src/RP2040.Core/Helpers/InstructionEmiter.cs | 27 ++++ .../Cpu/InstructionDecoderTests.cs | 6 +- .../Cpu/Instructions/ArithmeticOpsTests.cs | 141 ++++++++++++++++++ 5 files changed, 203 insertions(+), 2 deletions(-) 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..2e968fd 100644 --- a/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs +++ b/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs @@ -229,6 +229,27 @@ public static void SubsImmediate8 (ushort opcode, CortexM0Plus cpu) ptrRd = SubWithFlags (cpu, ptrRd, imm8); } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static void SubSp(ushort opcode, CortexM0Plus cpu) + { + 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) diff --git a/src/RP2040.Core/Helpers/InstructionEmiter.cs b/src/RP2040.Core/Helpers/InstructionEmiter.cs index 0ee4755..423de75 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 ADDS"); + + 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 ADDS"); + 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..6335c40 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 | 0].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 (); + } + } + + } From 7c7c38085cca8520372e68c5fdc9b20f61e870dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergio=20Dom=C3=ADnguez?= Date: Sun, 1 Feb 2026 17:36:10 -0600 Subject: [PATCH 2/4] Unnecessary bit operation removed --- tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs b/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs index 6335c40..7d4770d 100644 --- a/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs +++ b/tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs @@ -735,7 +735,7 @@ public void Should_Execute_SubsR5R3 () Cpu.Step(); // Assert - Cpu.Registers[R5 | 0].Should ().Be ((uint)(-5 & uint.MaxValue)); + Cpu.Registers[R5].Should ().Be ((uint)(-5 & uint.MaxValue)); Cpu.Registers.N.Should ().BeTrue(); Cpu.Registers.Z.Should ().BeFalse (); Cpu.Registers.C.Should ().BeFalse (); From 3226cebd425a8a79430056da36cf254b17980920 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Iv=C3=A1n=20Montiel=20Cardona?= Date: Mon, 2 Feb 2026 00:40:50 -0600 Subject: [PATCH 3/4] docs: fix comments to differentiate the different SUBS functions --- .../Cpu/Instructions/ArithmeticOps.cs | 21 +++++-------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs b/src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs index 2e968fd..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,11 +209,10 @@ 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); @@ -233,6 +221,7 @@ public static void SubsImmediate8 (ushort opcode, CortexM0Plus cpu) [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; } @@ -252,7 +241,7 @@ public static void SubsRegister(ushort opcode, CortexM0Plus cpu) } // ============================================================= - // MATH HELPERS (Sin cambios, reciben valores) + // MATH HELPERS // ============================================================= [MethodImpl (MethodImplOptions.AggressiveInlining)] From 8bba69924af97a838080e358ea5547973c722e38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Iv=C3=A1n=20Montiel=20Cardona?= Date: Mon, 2 Feb 2026 00:48:30 -0600 Subject: [PATCH 4/4] fix: fix the InstructionEmitter exception messages for SUBS --- src/RP2040.Core/Helpers/InstructionEmiter.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RP2040.Core/Helpers/InstructionEmiter.cs b/src/RP2040.Core/Helpers/InstructionEmiter.cs index 423de75..253a0a7 100644 --- a/src/RP2040.Core/Helpers/InstructionEmiter.cs +++ b/src/RP2040.Core/Helpers/InstructionEmiter.cs @@ -63,7 +63,7 @@ public static ushort SubSp (uint imm) 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 ADDS"); + if (imm3 > 7) throw new ArgumentException("Immediate too large for SUBS"); return (ushort)(0x1E00 | ((imm3 & 0x7) << 6) | ((rn & 7) << 3) | (rd & 7)); } @@ -71,7 +71,7 @@ public static ushort SubsImm3 (uint rd, uint rn, uint imm3) 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 ADDS"); + if (imm8 > 255) throw new ArgumentException ("Immediate too large for SUBS"); return (ushort)(0x3800 | ((rdn & 7) << 8) | (imm8 & 0xff)); }