1 files changed, 32 insertions, 6 deletions
diff --git a/src/core.rs b/src/core.rs
index a3b4541..37886c5 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -6,6 +6,7 @@ pub enum Opname {
     Nop,
     Ajmp,
     Ljmp,
+    Jnz,
     Mov,
     Push,
     Pop,
@@ -87,6 +88,7 @@ const OPCODE_LJMP: u8 = 0x02;
 const OPCODE_AJMP_P1: u8 = 0x21;
 const OPCODE_AJMP_P2: u8 = 0x41;
 const OPCODE_AJMP_P3: u8 = 0x61;
+const OPCODE_JNZ: u8 = 0x70;
 const OPCODE_MOV_A_DATA: u8 = 0x74;
 const OPCODE_MOV_DIRECT_DATA: u8 = 0x75;
 const OPCODE_SJMP: u8 = 0x80;
@@ -151,6 +153,12 @@ const OP_LUT: [IncompleteOp; u8::max_value() as usize + 1] = {
         operand1: Some(IncompleteOperand::Addr11(3)),
         operand2: None,
     };
+    lut[OPCODE_JNZ as usize] = IncompleteOp {
+        opname: Opname::Jnz,
+        size: 2,
+        operand1: Some(IncompleteOperand::Reladdr),
+        operand2: None,
+    };
     lut[OPCODE_MOV_A_DATA as usize] = IncompleteOp {
         opname: Opname::Mov,
         size: 2,
@@ -275,7 +283,7 @@ const OP_LUT: [IncompleteOp; u8::max_value() as usize + 1] = {
 };
 
 impl Register {
-    fn to_ram_offset(&self) -> u8 {
+    fn to_ram_offset(self) -> u8 {
         match self {
             Register::R0 => RAM_OFFSET_R0,
             Register::R1 => RAM_OFFSET_R1,
@@ -403,6 +411,16 @@ impl Core {
                     panic!("LJMP has no operand given")
                 }
             }
+            Opname::Jnz => {
+                if let Some(operand) = op.operand1 {
+                    match operand {
+                        Operand::Reladdr(addr) => self.jnz(addr),
+                        _ => panic!("JNZ got incompatible operand"),
+                    }
+                } else {
+                    panic!("JNZ has no operand given")
+                }
+            }
         }
     }
 
@@ -420,7 +438,7 @@ impl Core {
                 Operand::Reladdr(self.rom.array[(self.pc as usize + offset)] as i8)
             }
             IncompleteOperand::Addr11(high) => Operand::Addr11(
-                self.rom.array[(self.pc as usize + offset)] as u16 + (u16::from(high) << 8),
+                u16::from(self.rom.array[(self.pc as usize + offset)]) + (u16::from(high) << 8),
             ),
             IncompleteOperand::Addr16 => Operand::Addr16(
                 (u16::from(self.rom.array[(self.pc as usize + offset)]) << 8)
@@ -452,12 +470,20 @@ impl Core {
         self.pc = (self.pc as i16 + i16::from(reladdr)) as u16;
     }
 
-    fn ajmp(&mut self, pageaddr: u16) {
-        self.pc = (self.pc & !0x4FFu16) + pageaddr;
+    fn ajmp(&mut self, addr11: u16) {
+        assert!(addr11 <= 0x4FFu16);
+        // Clear 11 low bits of PC and add address, that is within these 11 bits
+        self.pc = (self.pc & !0x4FFu16) & addr11;
     }
 
-    fn ljmp(&mut self, pageaddr: u16) {
-        self.pc = pageaddr;
+    fn ljmp(&mut self, addr: u16) {
+        self.pc = addr;
+    }
+
+    fn jnz(&mut self, reladdr: i8) {
+        if self.ram[usize::from(RAM_OFFSET_ACC)] != 0 {
+            self.pc = (self.pc as i16 + i16::from(reladdr)) as u16;
+        }
     }
 }