From 5d09caaf02005b8f508bfe84057b5aeeb57c9ba4 Mon Sep 17 00:00:00 2001
From: Steven Le Rouzic <steven.lerouzic@gmail.com>
Date: Sat, 11 May 2024 18:11:49 +0200
Subject: Jumps & branches

---
 main.c | 144 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 140 insertions(+), 4 deletions(-)

(limited to 'main.c')

diff --git a/main.c b/main.c
index f1f7a9e..feba3b9 100644
--- a/main.c
+++ b/main.c
@@ -2,6 +2,7 @@
 #include <stdlib.h>
 #include <inttypes.h>
 #include <assert.h>
+#include <stdbool.h>
 
 inline uint32_t sign_extend(uint32_t word, uint32_t size)
 {
@@ -91,7 +92,9 @@ struct Hart
 
 void execute_op_imm(struct Hart* hart, uint32_t instruction)
 {
-    struct Instruction inst = decode_i_type(instruction);
+    const struct Instruction inst = decode_i_type(instruction);
+    if (inst.rd == 0) return;
+    
     switch (inst.funct3)
     {
         case 0: // ADDI
@@ -130,7 +133,9 @@ void execute_op_imm(struct Hart* hart, uint32_t instruction)
 
 void execute_op(struct Hart* hart, uint32_t instruction)
 {
-    struct Instruction inst = decode_r_type(instruction);
+    const struct Instruction inst = decode_r_type(instruction);
+    if (inst.rd == 0) return;
+    
     switch (inst.funct3)
     {
         case 0: // ADD, SUB
@@ -174,31 +179,94 @@ void execute_op(struct Hart* hart, uint32_t instruction)
     }
 }
 
+void execute_branch(struct Hart* hart, uint32_t instruction)
+{
+    const struct Instruction inst = decode_b_type(instruction);
+    const uint32_t r1 = hart->regs[inst.rs1];
+    const uint32_t r2 = hart->regs[inst.rs2];
+    bool take_branch = false;
+
+    switch (inst.funct3)
+    {
+        case 0: take_branch = (r1 == r2); break;
+        case 1: take_branch = (r1 != r2); break;
+        case 4: take_branch = (r1 < r2); break;
+        case 5: take_branch = (r1 >= r2); break;
+        case 6: take_branch = ((int32_t)r1 < (int32_t)r2); break;
+        case 7: take_branch = ((int32_t)r1 >= (int32_t)r2); break;
+    }
+
+    if (take_branch)
+    {
+        hart->pc += inst.imm;
+    }
+    else
+    {
+        hart->pc += 4;
+    }
+}
+
 void execute(struct Hart* hart, uint32_t instruction)
 {
     switch (instruction & 0x7f)
     {
         case 0x13:
             execute_op_imm(hart, instruction);
+            hart->pc += 4;
             break;
         case 0x17: // AUIPC
         {
             struct Instruction inst = decode_u_type(instruction);
-            hart->regs[inst.rd] = inst.imm + hart->pc;
+            if (inst.rd != 0)
+            {
+                hart->regs[inst.rd] = inst.imm + hart->pc;
+            }
+            hart->pc += 4;
             break;
         }
         case 0x33:
             execute_op(hart, instruction);
+            hart->pc += 4;
             break;
         case 0x37: // LUI
         {
             struct Instruction inst = decode_u_type(instruction);
-            hart->regs[inst.rd] = inst.imm;
+            if (inst.rd != 0)
+            {
+                hart->regs[inst.rd] = inst.imm;
+            }
+            hart->pc += 4;
+            break;
+        }
+        case 0x63:
+            execute_branch(hart, instruction);
+            break;
+        case 0x67: // JALR
+        {
+            struct Instruction inst = decode_i_type(instruction);
+            assert(inst.funct3 == 0);
+            if (inst.rd != 0)
+            {
+                hart->regs[inst.rd] = hart->pc + 4;
+            }
+            hart->pc = (hart->regs[inst.rs1] + inst.imm) & 0xFFFFFFFE;
+            break;
+        }
+        case 0x6F: // JAL
+        {
+            struct Instruction inst = decode_j_type(instruction);
+            if (inst.rd != 0)
+            {
+                hart->regs[inst.rd] = hart->pc + 4;
+            }
+            hart->pc += inst.imm;
             break;
         }
         default:
             assert(!"Unhandled opcode");
     }
+
+    assert(hart->regs[0] == 0);
 }
 
 void test_addi()
@@ -289,9 +357,11 @@ void test_lui_auipc()
 {
     struct Hart hart = {0};
     
+    hart.pc = 0;
     execute(&hart, 0x0007b0b7); // lui x1, 503808
     assert(hart.regs[1] == 503808);
     
+    hart.pc = 0;
     execute(&hart, 0x0007b097); // auipc x1, 503808
     assert(hart.regs[1] == 503808);
     
@@ -360,6 +430,69 @@ void test_op()
     assert(hart.regs[3] == 0xFFFFFFFE);
 }
 
+void test_jal()
+{
+    struct Hart hart = {0};
+
+    hart.pc = 12;
+
+    execute(&hart, 0x12c000ef); // jal x1, 300
+    assert(hart.regs[1] == 16);
+    assert(hart.pc == 312);
+
+    execute(&hart, 0xed5ff0ef); // jal x1, -300
+    assert(hart.regs[1] == 316);
+    assert(hart.pc == 12);
+}
+
+void test_jalr()
+{
+    struct Hart hart = {0};
+
+    hart.pc = 12;
+    hart.regs[1] = 300;
+
+    execute(&hart, 0x00a08167); // jalr x2, 10(x1)
+    assert(hart.regs[2] == 16);
+    assert(hart.pc == 310);
+
+    execute(&hart, 0xff608167); // jalr x2, -10(x1)
+    assert(hart.regs[2] == 314);
+    assert(hart.pc == 290);
+}
+
+void test_branch()
+{
+    struct Hart hart = {0};
+
+    hart.pc = 100;
+    hart.regs[1] = 2;
+    hart.regs[2] = 0xFFFFFFFC;
+
+    execute(&hart, 0x00208c63); // beq x1, x2, 24
+    assert(hart.pc == 104);
+
+    hart.pc = 100;
+    execute(&hart, 0x00209c63); // bne x1, x2, 24
+    assert(hart.pc == 124);
+
+    hart.pc = 100;
+    execute(&hart, 0x0020cc63); // blt x1, x2, 24
+    assert(hart.pc == 124);
+
+    hart.pc = 100;
+    execute(&hart, 0x0020dc63); // bge x1, x2, 24
+    assert(hart.pc == 104);
+
+    hart.pc = 100;
+    execute(&hart, 0x0020ec63); // bltu x1, x2, 24
+    assert(hart.pc == 104);
+
+    hart.pc = 100;
+    execute(&hart, 0x0020fc63); // bgeu x1, x2, 24
+    assert(hart.pc == 124);
+}
+
 void test()
 {
     test_addi();
@@ -368,6 +501,9 @@ void test()
     test_slli_srli_srai();
     test_lui_auipc();
     test_op();
+    test_jal();
+    test_jalr();
+    test_branch();
 }
 
 int main(int argc, char* argv[])
-- 
cgit