diff --git a/core/cpu.c b/core/cpu.c
index f972593..29a934f 100644
--- a/core/cpu.c
+++ b/core/cpu.c
@@ -529,8 +529,7 @@ static uint32_t cpuSetSystemRegister(VB*sim,
 }
 
 /* Addition common processing */
-static int32_t cpuAdd(VB *sim, int32_t b) {
-    int32_t a = cpuGetReg2(sim);
+static int32_t cpuAdd(VB *sim, int32_t a, int32_t b) {
     int32_t c = a + b;
     sim->cpu.psw.cy = (uint32_t) c < (uint32_t) a;
     sim->cpu.psw.ov = (~(a ^ b) & (a ^ c)) < 0;
@@ -659,8 +658,7 @@ static int32_t cpuShiftRight(VB *sim, int32_t b) {
 }
 
 /* Subtraction common processing */
-static int32_t cpuSubtract(VB *sim, int32_t b) {
-    int32_t a = cpuGetReg2(sim);
+static int32_t cpuSubtract(VB *sim, int32_t a, int32_t b) {
     int32_t c = a - b;
     sim->cpu.psw.cy = (uint32_t) a < (uint32_t) b;
     sim->cpu.psw.ov = ((a ^ b) & (a ^ c)) < 0;
@@ -735,13 +733,13 @@ static int cpuST_OUT(VB *sim, int type) {
 
 /* ADD immediate */
 static void cpuADDImm(VB *sim) {
-    cpuSetReg2(sim, cpuAdd(sim, cpuGetImm5S(sim)));
+    cpuSetReg2(sim, cpuAdd(sim, cpuGetReg2(sim), cpuGetImm5S(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
 /* ADD register */
 static void cpuADDReg(VB *sim) {
-    cpuSetReg2(sim, cpuAdd(sim, cpuGetReg1(sim)));
+    cpuSetReg2(sim, cpuAdd(sim, cpuGetReg2(sim), cpuGetReg1(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -762,7 +760,7 @@ static void cpuADDF_S(VB *sim) {
 
 /* ADDI */
 static void cpuADDI(VB *sim) {
-    cpuSetReg2(sim, cpuAdd(sim, cpuGetImm16S(sim)));
+    cpuSetReg2(sim, cpuAdd(sim, cpuGetReg1(sim), cpuGetImm16S(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -774,7 +772,7 @@ static void cpuAND(VB *sim) {
 
 /* ANDI */
 static void cpuANDI(VB *sim) {
-    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg2(sim) & cpuGetImm16U(sim)));
+    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) & cpuGetImm16U(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -803,7 +801,7 @@ static int cpuCAXI(VB *sim) {
 
             /* Update state */
             sim->cpu.clocks += cpuClocks(1); /* TODO: Research */
-            cpuSubtract(sim, auxData.value);
+            cpuSubtract(sim, cpuGetReg2(sim), auxData.value);
             if (sim->cpu.psw.z)
                 auxData.value = sim->cpu.program[30];
 
@@ -832,13 +830,13 @@ static void cpuCLI(VB *sim) {
 
 /* CMP immediate */
 static void cpuCMPImm(VB *sim) {
-    cpuSubtract(sim, cpuGetImm5S(sim));
+    cpuSubtract(sim, cpuGetReg2(sim), cpuGetImm5S(sim));
     cpuAdvance(sim, cpuClocks(1));
 }
 
 /* CMP register */
 static void cpuCMPReg(VB *sim) {
-    cpuSubtract(sim, cpuGetReg1(sim));
+    cpuSubtract(sim, cpuGetReg2(sim), cpuGetReg1(sim));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -1148,7 +1146,7 @@ static void cpuOR(VB *sim) {
 
 /* ORI */
 static void cpuORI(VB *sim) {
-    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg2(sim) | cpuGetImm16U(sim)));
+    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) | cpuGetImm16U(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -1262,7 +1260,7 @@ static void cpuSTSR(VB *sim) {
 
 /* SUB */
 static void cpuSUB(VB *sim) {
-    cpuSetReg2(sim, cpuSubtract(sim, cpuGetReg1(sim)));
+    cpuSetReg2(sim, cpuSubtract(sim, cpuGetReg2(sim), cpuGetReg1(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }
 
@@ -1334,7 +1332,7 @@ static void cpuXOR(VB *sim) {
 
 /* XORI */
 static void cpuXORI(VB *sim) {
-    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg2(sim) ^ cpuGetImm16U(sim)));
+    cpuSetReg2(sim, cpuBitwise(sim, cpuGetReg1(sim) ^ cpuGetImm16U(sim)));
     cpuAdvance(sim, cpuClocks(1));
 }