2 files changed, 119 insertions, 1 deletions

diff --git a/emux64/src/cpu/cpu_cycle.c b/emux64/src/cpu/cpu_cycle.c
index ef0cc24..cdbf87f 100644
--- a/emux64/src/cpu/cpu_cycle.c
+++ b/emux64/src/cpu/cpu_cycle.c
@@ -29,10 +29,23 @@
 
 #include <stdio.h>
 #include <errno.h>
+#include <stdbool.h>
 #include "rom.h"
 #include "cpu/cpu.h"
 
 /*
+ * Returns true if the register is an
+ * X<n> type register
+ *
+ * @reg: Register to check
+ */
+static inline bool
+reg_is_xn(uint8_t reg)
+{
+    return reg >= REG_XN_BASE && reg <= REG_XN_LIMIT;
+}
+
+/*
  * Fetch an instruction byte from memory
  *
  * @core: Core to fetch for
@@ -71,6 +84,89 @@ cpu_fetch(struct osmx_core *core, uint8_t *byte_res)
     return 0;
 }
 
+/*
+ * Get a 32-bit value from values @ PC
+ */
+static int
+cpu_fetch32(struct osmx_core *core, uint64_t *res)
+{
+    uint64_t v = 0;
+    uint8_t byte;
+    int error;
+
+    if (core == NULL || res == NULL) {
+        return -EINVAL;
+    }
+
+    for (int i = 0; i < 32; i += 8) {
+        error = cpu_fetch(core, &byte);
+        if (error < 0) {
+            return error;
+        }
+
+        v |= (byte << i);
+        core->pc++;
+    }
+
+    *res = v;
+    return 0;
+}
+
+static int
+cpu_do_add(struct osmx_core *core)
+{
+    uint8_t dest, op1, op2;
+    uint64_t op1_val;
+    uint64_t op2_val;
+    bool op2_is_imm;
+    bool op2_is_big;
+    int error = 0;
+
+    /* Grab the dest register */
+    error = cpu_fetch(core, &dest);
+    if (error < 0) {
+        return error;
+    }
+
+    ++core->pc;
+
+    error = cpu_fetch(core, &op1);
+    if (error < 0) {
+        return error;
+    }
+
+    ++core->pc;
+
+    /*
+     * Operand one is 7 bits as the first bit in the
+     * byte is used for indicating if the second operand
+     * is an immediate value (1) or a register (0)
+     */
+    op2_is_imm = op1 & 1;
+    op1 >>= 1;
+
+    /* Verify the register types */
+    if (!reg_is_xn(dest) || dest == 0)
+        return -1;
+    if (!reg_is_xn(op1))
+        return -1;
+
+    if (op2_is_imm) {
+        error = cpu_fetch32(core, &op2_val);
+    } else {
+        op2_val = core->xn[op2 - REG_XN_BASE];
+    }
+
+    if (error != 0) {
+        return error;
+    }
+
+    op1_val = core->xn[op1];
+    core->xn[dest] = op1_val + op2_val;
+    return 0;
+
+}
+
 int
 cpu_run(struct osmx_core *core)
 {
@@ -85,18 +181,33 @@ cpu_run(struct osmx_core *core)
     while (core->run) {
         error = cpu_fetch(core, &opcode);
         if (error < 0) {
+            printf("cpu: instruction fetch error\n");
             core->run = 0;
             break;
         }
 
+        ++core->pc;
+
         /* Test opcode cases */
         switch (opcode) {
         case OP_HLT:
             printf("** HALTED **\n");
             core->run = false;
             continue;
+        case OP_ADD:
+            if (cpu_do_add(core) < 0) {
+                core->run = false;
+                continue;
+            }
+            break;
+        case OP_NOP:
+            break;
+        default:
+            printf("** INVALID OPCODE **\n");
+            break;
         }
 
-        ++core->pc;
+        printf("--\n");
+        cpu_dump(core);
     }
 }
diff --git a/emux64/src/include/cpu/cpu.h b/emux64/src/include/cpu/cpu.h
index bffb6c0..84c8832 100644
--- a/emux64/src/include/cpu/cpu.h
+++ b/emux64/src/include/cpu/cpu.h
@@ -54,6 +54,13 @@
 #define OP_DIV 0x04     /* 2-op DIV */
 #define OP_HLT 0x0E     /* Halt */
 
+/*
+ * X<n> register encoding
+ *
+ * Range: 0x00 - 0xE
+ */
+#define REG_XN_BASE     0x00
+#define REG_XN_LIMIT    (REG_XN_BASE + 0x0E)
 
 /*
  * Represents a single unit of execution