diff --git a/src/chip.rs b/src/chip.rs
index d097f75..cf1847b 100644
--- a/src/chip.rs
+++ b/src/chip.rs
@@ -1,3 +1,4 @@
+/*
 use cpu::CPU;
 
 pub struct Chip {
@@ -17,58 +18,4 @@ impl Chip {
         }
     }
 }
-
-pub const GPIO0: u16 = 0x000;
-pub const GPIO1: u16 = 0x001;
-pub const GPIO2: u16 = 0x002;
-pub const GPIO3: u16 = 0x003;
-pub const GPIO4: u16 = 0x004;
-pub const GPIO5: u16 = 0x005;
-pub const GPIO6: u16 = 0x006;
-pub const GPIO7: u16 = 0x007;
-pub const GPIO8: u16 = 0x008;
-pub const GPIO9: u16 = 0x009;
-pub const GPIOA: u16 = 0x00A;
-pub const GPIOB: u16 = 0x00B;
-pub const GPIOC: u16 = 0x00C;
-pub const GPIOD: u16 = 0x00D;
-pub const GPIOE: u16 = 0x00E;
-pub const GPIOF: u16 = 0x00F;
-pub const VPORT0_DIR: u16 = 0x010;
-pub const VPORT0_OUT: u16 = 0x011;
-pub const VPORT0_IN: u16 = 0x012;
-pub const VPORT0_INTFLAGS: u16 = 0x013;
-pub const VPORT1_DIR: u16 = 0x014;
-pub const VPORT1_OUT: u16 = 0x015;
-pub const VPORT1_IN: u16 = 0x016;
-pub const VPORT1_INTFLAGS: u16 = 0x017;
-pub const VPORT2_DIR: u16 = 0x018;
-pub const VPORT2_OUT: u16 = 0x019;
-pub const VPORT2_IN: u16 = 0x01A;
-pub const VPORT2_INTFLAGS: u16 = 0x01B;
-pub const VPORT3_DIR: u16 = 0x01C;
-pub const VPORT3_OUT: u16 = 0x01D;
-pub const VPORT3_IN: u16 = 0x01E;
-pub const VPORT3_INTFLAGS: u16 = 0x01F;
-pub const PRODSIGNATURES_ADCACAL0: u16 = 0x020;
-pub const PRODSIGNATURES_ADCACAL1: u16 = 0x021;
-pub const PRODSIGNATURES_ADCBCAL0: u16 = 0x024;
-pub const PRODSIGNATURES_ADCBCAL1: u16 = 0x025;
-pub const OCD_OCDR0: u16 = 0x02E;
-pub const OCD_OCDR1: u16 = 0x02F;
-pub const PRODSIGNATURES_DACA0OFFCAL: u16 = 0x030;
-pub const PRODSIGNATURES_DACA0GAINCAL: u16 = 0x031;
-pub const PRODSIGNATURES_DACB0OFFCAL: u16 = 0x032;
-pub const PRODSIGNATURES_DACB0GAINCAL: u16 = 0x033;
-pub const CCP: u16 = 0x034;
-pub const PRODSIGNATURES_DACA1GAINCAL: u16 = 0x035;
-pub const PRODSIGNATURES_DACB1OFFCAL: u16 = 0x036;
-pub const PRODSIGNATURES_DACB1GAINCAL: u16 = 0x037;
-pub const RAMPD: u16 = 0x038;
-pub const RAMPX: u16 = 0x039;
-pub const RAMPY: u16 = 0x03A;
-pub const RAMPZ: u16 = 0x03B;
-pub const EIND: u16 = 0x03C;
-pub const SPL: u16 = 0x03D;
-pub const SPH: u16 = 0x03E;
-pub const SREG: u16 = 0x03F;
+*/
\ No newline at end of file
diff --git a/src/chip_definitions.rs b/src/chip_definitions.rs
index 12d6218..cc5a597 100644
--- a/src/chip_definitions.rs
+++ b/src/chip_definitions.rs
@@ -1,64 +1,66 @@
-pub const GPIO0: u16 = 0x000;
-pub const GPIO1: u16 = 0x001;
-pub const GPIO2: u16 = 0x002;
-pub const GPIO3: u16 = 0x003;
-pub const GPIO4: u16 = 0x004;
-pub const GPIO5: u16 = 0x005;
-pub const GPIO6: u16 = 0x006;
-pub const GPIO7: u16 = 0x007;
-pub const GPIO8: u16 = 0x008;
-pub const GPIO9: u16 = 0x009;
-pub const GPIOA: u16 = 0x00A;
-pub const GPIOB: u16 = 0x00B;
-pub const GPIOC: u16 = 0x00C;
-pub const GPIOD: u16 = 0x00D;
-pub const GPIOE: u16 = 0x00E;
-pub const GPIOF: u16 = 0x00F;
-pub const VPORT0_DIR: u16 = 0x010;
-pub const VPORT0_OUT: u16 = 0x011;
-pub const VPORT0_IN: u16 = 0x012;
-pub const VPORT0_INTFLAGS: u16 = 0x013;
-pub const VPORT1_DIR: u16 = 0x014;
-pub const VPORT1_OUT: u16 = 0x015;
-pub const VPORT1_IN: u16 = 0x016;
-pub const VPORT1_INTFLAGS: u16 = 0x017;
-pub const VPORT2_DIR: u16 = 0x018;
-pub const VPORT2_OUT: u16 = 0x019;
-pub const VPORT2_IN: u16 = 0x01A;
-pub const VPORT2_INTFLAGS: u16 = 0x01B;
-pub const VPORT3_DIR: u16 = 0x01C;
-pub const VPORT3_OUT: u16 = 0x01D;
-pub const VPORT3_IN: u16 = 0x01E;
-pub const VPORT3_INTFLAGS: u16 = 0x01F;
-pub const PRODSIGNATURES_ADCACAL0: u16 = 0x020;
-pub const PRODSIGNATURES_ADCACAL1: u16 = 0x021;
-pub const PRODSIGNATURES_ADCBCAL0: u16 = 0x024;
-pub const PRODSIGNATURES_ADCBCAL1: u16 = 0x025;
-pub const OCD_OCDR0: u16 = 0x02E;
-pub const OCD_OCDR1: u16 = 0x02F;
-pub const PRODSIGNATURES_DACA0OFFCAL: u16 = 0x030;
-pub const PRODSIGNATURES_DACA0GAINCAL: u16 = 0x031;
-pub const PRODSIGNATURES_DACB0OFFCAL: u16 = 0x032;
-pub const PRODSIGNATURES_DACB0GAINCAL: u16 = 0x033;
-pub const CCP: u16 = 0x034;
-pub const PRODSIGNATURES_DACA1GAINCAL: u16 = 0x035;
-pub const PRODSIGNATURES_DACB1OFFCAL: u16 = 0x036;
-pub const PRODSIGNATURES_DACB1GAINCAL: u16 = 0x037;
-pub const RAMPD: u16 = 0x038;
-pub const RAMPX: u16 = 0x039;
-pub const RAMPY: u16 = 0x03A;
-pub const RAMPZ: u16 = 0x03B;
-pub const EIND: u16 = 0x03C;
-pub const SPL: u16 = 0x03D;
-pub const SPH: u16 = 0x03E;
-pub const SREG: u16 = 0x03F;
-
-pub const USARTC0_DATA: u16 = 0x8A0;
-pub const USARTC0_STATUS: u16 = 0x8A1;
-pub const USARTC0_CTRLA: u16 = 0x8A3;
-pub const USARTC0_CTRLB: u16 = 0x8A4;
-pub const USARTC0_CTRLC: u16 = 0x8A5;
-pub const USARTC0_BAUDCTRLA: u16 = 0x8A6;
-pub const USARTC0_BAUDCTRLB: u16 = 0x8A7;
-
-pub const OSC_STATUS: u16 = 0x051;
\ No newline at end of file
+#[allow(dead_code)]
+#[allow(non_camel_case_types)]
+pub enum IOAdress {
+    GPIO0 = 0x000,
+    GPIO1 = 0x001,
+    GPIO2 = 0x002,
+    GPIO3 = 0x003,
+    GPIO4 = 0x004,
+    GPIO5 = 0x005,
+    GPIO6 = 0x006,
+    GPIO7 = 0x007,
+    GPIO8 = 0x008,
+    GPIO9 = 0x009,
+    GPIOA = 0x00A,
+    GPIOB = 0x00B,
+    GPIOC = 0x00C,
+    GPIOD = 0x00D,
+    GPIOE = 0x00E,
+    GPIOF = 0x00F,
+    VPORT0_DIR = 0x010,
+    VPORT0_OUT = 0x011,
+    VPORT0_IN = 0x012,
+    VPORT0_INTFLAGS = 0x013,
+    VPORT1_DIR = 0x014,
+    VPORT1_OUT = 0x015,
+    VPORT1_IN = 0x016,
+    VPORT1_INTFLAGS = 0x017,
+    VPORT2_DIR = 0x018,
+    VPORT2_OUT = 0x019,
+    VPORT2_IN = 0x01A,
+    VPORT2_INTFLAGS = 0x01B,
+    VPORT3_DIR = 0x01C,
+    VPORT3_OUT = 0x01D,
+    VPORT3_IN = 0x01E,
+    VPORT3_INTFLAGS = 0x01F,
+    PRODSIGNATURES_ADCACAL0 = 0x020,
+    PRODSIGNATURES_ADCACAL1 = 0x021,
+    PRODSIGNATURES_ADCBCAL0 = 0x024,
+    PRODSIGNATURES_ADCBCAL1 = 0x025,
+    OCD_OCDR0 = 0x02E,
+    OCD_OCDR1 = 0x02F,
+    PRODSIGNATURES_DACA0OFFCAL = 0x030,
+    PRODSIGNATURES_DACA0GAINCAL = 0x031,
+    PRODSIGNATURES_DACB0OFFCAL = 0x032,
+    PRODSIGNATURES_DACB0GAINCAL = 0x033,
+    CCP = 0x034,
+    PRODSIGNATURES_DACA1GAINCAL = 0x035,
+    PRODSIGNATURES_DACB1OFFCAL = 0x036,
+    PRODSIGNATURES_DACB1GAINCAL = 0x037,
+    RAMPD = 0x038,
+    RAMPX = 0x039,
+    RAMPY = 0x03A,
+    RAMPZ = 0x03B,
+    EIND = 0x03C,
+    SPL = 0x03D,
+    SPH = 0x03E,
+    SREG = 0x03F,
+    OSC_STATUS = 0x051,
+    USARTC0_DATA = 0x8A0,
+    USARTC0_STATUS = 0x8A1,
+    USARTC0_CTRLA = 0x8A3,
+    USARTC0_CTRLB = 0x8A4,
+    USARTC0_CTRLC = 0x8A5,
+    USARTC0_BAUDCTRLA = 0x8A6,
+    USARTC0_BAUDCTRLB = 0x8A7,
+}
diff --git a/src/cpu.rs b/src/cpu.rs
index e59e461..db8165a 100644
--- a/src/cpu.rs
+++ b/src/cpu.rs
@@ -17,11 +17,6 @@ pub enum CPUError {
     Exit,
 }
 
-enum FlagUpdateMode {
-    HSVNZC,
-    HSVNZC_KeepZero,
-}
-
 // CPU
 #[derive(Debug)]
 pub struct CPU {
@@ -38,7 +33,7 @@ pub struct CPU {
 
 impl fmt::Display for CPU {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "CPU @ 0x{:X} (file offset = 0x{:X})\n", self.pc, 2 * self.pc)?;
+        write!(f, "CPU @ 0x{:X} (file offset = 0x{:X}) ", self.pc, 2 * self.pc)?;
         write!(f, "SREG: 0x{:02X}: ", self.sreg)?;
         for i in 0..7 {
             let t = self.sreg & (1 << i);
@@ -49,7 +44,6 @@ impl fmt::Display for CPU {
             }
         }
         write!(f, "\n")?;
-        write!(f, "Registers:\n")?;
         for i in 0..32 {
             write!(f, " R{:-2} = 0x{:02X}  ", i, self.registers[i])?;
             if (i + 1) % 10 == 0{
@@ -70,12 +64,12 @@ impl CPU {
     }
 
     fn get_sp(&self, mem: &[u8]) -> u16 {
-        mem[chip_definitions::SPL as usize] as u16 | ((mem[chip_definitions::SPH as usize] as u16) << 8)
+        mem[chip_definitions::IOAdress::SPL as usize] as u16 | ((mem[chip_definitions::IOAdress::SPH as usize] as u16) << 8)
     }
 
     fn set_sp(&self, mem: &mut [u8], val: u16) {
-        mem[chip_definitions::SPL as usize] = val as u8;
-        mem[chip_definitions::SPH as usize] = (val >> 8) as u8;
+        mem[chip_definitions::IOAdress::SPL as usize] = val as u8;
+        mem[chip_definitions::IOAdress::SPH as usize] = (val >> 8) as u8;
     }
 
 
@@ -123,8 +117,8 @@ impl CPU {
             Err(CPUError::OutOfBoundsException)
         } else {
             // TODO: Hooks
-            if addr == chip_definitions::USARTC0_DATA {
-                print!("UART_OUT:{: <3} ({}) ", val, (val as char).escape_debug());
+            if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
+                print!("USART_OUT:{: <3} ({}) ", val, (val as char).escape_debug());
             }
             ram[addr as usize] = val;
             Ok(())
@@ -137,18 +131,16 @@ impl CPU {
             Err(CPUError::OutOfBoundsException)
         } else {
             // TODO: Hooks
-            if addr == chip_definitions::USARTC0_DATA {
-                print!("Tring to read from USART!");
+            if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
                 return Err(CPUError::Exit);
-            } else if addr == chip_definitions::USARTC0_STATUS {
-                print!("Trying to read USART flags!");
+            } else if addr == chip_definitions::IOAdress::USARTC0_STATUS as _ {
                 if self.pc == 0x5AC {
                     // USART data check -> Yes, there is indeed data available!
                     return Ok(0x80);
                 } else {
                     return Ok(0x20); // Usart is ready to send.
                 }
-            } else if addr == chip_definitions::OSC_STATUS {
+            } else if addr == chip_definitions::IOAdress::OSC_STATUS as _ {
                 // HACK: Osci is set right..
                 return Ok(0x02);
             }
@@ -169,39 +161,80 @@ impl CPU {
         self.ram_read(ram, sp.wrapping_add(1))
     }
 
-    fn update_flags(&mut self, mode: FlagUpdateMode, rd: u8, rr: u8, s: u8) {
-        match mode {
-            FlagUpdateMode::HSVNZC => {
-                // H: Set if there was a borrow from bit 3; cleared otherwise
-                // S: N ⊕ V, For signed tests.
-                // V: Set if two's complement overflow resulted from the operation; cleared otherwise.
-                // N: R7 Set if MSB of the result is set; cleared otherwise.
-                // Z: Set if the result is $00; cleared otherwise.
-                // C: Set if the absolute value of K is larger than the absolute value of Rd; cleared otherwise.
-                // cpu_.sreg_.C = ((~Rd & Rr) | (Rr & R) | (R & ~Rd)) & 0x80;
-                self.set_clear_flag(StatusFlag::Carry, ((!rd & rr) | (rr & s) | (s & !rd)) & 0x80 == 0x80);
-                self.set_clear_flag(StatusFlag::Zero, s == 0);
-                self.set_clear_flag(StatusFlag::Negative, (s & 0x80) == 0x80);
-                self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((rd & !rr & !s) | (!rd & rr & s)) & 0x80 == 0x80);
-                let t = self.test_flag(StatusFlag::Negative) ^ self.test_flag(StatusFlag::TwosComplementOverflow);
-                self.set_clear_flag(StatusFlag::SignBit, t);
-                self.set_clear_flag(StatusFlag::HalfCarry, ((!rd & rr) | (rr & s) | (s & !rd)) & 0x08 == 0x08);
-                //cpu_.sreg_.H = ((~Rd & Rr) | (Rr & R) | (R & ~Rd)) & 0x08;
+    // Flag update functions on a single value:
+    fn update_flags_zns_8(&mut self, v: u8) {
+        self.set_clear_flag(StatusFlag::Zero, v == 0);
+        self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
+        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
+        self.set_clear_flag(StatusFlag::SignBit, (v & 0x80 == 0x80) ^ V);
+    }
 
-                // self.set_clear_flag(StatusFlag::Zero, r.wrapping_sub(v) == 0);
-                // self.set_clear_flag(StatusFlag::Carry, v > r);
-            }
-            FlagUpdateMode::HSVNZC_KeepZero => {
-                self.set_clear_flag(StatusFlag::Carry, ((!rd & rr) | (rr & s) | (s & !rd)) & 0x80 == 0x80);
-                let t = self.test_flag(StatusFlag::Zero);
-                self.set_clear_flag(StatusFlag::Zero, s == 0 && t);
-                self.set_clear_flag(StatusFlag::Negative, (s & 0x80) == 0x80);
-                self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((rd & !rr & !s) | (!rd & rr & s)) & 0x80 == 0x80);
-                let t = self.test_flag(StatusFlag::Negative) ^ self.test_flag(StatusFlag::TwosComplementOverflow);
-                self.set_clear_flag(StatusFlag::SignBit, t);
-                self.set_clear_flag(StatusFlag::HalfCarry, ((!rd & rr) | (rr & s) | (s & !rd)) & 0x08 == 0x08);
-            }
+    fn update_flags_zns_16(&mut self, v: u16) {
+        self.set_clear_flag(StatusFlag::Zero, v == 0);
+        self.set_clear_flag(StatusFlag::Negative, v & 0x8000 == 0x8000);
+        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
+        self.set_clear_flag(StatusFlag::SignBit, (v & 0x8000 == 0x8000) ^ V);
+    }
+
+    // Copied from simavr, let's trust them for now.
+    fn update_flags_add_zns(&mut self, result: u8, d: u8, r: u8) {
+        let add_carry = (d & r) | (r & !result) | (!result & d);
+        self.set_clear_flag(StatusFlag::HalfCarry, add_carry & 0b1000 != 0);
+        self.set_clear_flag(StatusFlag::Carry, add_carry & 0b1000_0000 != 0);
+        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & r & !result) | (!d & !r & result)) & 0x80 == 0x80);
+        self.update_flags_zns_8(result);
+    }
+
+    fn update_flags_sub_zns(&mut self, result: u8, d: u8, r: u8) {
+        let sub_carry = (!d & r) | (r & result) | (result & !d);
+        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
+        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
+        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80);
+        self.update_flags_zns_8(result);
+    }
+
+    fn update_flags_Rzns(&mut self, r: u8) {
+        if r != 0 {
+            self.clear_flag(StatusFlag::Zero);
         }
+        self.set_clear_flag(StatusFlag::Negative, r & 0x80 == 0x80);
+        let b = self.test_flag(StatusFlag::TwosComplementOverflow);
+        self.set_clear_flag(StatusFlag::SignBit, r & 0x80 == 0x80 && b);
+    }
+
+    fn update_flags_sub_Rzns(&mut self, result: u8, d: u8, r: u8) {
+        let sub_carry = (!d & r) | (r & result) | (result & !d);
+        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
+        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
+        self.set_clear_flag(StatusFlag::TwosComplementOverflow, ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80);
+        self.update_flags_Rzns(result);
+    }
+
+    fn update_flags_zcvs(&mut self, result: u8, vr: u8) {
+        // Workaround lexicalic lifetimes.
+        let z = result == 0;
+        let c = vr & 1 == 1;
+        let n = self.test_flag(StatusFlag::Negative);
+        self.set_clear_flag(StatusFlag::Zero, z);
+        self.set_clear_flag(StatusFlag::Carry, c);
+        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
+        self.set_clear_flag(StatusFlag::SignBit, c);
+    }
+
+    fn update_flags_zcnvs(&mut self, result: u8, vr: u8) {
+        let z = result == 0;
+        let c = vr & 1 == 1;
+        let n = result & 0x80 == 0x80;
+        self.set_clear_flag(StatusFlag::Zero, z);
+        self.set_clear_flag(StatusFlag::Negative, n);
+        self.set_clear_flag(StatusFlag::Carry, c);
+        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
+        self.set_clear_flag(StatusFlag::SignBit, c);
+    }
+
+    fn update_flags_znv0s(&mut self, v: u8) {
+        self.clear_flag(StatusFlag::TwosComplementOverflow);
+        self.update_flags_zns_8(v);
     }
 
     // Returns # of ticks the executed instruction took
@@ -222,9 +255,15 @@ impl CPU {
         // Instruction execute
         match ins {
             Instruction::JMP(v) => self.pc = v,
-            Instruction::CLR(ref r) => self.set_register(r, 0),
-            Instruction::EOR(ref d, ref r) =>
-                self.registers[d.as_usize()] ^= self.registers[r.as_usize()],
+            Instruction::CLR(ref r) => {
+                self.set_register(r, 0);
+                self.update_flags_znv0s(0);
+            },
+            Instruction::EOR(ref d, ref r) => {
+                self.registers[d.as_usize()] ^= self.registers[r.as_usize()];
+                let r = self.registers[d.as_usize()];
+                self.update_flags_znv0s(r);
+            },
             Instruction::LDI(ref r, v) => self.set_register(r, v),
             Instruction::SER(ref r) => self.set_register(r, 0xFF),
             Instruction::RJMP(v) => self.pc = self.pc.wrapping_add(v as _),
@@ -232,23 +271,24 @@ impl CPU {
             Instruction::SET_FLAG(v) => self.set_flag(v),
             Instruction::CPI(ref r, v) => {
                 let rv = self.get_register(r);
-                self.update_flags(FlagUpdateMode::HSVNZC, rv, v, rv.wrapping_sub(v));
+                self.update_flags_sub_zns(rv.wrapping_sub(v), rv, v);
             },
             Instruction::CP(ref r, ref i) => {
                 let rv = self.get_register(r);
                 let iv = self.get_register(i);
-                self.update_flags(FlagUpdateMode::HSVNZC, rv, iv, rv.wrapping_sub(iv));
+                self.update_flags_sub_zns(rv.wrapping_sub(iv), rv, iv);
             },
-            Instruction::CPC(ref rd, ref rr) => {
-                let rd: u8 = self.get_register(rd);
-                let rr: u8 = self.get_register(rr);
+            Instruction::CPC(ref d, ref r) => {
+                let rd: u8 = self.get_register(d);
+                let rr: u8 = self.get_register(r);
 
                 let s = if self.test_flag(StatusFlag::Carry) {
                     rd.wrapping_sub(rr).wrapping_sub(1)
                 } else {
                     rd.wrapping_sub(rr)
                 };
-                self.update_flags(FlagUpdateMode::HSVNZC_KeepZero, rd, rr, s);
+
+                self.update_flags_sub_Rzns(s, rd, rr);
             },
             Instruction::BR_IF(offset, flag, test) => {
                 if self.test_flag(flag) == test {
@@ -351,34 +391,33 @@ impl CPU {
                 let v = self.registers[reg.as_usize()];
                 self.push(ram, v)?;
             },
-            Instruction::SUBI(ref reg, ref v) => {
-                let v = self.registers[reg.as_usize()].wrapping_sub(*v);
+            Instruction::SUBI(ref d, ref v) => {
+                let dv = self.get_register(d);
+                let vres = dv.wrapping_sub(*v);
 
-                // HACK! Make use of more flags...
-                let r = self.get_register(reg);
-                self.set_clear_flag(StatusFlag::Carry, v > r);
-                self.registers[reg.as_usize()] = v;
-                // TODO: flags
+                self.update_flags_sub_zns(vres, dv, *v);
+                self.set_register(d, vres);
             },
             Instruction::SBC(ref d, ref r) => {
-                let mut v = self.registers[d.as_usize()].wrapping_sub(self.registers[r.as_usize()]);
+                let dv = self.get_register(d);
+                let rv = self.get_register(r);
+                let mut vres = dv.wrapping_sub(rv);
                 if self.test_flag(StatusFlag::Carry) {
-                    v = v.wrapping_sub(1);
+                    vres = vres.wrapping_sub(1);
                 }
-                // HACK! Make use of more flags...
-                let dd = self.get_register(d);
-                self.set_clear_flag(StatusFlag::Carry, v > dd);
-                self.registers[d.as_usize()] = v;
+
+                self.update_flags_sub_Rzns(vres, dv, rv);
+                self.set_register(d, vres);
             },
-            Instruction::SBCI(ref d, ref r) => {
-                let mut v = self.registers[d.as_usize()].wrapping_sub(*r);
+            Instruction::SBCI(ref d, ref v) => {
+                let dv = self.get_register(d);
+                let mut vres = dv.wrapping_sub(*v);
                 if self.test_flag(StatusFlag::Carry) {
-                    v = v.wrapping_sub(1);
+                    vres = vres.wrapping_sub(1);
                 }
-                // HACK! Make use of more flags...
-                let dd = self.get_register(d);
-                self.set_clear_flag(StatusFlag::Carry, v > dd);
-                self.registers[d.as_usize()] = v;
+
+                self.update_flags_sub_Rzns(vres, dv, *v);
+                self.set_register(d, vres);
             },
             Instruction::MOVW(ref d, ref r) => {
                 let t = self.get_register_pair(r);
@@ -386,26 +425,22 @@ impl CPU {
             },
             Instruction::OR(ref d, ref r) => {
                 let t = self.get_register(d) | self.get_register(r);
-                self.set_clear_flag(StatusFlag::Zero, t == 0);
                 self.set_register(d, t);
+                self.update_flags_znv0s(t);
             }
             Instruction::ORI(ref d, ref v) => {
                 let t = self.get_register(d) | *v;
-                self.set_clear_flag(StatusFlag::Zero, t == 0);
                 self.set_register(d, t);
+                self.update_flags_znv0s(t);
             },
             Instruction::AND(ref d, ref r) => {
                 let t = self.get_register(d) & self.get_register(r);
-                self.set_clear_flag(StatusFlag::Zero, t == 0);
-                self.set_clear_flag(StatusFlag::Negative, t & 0x80 == 0x80);
                 self.set_register(d, t);
+                self.update_flags_znv0s(t);
             },
             Instruction::TST(ref r) => {
                 let t = self.get_register(r);
-                self.set_clear_flag(StatusFlag::Zero, t == 0);
-                self.set_clear_flag(StatusFlag::Negative, t & 0x80 == 0x80);
-                self.set_clear_flag(StatusFlag::SignBit, t & 0x80 == 0x80);
-                self.clear_flag(StatusFlag::TwosComplementOverflow);
+                self.update_flags_znv0s(t);
             },
             Instruction::ANDI(ref d, ref v) => {
                 let t = self.get_register(d) & *v;
@@ -430,43 +465,46 @@ impl CPU {
                 self.set_register(d, t);
             },
             Instruction::SUB(ref d, ref r) => {
-                let t = self.get_register(d).wrapping_sub(self.get_register(r));
-                let dd = self.get_register(d);
-                self.set_clear_flag(StatusFlag::Carry, t > dd);
-                self.set_clear_flag(StatusFlag::Zero, t == 0);
-                self.set_register(d, t);
+                let dv = self.get_register(d);
+                let rv = self.get_register(r);
+                let res = dv.wrapping_sub(rv);
+                self.update_flags_sub_zns(res, dv, rv);
+                self.set_register(d, res);
             },
             Instruction::MOV(ref d, ref r) => {
                 let v = self.get_register(r);
                 self.set_register(d, v);
             },
             Instruction::ADIW(ref d, ref v) => {
-                let v = self.get_register_pair(d).wrapping_add(*v);
-                let dd = self.get_register_pair(d);
-                self.set_clear_flag(StatusFlag::Carry, v < dd);
-                self.set_clear_flag(StatusFlag::Zero, v == 0);
-                self.set_register_pair(d, v);
+                let dv = self.get_register_pair(d);
+                let res = dv.wrapping_add(*v);
+                self.set_register_pair(d, res);
+                self.set_clear_flag(StatusFlag::TwosComplementOverflow, (dv & !res) & 0x8000 == 0x8000);
+                self.set_clear_flag(StatusFlag::Carry, (!dv & res) & 0x8000 == 0x8000);
+                self.update_flags_zns_16(res);
             },
             Instruction::SBIW(ref d, ref v) => {
-                let v = self.get_register_pair(d).wrapping_sub(*v as _);
-                let dd = self.get_register_pair(d);
-                self.set_clear_flag(StatusFlag::Carry, v > dd);
-                self.set_clear_flag(StatusFlag::Zero, v == 0);
-                self.set_register_pair(d, v);
+                let dv = self.get_register_pair(d);
+                let res = dv.wrapping_sub(*v as _);
+                self.set_clear_flag(StatusFlag::TwosComplementOverflow, (!dv & res) & 0x8000 == 0x8000);
+                self.set_clear_flag(StatusFlag::Carry, (dv & !res) & 0x8000 == 0x8000);
+                self.update_flags_zns_16(res);
+                self.set_register_pair(d, res);
             },
             Instruction::DEC(ref r) => {
-                let v = self.get_register(r).wrapping_sub(1);
-                self.set_register(r, v);
-                self.set_clear_flag(StatusFlag::Carry, v == 0xFF);
-                self.set_clear_flag(StatusFlag::Zero, v == 0);
-                self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
+                let rv = self.get_register(r);
+                let res = rv.wrapping_sub(1);
+                self.set_register(r, res);
+                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x7F);
+                self.update_flags_zns_8(res);
             },
             Instruction::INC(ref r) => {
-                let v = self.get_register(r).wrapping_add(1);
-                self.set_register(r, v);
-                self.set_clear_flag(StatusFlag::Carry, v == 0);
-                self.set_clear_flag(StatusFlag::Zero, v == 0);
-                self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
+                let rv = self.get_register(r);
+                let res = rv.wrapping_add(1);
+                self.set_register(r, res);
+
+                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
+                self.update_flags_zns_8(res);
             },
             Instruction::STS16(ref addr, ref r) => {
                 self.ram_write(ram, *addr, self.get_register(r))?;
@@ -495,18 +533,24 @@ impl CPU {
                 self.set_register(r, v << 1 | c);
                 self.set_clear_flag(StatusFlag::Zero, (v << 1) | c == 0);
             },
+            Instruction::ASR(ref r) => {
+                let rv = self.get_register(r);
+                let res = rv >> 1 | (rv & 0x80);
+                self.set_register(r, res);
+                self.update_flags_zcnvs(res, rv);
+            },
             Instruction::LSR(ref r) => {
-                let v = self.get_register(r);
-                self.set_clear_flag(StatusFlag::Carry, v & 1 == 1);
-                self.set_register(r, v >> 1);
-                self.set_clear_flag(StatusFlag::Zero, (v >> 1) == 0);
+                let rv = self.get_register(r);
+                self.set_register(r, rv >> 1);
+                self.clear_flag(StatusFlag::Negative);
+                self.update_flags_zcvs(rv >> 1, rv);
             },
             Instruction::ROR(ref r) => {
-                let v = self.get_register(r);
+                let rv = self.get_register(r);
                 let c = if self.test_flag(StatusFlag::Carry) { 0x80 } else { 0 };
-                self.set_clear_flag(StatusFlag::Carry, v & 1 == 1);
-                self.set_register(r, v >> 1 | c);
-                self.set_clear_flag(StatusFlag::Zero, (v >> 1 | c) == 0);
+                let res = rv >> 1 | c;
+                self.set_register(r, res);
+                self.update_flags_zcnvs(res, rv);
             },
             Instruction::SBRS(ref r, ref bit) => {
                 let r = self.get_register(r);
@@ -527,9 +571,20 @@ impl CPU {
                 }
             },
             Instruction::COM(ref r) => {
-                // Rd ← $FF - Rd
-                let rv = 0xFFu8.wrapping_sub(self.get_register(r));
-                self.set_register(r, rv);
+                let rv = self.get_register(r);
+                let res = 0xFFu8.wrapping_sub(rv);
+                self.set_register(r, res);
+                self.update_flags_znv0s(res);
+                self.set_flag(StatusFlag::Carry);
+            },
+            Instruction::NEG(ref r) => {
+                let rv = self.get_register(r);
+                let res = 0u8.wrapping_sub(rv);
+                self.set_register(r, res);
+                self.set_clear_flag(StatusFlag::HalfCarry, (rv | res) & 0x08 == 0x08);
+                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
+                self.set_clear_flag(StatusFlag::Carry, res != 0);
+                self.update_flags_zns_8(rv);
             },
             Instruction::MUL(ref r, ref d) => {
                 //  R1:R0 ← Rd × Rr(unsigned ← unsigned × unsigned)
@@ -545,6 +600,10 @@ impl CPU {
                 let r = self.get_register(r);
                 self.set_clear_flag(StatusFlag::BitCopyStorage, r & (1 << *v) != 0);
             },
+            Instruction::SWAP(ref r) => {
+                let rv = self.get_register(r);
+                self.set_register(r, rv >> 4 | ((rv << 4) & 0xF0));
+            },
             Instruction::NOP => {},
             _ => return Err(CPUError::UnimplementedInstruction)
         }
diff --git a/src/decoder.rs b/src/decoder.rs
index 0b17eba..996f8dd 100644
--- a/src/decoder.rs
+++ b/src/decoder.rs
@@ -18,6 +18,7 @@ pub enum IncrementMode {
 }
 
 #[derive(Debug)]
+#[allow(non_camel_case_types)]
 pub enum Instruction {
     ADC(GeneralPurposeRegister, GeneralPurposeRegister),
     ADD(GeneralPurposeRegister, GeneralPurposeRegister),
@@ -85,7 +86,7 @@ pub enum Instruction {
     SBIC(IORegister, u8),
     SBIS(IORegister, u8),
     SBIW(GeneralPurposeRegisterPair, u16),
-    SBR(GeneralPurposeRegister, u8),
+    // SBR(GeneralPurposeRegister, u8), => is the same as ORI, so ignore this :)
     SBRC(GeneralPurposeRegister, u8),
     SBRS(GeneralPurposeRegister, u8),
     SER(GeneralPurposeRegister),
@@ -173,7 +174,6 @@ impl fmt::Display for Instruction {
             Instruction::SBIC(ref ior, v) => write!(f, "SBIC {}, {}", ior, v),
             Instruction::SBIS(ref ior, v) => write!(f, "SBIS {}, {}", ior, v),
             Instruction::SBIW(ref r, v) => write!(f, "SBIW {}, {}", r, v),
-            Instruction::SBR(ref r, v) => write!(f, "SBR {}, {}", r, v),
             Instruction::SBRC(ref r, v) => write!(f, "SBRC {}, {}", r, v),
             Instruction::SBRS(ref r, v) => write!(f, "SBRS {}, {}", r, v),
             Instruction::SER(ref r) => write!(f, "SER {}", r),
diff --git a/src/main.rs b/src/main.rs
index 27ec769..fd02617 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -16,51 +16,18 @@ fn main() {
     let mut ram = [0u8; 8 * 1024 + 8 * 1024];
     let mut cpu = cpu::CPU::new();
 
-    // Patch some loops cause we don't emulate the devices yet.
-    // (Wait for OSC_STATUS - currently fixed by setting the reg)
-    // rom[2 * 0x177] = 0x00;
-    // rom[2 * 0x177 + 1] = 0x00;
-
-    // Some call (vprintf), to limit the bug surface? With this it crashes
-    // differently, so no real alternative :(
-    /*
-    rom[0x1d58] = 0x00;
-    rom[0x1d58 + 1] = 0x00;
-    rom[0x1d58 + 2] = 0x00;
-    rom[0x1d58 + 3] = 0x00;
-    */
-
-    for _ in 0..2800000 {
+    for _ in 0..280000 {
         let r = cpu.step(&mut rom, &mut ram);
-        print!("[r28={:02X} r29={:02X}] ", cpu.registers[28], cpu.registers[29]);
         println!("{:?}", r);
         match r {
             Err(cpu::CPUError::OutOfBoundsException) => break,
             Err(cpu::CPUError::Exit) => break,
+            Err(cpu::CPUError::UnimplementedInstruction) => break,
             _ => {}
         }
-
-        /*
-        if cpu.pc == 0xaea / 2 {
-            println!("{}", cpu);
-        }
-        */
     }
     println!("{}", cpu);
-
-    write_file("/tmp/endstate.ram", &ram).unwrap();
-
-    /*
-    let mut off = 0;
-    while off < data.len() {
-        let instr = decoder::decode(&data[off..]);
-        println!("{:X}: {:?}", off, instr);
-        match instr {
-            Ok(o) => off += o.size(),
-            _ => off += 2,
-        }
-    }
-    */
+    write_file("ram.dmp", &ram).unwrap();
 }
 
 pub fn read_file<P: AsRef<Path>>(rom_path: P) -> Result<Box<[u8]>, io::Error> {