From 4fb5ee1a65b36c40bdc368b42da9872c70805133 Mon Sep 17 00:00:00 2001
From: Madeline Cronin <maddie.henlo@gmail.com>
Date: Wed, 4 Sep 2024 17:58:37 +0100
Subject: [PATCH] Implement control flow instructions.

---
 src/processor.rs | 49 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/src/processor.rs b/src/processor.rs
index c0391eb..e50816a 100644
--- a/src/processor.rs
+++ b/src/processor.rs
@@ -190,6 +190,55 @@ impl <'a> Processor<'a> {
                 self.reg[31] += 2;
                 return false
             }
+            0x0400 => { //Control flow
+                let toggles: (bool,bool,bool,bool,bool,bool) = ( //J,S,P,I,TT, see ISA for meanings
+                    (instruction.0 & 0x0080) != 0,
+                    (instruction.0 & 0x0040) != 0,
+                    (instruction.0 & 0x0020) != 0,
+                    (instruction.0 & 0x0010) != 0,
+                    (instruction.0 & 0x0008) != 0,
+                    (instruction.0 & 0x0004) != 0,
+                );
+                
+                if self.itr_toggle && toggles.3 { //Software interrupt
+                    if toggles.1 {   //Push what would have been the next address to stack
+                        self.memory[self.reg[30] as usize] = self.reg[31] + 2;
+                        self.reg[30] -= 1;
+                    }                 
+                    self.reg[31] = 0; //Jump to zero
+                    if toggles.4 { //Set the interrupt toggle
+                        self.itr_toggle = toggles.5;
+                    }
+
+                    return false
+                }
+                let address = {
+                    if toggles.2 { //Pop from the stack
+                        self.reg[30] += 1;
+                        self.memory[self.reg[30] as usize]
+                    }
+                    else { //Use a register value
+                        let operand: usize = (instruction.1 & 0xF800) >> 11 as usize;
+                        self.reg[operand]
+                    }
+                }
+
+                if toggles.1 { //Push to stack
+                    self.memory[self.reg[30] as usize] = self.reg[31] + 2;
+                    self.reg[30] -= 1;
+                }
+                if toggles.0 { //Relative jump
+                    self.reg[31].wrapping_add(address);
+                }
+                else { //Static jump
+                    self.reg[31] = address;
+                }
+
+                if toggles.4 { //Set the interrupt enable bit
+                    self.itr_toggle = toggles.5; 
+                }
+                return false
+            }
             _ => return true,
         }
     }
-- 
2.43.4