1 files changed, 46 insertions, 12 deletions

diff --git a/src/core.rs b/src/core.rs
index 021b04f..b51cf80 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -6,6 +6,7 @@ pub enum Opcode {
     Push,
     Pop,
     Illegal,
+    Sjmp,
 }
 
 #[derive(Debug, Clone)]
@@ -20,6 +21,7 @@ pub struct Op {
 const OPCODE_NOP: u8 = 0x00;
 const OPCODE_PUSH: u8 = 0xC0;
 const OPCODE_POP: u8 = 0xD0;
+const OPCODE_SJMP: u8 = 0xD2;
 
 impl fmt::Display for Opcode {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
@@ -40,7 +42,7 @@ pub struct Core {
 
 impl Core {
     pub fn new() -> Self {
-        Core { pc: 0, ram: [0; u16::max_value() as usize + 1]}
+        Self { pc: 0, ram: [0; u16::max_value() as usize + 1] }
     }
 
     pub fn step(&mut self) -> u16 {
@@ -53,39 +55,71 @@ impl Core {
         self.ram[self.pc as usize].op().0
     }
 
+    ///
+    /// Increment PC
+    /// Return the whole operation
+    ///
     fn fetch(&mut self) -> Op {
-        let (opcode, size) = self.ram[self.pc as usize].op();
+        let mut operand1 = None;
+        let opcode = self.ram[self.pc as usize].opcode();
+        let size = self.ram[self.pc as usize].opsize();
+        if size == 2 {
+            operand1 = Some(self.ram[self.pc as usize]);
+        }
         self.pc += size as u16;
         Op {
             opcode,
             size,
-            operand1: None,
+            operand1,
             operand2: None,
             operand3: None,
         }
     }
 
-    fn exec(&mut self, op: Op) -> () {
+    fn exec(&mut self, op: Op) {
         match op.opcode {
             Opcode::Nop => (),
             Opcode::Illegal => (),
             Opcode::Push => (),
             Opcode::Pop => (),
+            Opcode::Sjmp => self.sjmp(op.operand1.unwrap() as i8),
         }
     }
+
+    fn sjmp(&mut self, reladdr: i8) {
+        self.pc = (self.pc as i16 + reladdr as i16) as u16;
+    }
 }
 
-pub trait Isa {
-    fn op(&self) -> (Opcode, usize);
+type Register = u8;
+
+pub trait Isa8051 {
+    fn op(&self) -> (Opcode, usize, Option<Register>);
+    fn opcode(&self) -> Opcode;
+    fn opsize(&self) -> usize;
+    fn opreg(&self) -> Option<Register>;
 }
 
-impl Isa for u8 {
-    fn op(&self) -> (Opcode, usize) {
+impl Isa8051 for u8 {
+    fn op(&self) -> (Opcode, usize, Option<Register>) {
         match *self {
-            OPCODE_NOP  => (Opcode::Nop,        1),
-            OPCODE_PUSH => (Opcode::Push,       2),
-            OPCODE_POP  => (Opcode::Pop,        2),
-            _           => (Opcode::Illegal,    1),
+            OPCODE_NOP  => (Opcode::Nop,        1, None),
+            OPCODE_PUSH => (Opcode::Push,       2, None),
+            OPCODE_POP  => (Opcode::Pop,        2, None),
+            OPCODE_SJMP => (Opcode::Sjmp,       2, None),
+            _           => (Opcode::Illegal,    1, None),
         }
     }
+
+    fn opcode(&self) -> Opcode {
+        self.op().0
+    }
+
+    fn opsize(&self) -> usize {
+        self.op().1
+    }
+
+    fn opreg(&self) -> Option<Register> {
+        self.op().2
+    }
 }