Skip to content

Implements SUB and SUBS instructions. #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
begeistert merged 4 commits into from
Feb 2, 2026
Merged

Implements SUB and SUBS instructions. #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
cfb7a68
Implements SUB and SUBS instructions.
lmSeryi Feb 1, 2026
7c7c380
Unnecessary bit operation removed
lmSeryi Feb 1, 2026
3226ceb
docs: fix comments to differentiate the different SUBS functions
begeistert Feb 2, 2026
8bba699
fix: fix the InstructionEmitter exception messages for SUBS
begeistert Feb 2, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion src/RP2040.Core/Cpu/InstructionDecoder.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

namespace RP2040.Core.Cpu;

public unsafe sealed class InstructionDecoder : IDisposable

Check warning on line 9 in src/RP2040.Core/Cpu/InstructionDecoder.cs

View workflow job for this annotation

GitHub Actions / Unit Tests 

Make sure that using "unsafe" is safe here. (https://rules.sonarsource.com/csharp/RSPEC-6640)

Check warning on line 9 in src/RP2040.Core/Cpu/InstructionDecoder.cs

View workflow job for this annotation

GitHub Actions / Unit Tests 

Make sure that using "unsafe" is safe here. (https://rules.sonarsource.com/csharp/RSPEC-6640)
{
public static InstructionDecoder Instance { get; } = new InstructionDecoder ();

Expand Down Expand Up @@ -106,6 +106,8 @@
// ================================================================
// 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)
Expand Down Expand Up @@ -151,14 +153,20 @@
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)
// ================================================================
// ADD (Rd = SP + imm8)
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)
Expand Down Expand Up @@ -213,7 +221,7 @@

private static void HandleUndefined (ushort opcode, CortexM0Plus cpu)
{
throw new Exception ($"Undefined Opcode: 0x{opcode:X4} PC={cpu.Registers.PC:X8}");

Check warning on line 224 in src/RP2040.Core/Cpu/InstructionDecoder.cs

View workflow job for this annotation

GitHub Actions / Unit Tests 

'System.Exception' should not be thrown by user code. (https://rules.sonarsource.com/csharp/RSPEC-112)

Check warning on line 224 in src/RP2040.Core/Cpu/InstructionDecoder.cs

View workflow job for this annotation

GitHub Actions / Unit Tests 

'System.Exception' should not be thrown by user code. (https://rules.sonarsource.com/csharp/RSPEC-112)
}

[ExcludeFromCodeCoverage]
Expand Down
42 changes: 26 additions & 16 deletions src/RP2040.Core/Cpu/Instructions/ArithmeticOps.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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];
Expand All @@ -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];

Expand All @@ -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;
}
Expand All @@ -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;
}

Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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);
Expand All @@ -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)]
Expand Down
27 changes: 27 additions & 0 deletions src/RP2040.Core/Helpers/InstructionEmiter.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
6 changes: 5 additions & 1 deletion tests/RP2040.Core.Tests/Cpu/InstructionDecoderTests.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,11 @@ public static TheoryData<string, ushort, ulong> 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);
Expand Down
141 changes: 141 additions & 0 deletions tests/RP2040.Core.Tests/Cpu/Instructions/ArithmeticOpsTests.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 ();
}
}


}