refactor: added peripherals and cpu feature folders

6 files changed, 1687 insertions, 0 deletions

diff --git a/src/cpu/cpu.spec.ts b/src/cpu/cpu.spec.ts
new file mode 100644
index 0000000..92df7ee
--- /dev/null
+++ b/src/cpu/cpu.spec.ts
@@ -0,0 +1,8 @@
+import { CPU } from './cpu';
+
+describe('cpu', () => {
+  it('should set initial value of SP to the last address of internal SRAM', () => {
+    const cpu = new CPU(new Uint16Array(1024), 0x1000);
+    expect(cpu.SP).toEqual(0x10ff);
+  });
+});
diff --git a/src/cpu/cpu.ts b/src/cpu/cpu.ts
new file mode 100644
index 0000000..93f79d0
--- /dev/null
+++ b/src/cpu/cpu.ts
@@ -0,0 +1,78 @@
+/**
+ * AVR 8 CPU data structures
+ * Part of AVR8js
+ *
+ * Copyright (C) 2019, Uri Shaked
+ */
+
+import { u16, u8 } from '../types';
+
+const registerSpace = 0x100;
+
+// eslint-disable-next-line @typescript-eslint/interface-name-prefix
+export interface ICPU {
+  readonly data: Uint8Array;
+  readonly dataView: DataView;
+  readonly progMem: Uint16Array;
+  readonly progBytes: Uint8Array;
+  pc: u16;
+  cycles: number;
+
+  readData(addr: u16): u8;
+  writeData(addr: u16, value: u8): void;
+}
+
+export type CPUMemoryHook = (value: u8, oldValue: u8, addr: u16) => boolean | void;
+export interface CPUMemoryHooks {
+  [key: number]: CPUMemoryHook;
+}
+
+export class CPU implements ICPU {
+  readonly data: Uint8Array = new Uint8Array(this.sramBytes + registerSpace);
+  readonly data16 = new Uint16Array(this.data.buffer);
+  readonly dataView = new DataView(this.data.buffer);
+  readonly progBytes = new Uint8Array(this.progMem.buffer);
+  readonly writeHooks: CPUMemoryHooks = [];
+
+  pc = 0;
+  cycles = 0;
+
+  constructor(public progMem: Uint16Array, private sramBytes = 8192) {
+    this.reset();
+  }
+
+  reset() {
+    this.data.fill(0);
+    this.SP = this.data.length - 1;
+  }
+
+  readData(addr: number) {
+    return this.data[addr];
+  }
+
+  writeData(addr: number, value: number) {
+    const hook = this.writeHooks[addr];
+    if (hook) {
+      if (hook(value, this.data[addr], addr)) {
+        return;
+      }
+    }
+    this.data[addr] = value;
+  }
+
+  get SP() {
+    return this.dataView.getUint16(93, true);
+  }
+
+  set SP(value: number) {
+    this.dataView.setUint16(93, value, true);
+  }
+
+  get SREG() {
+    return this.data[95];
+  }
+
+  get interruptsEnabled() {
+    return this.SREG & 0x80 ? true : false;
+  }
+}
diff --git a/src/cpu/instruction.spec.ts b/src/cpu/instruction.spec.ts
new file mode 100644
index 0000000..2c5244e
--- /dev/null
+++ b/src/cpu/instruction.spec.ts
@@ -0,0 +1,837 @@
+import { CPU } from './cpu';
+import { avrInstruction } from './instruction';
+
+describe('avrInstruction', () => {
+  let cpu: CPU;
+
+  beforeEach(() => {
+    cpu = new CPU(new Uint16Array(0x8000));
+  });
+
+  function loadProgram(bytes: string) {
+    const progBuf = cpu.progBytes;
+    for (let i = 0; i < bytes.length; i += 2) {
+      progBuf[i / 2] = parseInt(bytes.substr(i, 2), 16);
+    }
+  }
+
+  it('should execute `ADC r0, r1` instruction when carry is on', () => {
+    loadProgram('011c');
+    cpu.data[0] = 10; // r0 <- 10
+    cpu.data[1] = 20; // r1 <- 20
+    cpu.data[95] = 0b00000001; // SREG <- -------C
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0]).toEqual(31);
+    expect(cpu.data[95]).toEqual(0); // SREG: --------
+  });
+
+  it('should execute `ADC r0, r1` instruction when carry is on and the result overflows', () => {
+    loadProgram('011c');
+    cpu.data[0] = 10; // r0 <- 10
+    cpu.data[1] = 245; // r1 <- 20
+    cpu.data[95] = 0b00000001; // SREG <- -------C
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0]).toEqual(0);
+    expect(cpu.data[95]).toEqual(0b00100011); // SREG: --H---ZC
+  });
+
+  it('should execute `BCLR 2` instruction', () => {
+    loadProgram('a894');
+    cpu.data[95] = 0xff; // SREG <- 0xff
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0xfb);
+  });
+
+  it('should execute `BLD r4, 7` instruction', () => {
+    loadProgram('47f8');
+    cpu.data[4] = 0x15; // r <- 0x15
+    cpu.data[95] = 0x40; // SREG <- 0x40
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[4]).toEqual(0x95);
+    expect(cpu.data[95]).toEqual(0x40);
+  });
+
+  it('should execute `BRBC 0, +8` instruction when SREG.C is clear', () => {
+    loadProgram('20f4');
+    cpu.data[95] = 0b00001000; // SREG: V
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1 + 8 / 2);
+    expect(cpu.cycles).toEqual(2);
+  });
+
+  it('should execute `BRBC 0, +8` instruction when SREG.C is set', () => {
+    loadProgram('20f4');
+    cpu.data[95] = 0b00000001; // SREG: C
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+  });
+
+  it('should execute `BRBS 3, 92` instruction when SREG.V is set', () => {
+    loadProgram('73f1');
+    cpu.data[95] = 0b00001000; // SREG: V
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1 + 92 / 2);
+    expect(cpu.cycles).toEqual(2);
+  });
+
+  it('should execute `BRBS 3, -4` instruction when SREG.V is set', () => {
+    loadProgram('0000f3f3');
+    cpu.data[95] = 0b00001000; // SREG: V
+    avrInstruction(cpu);
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0);
+    expect(cpu.cycles).toEqual(3); // 1 for NOP, 2 for BRBS
+  });
+
+  it('should execute `BRBS 3, -4` instruction when SREG.V is clear', () => {
+    loadProgram('f3f3');
+    cpu.data[95] = 0x0; // SREG <- 0x0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+  });
+
+  it('should execute `CBI 0x0c, 5`', () => {
+    loadProgram('6598');
+    cpu.data[0x2c] = 0b11111111;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x2c]).toEqual(0b11011111);
+  });
+
+  it('should execute `CALL` instruction', () => {
+    loadProgram('0e945c00');
+    cpu.data[94] = 0;
+    cpu.data[93] = 150; // SP <- 50
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x5c);
+    expect(cpu.cycles).toEqual(5);
+    expect(cpu.data[150]).toEqual(2); // return addr
+    expect(cpu.data[93]).toEqual(148); // SP should be decremented
+  });
+
+  it('should execute `CPC r27, r18` instruction', () => {
+    loadProgram('b207');
+    cpu.data[18] = 0x1;
+    cpu.data[27] = 0x1;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0); // SREG clear
+  });
+
+  it('should execute `CPC r24, r1` instruction and set', () => {
+    loadProgram('8105');
+    cpu.data[1] = 0; // r1 <- 0
+    cpu.data[24] = 0; // r24 <- 0
+    cpu.data[95] = 0b10000001; // SREG: I-------C
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0b10110101); // SREG: I-HS-N-C
+  });
+
+  it('should execute `CPI r26, 0x9` instruction', () => {
+    loadProgram('a930');
+    cpu.data[26] = 0x8;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0b00110101); // SREG: HSNC
+  });
+
+  it('should execute `CPSE r2, r3` when r2 != r3', () => {
+    loadProgram('2310');
+    cpu.data[2] = 10; // r2 <- 10
+    cpu.data[3] = 11; // r3 <- 11
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+  });
+
+  it('should execute `CPSE r2, r3` when r2 == r3', () => {
+    loadProgram('23101c92');
+    cpu.data[2] = 10; // r2 <- 10
+    cpu.data[3] = 10; // r3 <- 10
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(2);
+    expect(cpu.cycles).toEqual(2);
+  });
+
+  it('should execute `CPSE r2, r3` when r2 == r3 and followed by 2-word instruction', () => {
+    loadProgram('23100e945c00');
+    cpu.data[2] = 10; // r2 <- 10
+    cpu.data[3] = 10; // r3 <- 10
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(3);
+    expect(cpu.cycles).toEqual(3);
+  });
+
+  it('should execute `ICALL` instruction', () => {
+    loadProgram('0995');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x80;
+    cpu.dataView.setUint16(30, 0x2020, true); // Z <- 0x2020
+    avrInstruction(cpu);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.pc).toEqual(0x2020);
+    expect(cpu.data[0x80]).toEqual(1); // Return address
+    expect(cpu.data[93]).toEqual(0x7e);
+  });
+
+  it('should execute `IJMP` instruction', () => {
+    loadProgram('0994');
+    cpu.dataView.setUint16(30, 0x1040, true); // Z <- 0x1040
+    avrInstruction(cpu);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.pc).toEqual(0x1040);
+  });
+
+  it('should execute `IN r5, 0xb` instruction', () => {
+    loadProgram('5bb0');
+    cpu.data[0x2b] = 0xaf;
+    avrInstruction(cpu);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.data[5]).toEqual(0xaf);
+  });
+
+  it('should execute `INC r5` instruction', () => {
+    loadProgram('5394');
+    cpu.data[5] = 0x7f;
+    avrInstruction(cpu);
+    expect(cpu.data[5]).toEqual(0x80);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0b00001100); // SREG: NV
+  });
+
+  it('should execute `INC r5` instruction when r5 == 0xff', () => {
+    loadProgram('5394');
+    cpu.data[5] = 0xff;
+    avrInstruction(cpu);
+    expect(cpu.data[5]).toEqual(0);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0b00000010); // SREG: Z
+  });
+
+  it('should execute `JMP 0xb8` instruction', () => {
+    loadProgram('0c945c00');
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x5c);
+    expect(cpu.cycles).toEqual(3);
+  });
+
+  it('should execute `LAC r19` instruction', () => {
+    loadProgram('3693');
+    cpu.data[19] = 0x02; // r19 <- 0x02
+    cpu.dataView.setUint16(30, 0x100, true); // Z <- 0x100
+    cpu.data[0x100] = 0x96;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[19]).toEqual(0x96);
+    expect(cpu.dataView.getUint16(30, true)).toEqual(0x100);
+    expect(cpu.data[0x100]).toEqual(0x94);
+  });
+
+  it('should execute `LAS r17` instruction', () => {
+    loadProgram('1593');
+    cpu.data[17] = 0x11; // r17 <- 0x11
+    cpu.data[30] = 0x80; // Z <- 0x80
+    cpu.data[0x80] = 0x44;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[17]).toEqual(0x44);
+    expect(cpu.data[30]).toEqual(0x80);
+    expect(cpu.data[0x80]).toEqual(0x55);
+  });
+
+  it('should execute `LAT r0` instruction', () => {
+    loadProgram('0792');
+    cpu.data[0] = 0x33; // r0 <- 0x33
+    cpu.data[30] = 0x80; // Z <- 0x80
+    cpu.data[0x80] = 0x66;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0]).toEqual(0x66);
+    expect(cpu.data[30]).toEqual(0x80);
+    expect(cpu.data[0x80]).toEqual(0x55);
+  });
+
+  it('should execute `LDI r28, 0xff` instruction', () => {
+    loadProgram('cfef');
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[28]).toEqual(0xff);
+  });
+
+  it('should execute `LDS r5, 0x150` instruction', () => {
+    loadProgram('50905001');
+    cpu.data[0x150] = 0x7a;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x2);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[5]).toEqual(0x7a);
+  });
+
+  it('should execute `LD r1, X` instruction', () => {
+    loadProgram('1c90');
+    cpu.data[0xc0] = 0x15;
+    cpu.data[26] = 0xc0; // X <- 0xc0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[1]).toEqual(0x15);
+    expect(cpu.data[26]).toEqual(0xc0); // verify that X was unchanged
+  });
+
+  it('should execute `LD r17, X+` instruction', () => {
+    loadProgram('1d91');
+    cpu.data[0xc0] = 0x15;
+    cpu.data[26] = 0xc0; // X <- 0xc0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[17]).toEqual(0x15);
+    expect(cpu.data[26]).toEqual(0xc1); // verify that X was incremented
+  });
+
+  it('should execute `LD r1, -X` instruction', () => {
+    loadProgram('1e90');
+    cpu.data[0x98] = 0x22;
+    cpu.data[26] = 0x99; // X <- 0x99
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[1]).toEqual(0x22);
+    expect(cpu.data[26]).toEqual(0x98); // verify that X was decremented
+  });
+
+  it('should execute `LD r8, Y` instruction', () => {
+    loadProgram('8880');
+    cpu.data[0xc0] = 0x15;
+    cpu.data[28] = 0xc0; // Y <- 0xc0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[8]).toEqual(0x15);
+    expect(cpu.data[28]).toEqual(0xc0); // verify that Y was unchanged
+  });
+
+  it('should execute `LD r3, Y+` instruction', () => {
+    loadProgram('3990');
+    cpu.data[0xc0] = 0x15;
+    cpu.data[28] = 0xc0; // Y <- 0xc0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[3]).toEqual(0x15);
+    expect(cpu.data[28]).toEqual(0xc1); // verify that Y was incremented
+  });
+
+  it('should execute `LD r0, -Y` instruction', () => {
+    loadProgram('0a90');
+    cpu.data[0x98] = 0x22;
+    cpu.data[28] = 0x99; // Y <- 0x99
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[0]).toEqual(0x22);
+    expect(cpu.data[28]).toEqual(0x98); // verify that Y was decremented
+  });
+
+  it('should execute `LDD r4, Y+2` instruction', () => {
+    loadProgram('4a80');
+    cpu.data[0x82] = 0x33;
+    cpu.data[28] = 0x80; // Y <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[4]).toEqual(0x33);
+    expect(cpu.data[28]).toEqual(0x80); // verify that Y was unchanged
+  });
+
+  it('should execute `LD r5, Z` instruction', () => {
+    loadProgram('5080');
+    cpu.data[0xcc] = 0xf5;
+    cpu.data[30] = 0xcc; // Z <- 0xcc
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[5]).toEqual(0xf5);
+    expect(cpu.data[30]).toEqual(0xcc); // verify that Z was unchanged
+  });
+
+  it('should execute `LD r7, Z+` instruction', () => {
+    loadProgram('7190');
+    cpu.data[0xc0] = 0x25;
+    cpu.data[30] = 0xc0; // Z <- 0xc0
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[7]).toEqual(0x25);
+    expect(cpu.data[30]).toEqual(0xc1); // verify that Y was incremented
+  });
+
+  it('should execute `LD r0, -Z` instruction', () => {
+    loadProgram('0290');
+    cpu.data[0x9e] = 0x66;
+    cpu.data[30] = 0x9f; // Z <- 0x9f
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[0]).toEqual(0x66);
+    expect(cpu.data[30]).toEqual(0x9e); // verify that Y was decremented
+  });
+
+  it('should execute `LDD r15, Z+31` instruction', () => {
+    loadProgram('f78c');
+    cpu.data[0x9f] = 0x33;
+    cpu.data[30] = 0x80; // Z <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[15]).toEqual(0x33);
+    expect(cpu.data[30]).toEqual(0x80); // verify that Z was unchanged
+  });
+
+  it('should execute `LPM` instruction', () => {
+    loadProgram('c895');
+    cpu.progMem[0x40] = 0xa0;
+    cpu.data[30] = 0x80; // Z <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[0]).toEqual(0xa0);
+    expect(cpu.data[30]).toEqual(0x80); // verify that Z was unchanged
+  });
+
+  it('should execute `LPM r2` instruction', () => {
+    loadProgram('2490');
+    cpu.progMem[0x40] = 0xa0;
+    cpu.data[30] = 0x80; // Z <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[2]).toEqual(0xa0);
+    expect(cpu.data[30]).toEqual(0x80); // verify that Z was unchanged
+  });
+
+  it('should execute `LPM r1, Z+` instruction', () => {
+    loadProgram('1590');
+    cpu.progMem[0x40] = 0xa0;
+    cpu.data[30] = 0x80; // Z <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(3);
+    expect(cpu.data[1]).toEqual(0xa0);
+    expect(cpu.data[30]).toEqual(0x81); // verify that Z was incremented
+  });
+
+  it('should execute `LSR r7` instruction', () => {
+    loadProgram('7694');
+    cpu.data[7] = 0x45; // r7 <- 0x45
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[7]).toEqual(0x22);
+    expect(cpu.data[95]).toEqual(0b00011001); // SREG SVC
+  });
+
+  it('should execute `MOV r7, r8` instruction', () => {
+    loadProgram('782c');
+    cpu.data[8] = 0x45; // r7 <- 0x45
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[7]).toEqual(0x45);
+  });
+
+  it('should execute `MOVW r26, r22` instruction', () => {
+    loadProgram('db01');
+    cpu.data[22] = 0x45; // r22 <- 0x45
+    cpu.data[23] = 0x9a; // r23 <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[26]).toEqual(0x45);
+    expect(cpu.data[27]).toEqual(0x9a);
+  });
+
+  it('should execute `MUL r5, r6` instruction', () => {
+    loadProgram('569c');
+    cpu.data[5] = 100; // r5 <- 55
+    cpu.data[6] = 5; // r6 <- 5
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.dataView.getUint16(0, true)).toEqual(500);
+    expect(cpu.data[95]).toEqual(0b0); // SREG: 0
+  });
+
+  it('should execute `MUL r5, r6` instruction and update carry flag when numbers are big', () => {
+    loadProgram('569c');
+    cpu.data[5] = 200; // r5 <- 200
+    cpu.data[6] = 200; // r6 <- 200
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.dataView.getUint16(0, true)).toEqual(40000);
+    expect(cpu.data[95]).toEqual(0b00000001); // SREG: C
+  });
+
+  it('should execute `MUL r0, r1` and update the zero flag', () => {
+    loadProgram('019c');
+    cpu.data[0] = 0; // r0 <- 0
+    cpu.data[1] = 9; // r1 <- 9
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.dataView.getUint16(0, true)).toEqual(0);
+    expect(cpu.data[95]).toEqual(0b00000010); // SREG: Z
+  });
+
+  it('should execute `MULS r18, r19` instruction', () => {
+    loadProgram('2302');
+    cpu.data[18] = -5; // r18 <- -5
+    cpu.data[19] = 100; // r19 <- 100
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.dataView.getInt16(0, true)).toEqual(-500);
+    expect(cpu.data[95]).toEqual(0b00000001); // SREG: C
+  });
+
+  it('should execute `MULSU r16, r17` instruction', () => {
+    loadProgram('0103');
+    cpu.data[16] = -5; // r16 <- -5
+    cpu.data[17] = 200; // r17 <- 200
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.dataView.getInt16(0, true)).toEqual(-1000);
+    expect(cpu.data[95]).toEqual(0b00000001); // SREG: C
+  });
+
+  it('should execute `NEG r20` instruction', () => {
+    loadProgram('4195');
+    cpu.data[20] = 0x56; // r20 <- 0x56
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[20]).toEqual(0xaa);
+    expect(cpu.data[95]).toEqual(0b00010101); // SREG: NC
+  });
+
+  it('should execute `NOP` instruction', () => {
+    loadProgram('0000');
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+  });
+
+  it('should execute `OUT 0x3f, r1` instruction', () => {
+    loadProgram('1fbe');
+    cpu.data[1] = 0x5a; // r1 <- 0x5a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x5f]).toEqual(0x5a);
+  });
+
+  it('should execute `POP r26` instruction', () => {
+    loadProgram('af91');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0xff; // SP <- 0xff
+    cpu.data[0x100] = 0x1a;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[26]).toEqual(0x1a);
+    expect(cpu.dataView.getUint16(93, true)).toEqual(0x100); // SP
+  });
+
+  it('should execute `PUSH r11` instruction', () => {
+    loadProgram('bf92');
+    cpu.data[11] = 0x2a;
+    cpu.data[94] = 0;
+    cpu.data[93] = 0xff; // SP <- 0xff
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0x1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0xff]).toEqual(0x2a);
+    expect(cpu.dataView.getUint16(93, true)).toEqual(0xfe); // SP
+  });
+
+  it('should execute `RCALL .+6` instruction', () => {
+    loadProgram('03d0');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x80; // SP <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(4);
+    expect(cpu.cycles).toEqual(4);
+    expect(cpu.dataView.getUint16(0x80, true)).toEqual(1); // RET address
+    expect(cpu.data[93]).toEqual(0x7e); // SP
+  });
+
+  it('should execute `RCALL .-4` instruction', () => {
+    loadProgram('0000fedf');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x80; // SP <- 0x80
+    avrInstruction(cpu);
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(0);
+    expect(cpu.cycles).toEqual(5); // 1 for NOP, 4 for RCALL
+    expect(cpu.dataView.getUint16(0x80, true)).toEqual(2); // RET address
+    expect(cpu.data[93]).toEqual(0x7e); // SP
+  });
+
+  it('should execute `RET` instruction', () => {
+    loadProgram('0895');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x90; // SP <- 0x90
+    cpu.data[0x92] = 16;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(16);
+    expect(cpu.cycles).toEqual(5);
+    expect(cpu.data[93]).toEqual(0x92); // SP should increment
+  });
+
+  it('should execute `RETI` instruction', () => {
+    loadProgram('1895');
+    cpu.data[94] = 0;
+    cpu.data[93] = 0xc0; // SP <- 0xc0
+    cpu.data[0xc2] = 200;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(200);
+    expect(cpu.cycles).toEqual(5);
+    expect(cpu.data[93]).toEqual(0xc2); // SP should increment
+    expect(cpu.data[95]).toEqual(0b10000000); // SREG: I
+  });
+
+  it('should execute `RJMP 2` instruction', () => {
+    loadProgram('01c0');
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(2);
+    expect(cpu.cycles).toEqual(2);
+  });
+
+  it('should execute `ROR r0` instruction', () => {
+    loadProgram('0794');
+    cpu.data[0] = 0x11; // r0 <- 0x11
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0]).toEqual(0x08); // r0 should be right-shifted
+    expect(cpu.data[95]).toEqual(0b00011001); // SREG: SVI
+  });
+
+  it('should execute `SBCI r23, 3`', () => {
+    loadProgram('7340');
+    cpu.data[23] = 3; // r23 <- 3
+    cpu.data[95] = 0b10000001; // SREG <- I------C
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[95]).toEqual(0b10110101); // SREG: I-HS-N-C
+  });
+
+  it('should execute `SBI 0x0c, 5`', () => {
+    loadProgram('659a');
+    cpu.data[0x2c] = 0b00001111;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x2c]).toEqual(0b00101111);
+  });
+
+  it('should execute `SBIS 0x0c, 5` when bit is clear', () => {
+    loadProgram('659b1c92');
+    cpu.data[0x2c] = 0b00001111;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+  });
+
+  it('should execute `SBIS 0x0c, 5` when bit is set', () => {
+    loadProgram('659b1c92');
+    cpu.data[0x2c] = 0b00101111;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(2);
+    expect(cpu.cycles).toEqual(2);
+  });
+
+  it('should execute `SBIS 0x0c, 5` when bit is set and followed by 2-word instruction', () => {
+    loadProgram('659b0e945c00');
+    cpu.data[0x2c] = 0b00101111;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(3);
+    expect(cpu.cycles).toEqual(3);
+  });
+
+  it('should execute `STS 0x151, r31` instruction', () => {
+    loadProgram('f0935101');
+    cpu.data[31] = 0x80; // r31 <- 0x80
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(2);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x151]).toEqual(0x80);
+  });
+
+  it('should execute `ST X, r1` instruction', () => {
+    loadProgram('1c92');
+    cpu.data[1] = 0x5a; // r1 <- 0x5a
+    cpu.data[26] = 0x9a; // X <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x9a]).toEqual(0x5a);
+    expect(cpu.data[26]).toEqual(0x9a); // verify that X was unchanged
+  });
+
+  it('should execute `ST X+, r1` instruction', () => {
+    loadProgram('1d92');
+    cpu.data[1] = 0x5a; // r1 <- 0x5a
+    cpu.data[26] = 0x9a; // X <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x9a]).toEqual(0x5a);
+    expect(cpu.data[26]).toEqual(0x9b); // verify that X was incremented
+  });
+
+  it('should execute `ST -X, r17` instruction', () => {
+    loadProgram('1e93');
+    cpu.data[17] = 0x88; // r17 <- 0x88
+    cpu.data[26] = 0x99; // X <- 0x99
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x98]).toEqual(0x88);
+    expect(cpu.data[26]).toEqual(0x98); // verify that X was decremented
+  });
+
+  it('should execute `ST Y, r2` instruction', () => {
+    loadProgram('2882');
+    cpu.data[2] = 0x5b; // r2 <- 0x5b
+    cpu.data[28] = 0x9a; // Y <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x9a]).toEqual(0x5b);
+    expect(cpu.data[28]).toEqual(0x9a); // verify that Y was unchanged
+  });
+
+  it('should execute `ST Y+, r1` instruction', () => {
+    loadProgram('1992');
+    cpu.data[1] = 0x5a; // r1 <- 0x5a
+    cpu.data[28] = 0x9a; // Y <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x9a]).toEqual(0x5a);
+    expect(cpu.data[28]).toEqual(0x9b); // verify that Y was incremented
+  });
+
+  it('should execute `ST -Y, r1` instruction', () => {
+    loadProgram('1a92');
+    cpu.data[1] = 0x5a; // r1 <- 0x5a
+    cpu.data[28] = 0x9a; // Y <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x99]).toEqual(0x5a);
+    expect(cpu.data[28]).toEqual(0x99); // verify that Y was decremented
+  });
+
+  it('should execute `STD Y+17, r0` instruction', () => {
+    loadProgram('098a');
+    cpu.data[0] = 0xba; // r0 <- 0xba
+    cpu.data[28] = 0x9a; // Y <- 0x9a
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x9a + 17]).toEqual(0xba);
+    expect(cpu.data[28]).toEqual(0x9a); // verify that Y was unchanged
+  });
+
+  it('should execute `ST Z, r16` instruction', () => {
+    loadProgram('0083');
+    cpu.data[16] = 0xdf; // r2 <- 0xdf
+    cpu.data[30] = 0x40; // Z <- 0x40
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x40]).toEqual(0xdf);
+    expect(cpu.data[30]).toEqual(0x40); // verify that Z was unchanged
+  });
+
+  it('should execute `ST Z+, r0` instruction', () => {
+    loadProgram('0192');
+    cpu.data[0] = 0x55; // r0 <- 0x55
+    cpu.dataView.setUint16(30, 0x155, true); // Z <- 0x155
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[0x155]).toEqual(0x55);
+    expect(cpu.dataView.getUint16(30, true)).toEqual(0x156); // verify that Z was incremented
+  });
+
+  it('should execute `ST -Z, r16` instruction', () => {
+    loadProgram('0293');
+    cpu.data[16] = 0x5a; // r16 <- 0x5a
+    cpu.data[30] = 0xff; // Z <- 0xff
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0xfe]).toEqual(0x5a);
+    expect(cpu.data[30]).toEqual(0xfe); // verify that Z was decremented
+  });
+
+  it('should execute `STD Z+1, r0` instruction', () => {
+    loadProgram('0182');
+    cpu.data[0] = 0xcc; // r0 <- 0xcc
+    cpu.data[30] = 0x50; // Z <- 0x50
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.data[0x51]).toEqual(0xcc);
+    expect(cpu.data[30]).toEqual(0x50); // verify that Z was unchanged
+  });
+
+  it('should execute `SWAP r1` instruction', () => {
+    loadProgram('1294');
+    cpu.data[1] = 0xa5; // r1 <- 0xa5
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[1]).toEqual(0x5a); // r1
+  });
+
+  it('should execute `XCH r21` instruction', () => {
+    loadProgram('5493');
+    cpu.data[21] = 0xa1; // r21 <- 0xa1
+    cpu.data[30] = 0x50; // Z <- 0x50
+    cpu.data[0x50] = 0xb9;
+    avrInstruction(cpu);
+    expect(cpu.pc).toEqual(1);
+    expect(cpu.cycles).toEqual(1);
+    expect(cpu.data[21]).toEqual(0xb9); // r21
+    expect(cpu.data[0x50]).toEqual(0xa1);
+  });
+});
diff --git a/src/cpu/instruction.ts b/src/cpu/instruction.ts
new file mode 100644
index 0000000..7e14aad
--- /dev/null
+++ b/src/cpu/instruction.ts
@@ -0,0 +1,726 @@
+/**
+ * AVR-8 Instruction Simulation
+ * Part of AVR8js
+ * Reference: http://ww1.microchip.com/downloads/en/devicedoc/atmel-0856-avr-instruction-set-manual.pdf
+ *
+ * Copyright (C) 2019, Uri Shaked
+ */
+
+import { ICPU } from './cpu';
+import { u16 } from '../types';
+
+function isTwoWordInstruction(opcode: u16) {
+  return (
+    /* LDS */
+    (opcode & 0xfe0f) === 0x9000 ||
+    /* STS */
+    (opcode & 0xfe0f) === 0x9200 ||
+    /* CALL */
+    (opcode & 0xfe0e) === 0x940e ||
+    /* JMP */
+    (opcode & 0xfe0e) === 0x940c
+  );
+}
+
+export function avrInstruction(cpu: ICPU) {
+  const opcode = cpu.progMem[cpu.pc];
+
+  if ((opcode & 0xfc00) === 0x1c00) {
+    /* ADC, 0001 11rd dddd rrrr */
+    const d = cpu.data[(opcode & 0x1f0) >> 4];
+    const r = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    const sum = d + r + (cpu.data[95] & 1);
+    const R = sum & 255;
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (R ^ r) & (d ^ R) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= sum & 256 ? 1 : 0;
+    sreg |= 1 & ((d & r) | (r & ~R) | (~R & d)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0xc00) {
+    /* ADD, 0000 11rd dddd rrrr */
+    const d = cpu.data[(opcode & 0x1f0) >> 4];
+    const r = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    const R = (d + r) & 255;
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (R ^ r) & (R ^ d) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= (d + r) & 256 ? 1 : 0;
+    sreg |= 1 & ((d & r) | (r & ~R) | (~R & d)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xff00) === 0x9600) {
+    /* ADIW, 1001 0110 KKdd KKKK */
+    const addr = 2 * ((opcode & 0x30) >> 4) + 24;
+    const value = cpu.dataView.getUint16(addr, true);
+    const R = (value + ((opcode & 0xf) | ((opcode & 0xc0) >> 2))) & 0xffff;
+    cpu.dataView.setUint16(addr, R, true);
+    let sreg = cpu.data[95] & 0xe0;
+    sreg |= R ? 0 : 2;
+    sreg |= 0x8000 & R ? 4 : 0;
+    sreg |= ~value & R & 0x8000 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= ~R & value & 0x8000 ? 1 : 0;
+    cpu.data[95] = sreg;
+    cpu.cycles++;
+  } else if ((opcode & 0xfc00) === 0x2000) {
+    /* AND, 0010 00rd dddd rrrr */
+    const R = cpu.data[(opcode & 0x1f0) >> 4] & cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf000) === 0x7000) {
+    /* ANDI, 0111 KKKK dddd KKKK */
+    const R = cpu.data[((opcode & 0xf0) >> 4) + 16] & ((opcode & 0xf) | ((opcode & 0xf00) >> 4));
+    cpu.data[((opcode & 0xf0) >> 4) + 16] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfe0f) === 0x9405) {
+    /* ASR, 1001 010d dddd 0101 */
+    const value = cpu.data[(opcode & 0x1f0) >> 4];
+    const R = (value >>> 1) | (128 & value);
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= value & 1;
+    sreg |= ((sreg >> 2) & 1) ^ (sreg & 1) ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xff8f) === 0x9488) {
+    /* BCLR, 1001 0100 1sss 1000 */
+    cpu.data[95] &= ~(1 << ((opcode & 0x70) >> 4));
+  } else if ((opcode & 0xfe08) === 0xf800) {
+    /* BLD, 1111 100d dddd 0bbb */
+    const b = opcode & 7;
+    const d = (opcode & 0x1f0) >> 4;
+    cpu.data[d] = (~(1 << b) & cpu.data[d]) | (((cpu.data[95] >> 6) & 1) << b);
+  } else if ((opcode & 0xfc00) === 0xf400) {
+    /* BRBC, 1111 01kk kkkk ksss */
+    if (!(cpu.data[95] & (1 << (opcode & 7)))) {
+      cpu.pc = cpu.pc + (((opcode & 0x1f8) >> 3) - (opcode & 0x200 ? 0x40 : 0));
+      cpu.cycles++;
+    }
+  } else if ((opcode & 0xfc00) === 0xf000) {
+    /* BRBS, 1111 00kk kkkk ksss */
+    if (cpu.data[95] & (1 << (opcode & 7))) {
+      cpu.pc = cpu.pc + (((opcode & 0x1f8) >> 3) - (opcode & 0x200 ? 0x40 : 0));
+      cpu.cycles++;
+    }
+  } else if ((opcode & 0xff8f) === 0x9408) {
+    /* BSET, 1001 0100 0sss 1000 */
+    cpu.data[95] |= 1 << ((opcode & 0x70) >> 4);
+  } else if ((opcode & 0xfe08) === 0xfa00) {
+    /* BST, 1111 101d dddd 0bbb */
+    const d = cpu.data[(opcode & 0x1f0) >> 4];
+    const b = opcode & 7;
+    cpu.data[95] = (cpu.data[95] & 0xbf) | ((d >> b) & 1 ? 0x40 : 0);
+  } else if ((opcode & 0xfe0e) === 0x940e) {
+    /* CALL, 1001 010k kkkk 111k kkkk kkkk kkkk kkkk */
+    const k = cpu.progMem[cpu.pc + 1] | ((opcode & 1) << 16) | ((opcode & 0x1f0) << 13);
+    const ret = cpu.pc + 2;
+    const sp = cpu.dataView.getUint16(93, true);
+    cpu.data[sp] = 255 & ret;
+    cpu.data[sp - 1] = (ret >> 8) & 255;
+    cpu.dataView.setUint16(93, sp - 2, true);
+    cpu.pc = k - 1;
+    cpu.cycles += 4;
+  } else if ((opcode & 0xff00) === 0x9800) {
+    /* CBI, 1001 1000 AAAA Abbb */
+    const A = opcode & 0xf8;
+    const b = opcode & 7;
+    const R = cpu.readData((A >> 3) + 32);
+    cpu.writeData((A >> 3) + 32, R & ~(1 << b));
+  } else if ((opcode & 0xfe0f) === 0x9400) {
+    /* COM, 1001 010d dddd 0000 */
+    const d = (opcode & 0x1f0) >> 4;
+    const R = 255 - cpu.data[d];
+    cpu.data[d] = R;
+    let sreg = (cpu.data[95] & 0xe1) | 1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x1400) {
+    /* CP, 0001 01rd dddd rrrr */
+    const val1 = cpu.data[(opcode & 0x1f0) >> 4];
+    const val2 = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    const R = val1 - val2;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= 0 !== ((val1 ^ val2) & (val1 ^ R) & 128) ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= val2 > val1 ? 1 : 0;
+    sreg |= 1 & ((~val1 & val2) | (val2 & R) | (R & ~val1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x400) {
+    /* CPC, 0000 01rd dddd rrrr */
+    const arg1 = cpu.data[(opcode & 0x1f0) >> 4];
+    const arg2 = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    let sreg = cpu.data[95];
+    const r = arg1 - arg2 - (sreg & 1);
+    sreg = (sreg & 0xc0) | (!r && (sreg >> 1) & 1 ? 2 : 0) | (arg2 + (sreg & 1) > arg1 ? 1 : 0);
+    sreg |= 128 & r ? 4 : 0;
+    sreg |= (arg1 ^ arg2) & (arg1 ^ r) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= 1 & ((~arg1 & arg2) | (arg2 & r) | (r & ~arg1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf000) === 0x3000) {
+    /* CPI, 0011 KKKK dddd KKKK */
+    const arg1 = cpu.data[((opcode & 0xf0) >> 4) + 16];
+    const arg2 = (opcode & 0xf) | ((opcode & 0xf00) >> 4);
+    const r = arg1 - arg2;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= r ? 0 : 2;
+    sreg |= 128 & r ? 4 : 0;
+    sreg |= (arg1 ^ arg2) & (arg1 ^ r) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= arg2 > arg1 ? 1 : 0;
+    sreg |= 1 & ((~arg1 & arg2) | (arg2 & r) | (r & ~arg1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x1000) {
+    /* CPSE, 0001 00rd dddd rrrr */
+    if (cpu.data[(opcode & 0x1f0) >> 4] === cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)]) {
+      const nextOpcode = cpu.progMem[cpu.pc + 1];
+      const skipSize = isTwoWordInstruction(nextOpcode) ? 2 : 1;
+      cpu.pc += skipSize;
+      cpu.cycles += skipSize;
+    }
+  } else if ((opcode & 0xfe0f) === 0x940a) {
+    /* DEC, 1001 010d dddd 1010 */
+    const value = cpu.data[(opcode & 0x1f0) >> 4];
+    const R = value - 1;
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= 128 === value ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x2400) {
+    /* EOR, 0010 01rd dddd rrrr */
+    const R = cpu.data[(opcode & 0x1f0) >> 4] ^ cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xff88) === 0x308) {
+    /* FMUL, 0000 0011 0ddd 1rrr */
+    const v1 = cpu.data[((opcode & 0x70) >> 4) + 16];
+    const v2 = cpu.data[(opcode & 7) + 16];
+    const R = (v1 * v2) << 1;
+    cpu.dataView.setUint16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 0 : 2) | ((v1 * v2) & 0x8000 ? 1 : 0);
+    cpu.cycles++;
+  } else if ((opcode & 0xff88) === 0x380) {
+    /* FMULS, 0000 0011 1ddd 0rrr */
+    const v1 = cpu.dataView.getInt8(((opcode & 0x70) >> 4) + 16);
+    const v2 = cpu.dataView.getInt8((opcode & 7) + 16);
+    const R = (v1 * v2) << 1;
+    cpu.dataView.setInt16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 0 : 2) | ((v1 * v2) & 0x8000 ? 1 : 0);
+    cpu.cycles++;
+  } else if ((opcode & 0xff88) === 0x388) {
+    /* FMULSU, 0000 0011 1ddd 1rrr */
+    const v1 = cpu.dataView.getInt8(((opcode & 0x70) >> 4) + 16);
+    const v2 = cpu.data[(opcode & 7) + 16];
+    const R = (v1 * v2) << 1;
+    cpu.dataView.setInt16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 2 : 0) | ((v1 * v2) & 0x8000 ? 1 : 0);
+    cpu.cycles++;
+  } else if (opcode === 0x9509) {
+    /* ICALL, 1001 0101 0000 1001 */
+    const retAddr = cpu.pc + 1;
+    const sp = cpu.dataView.getUint16(93, true);
+    cpu.data[sp] = retAddr & 255;
+    cpu.data[sp - 1] = (retAddr >> 8) & 255;
+    cpu.dataView.setUint16(93, sp - 2, true);
+    cpu.pc = cpu.dataView.getUint16(30, true) - 1;
+    cpu.cycles += 2;
+  } else if (opcode === 0x9409) {
+    /* IJMP, 1001 0100 0000 1001 */
+    cpu.pc = cpu.dataView.getUint16(30, true) - 1;
+    cpu.cycles++;
+  } else if ((opcode & 0xf800) === 0xb000) {
+    /* IN, 1011 0AAd dddd AAAA */
+    const i = cpu.readData(((opcode & 0xf) | ((opcode & 0x600) >> 5)) + 32);
+    cpu.data[(opcode & 0x1f0) >> 4] = i;
+  } else if ((opcode & 0xfe0f) === 0x9403) {
+    /* INC, 1001 010d dddd 0011 */
+    const d = cpu.data[(opcode & 0x1f0) >> 4];
+    const r = (d + 1) & 255;
+    cpu.data[(opcode & 0x1f0) >> 4] = r;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= r ? 0 : 2;
+    sreg |= 128 & r ? 4 : 0;
+    sreg |= 127 === d ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfe0e) === 0x940c) {
+    /* JMP, 1001 010k kkkk 110k kkkk kkkk kkkk kkkk */
+    cpu.pc = (cpu.progMem[cpu.pc + 1] | ((opcode & 1) << 16) | ((opcode & 0x1f0) << 13)) - 1;
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9206) {
+    /* LAC, 1001 001r rrrr 0110 */
+    const r = (opcode & 0x1f0) >> 4;
+    const clear = cpu.data[r];
+    const value = cpu.readData(cpu.dataView.getUint16(30, true));
+    cpu.writeData(cpu.dataView.getUint16(30, true), value & (255 - clear));
+    cpu.data[r] = value;
+  } else if ((opcode & 0xfe0f) === 0x9205) {
+    /* LAS, 1001 001r rrrr 0101 */
+    const r = (opcode & 0x1f0) >> 4;
+    const set = cpu.data[r];
+    const value = cpu.readData(cpu.dataView.getUint16(30, true));
+    cpu.writeData(cpu.dataView.getUint16(30, true), value | set);
+    cpu.data[r] = value;
+  } else if ((opcode & 0xfe0f) === 0x9207) {
+    /* LAT, 1001 001r rrrr 0111 */
+    const r = cpu.data[(opcode & 0x1f0) >> 4];
+    const R = cpu.readData(cpu.dataView.getUint16(30, true));
+    cpu.writeData(cpu.dataView.getUint16(30, true), r ^ R);
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+  } else if ((opcode & 0xf000) === 0xe000) {
+    /* LDI, 1110 KKKK dddd KKKK */
+    cpu.data[((opcode & 0xf0) >> 4) + 16] = (opcode & 0xf) | ((opcode & 0xf00) >> 4);
+  } else if ((opcode & 0xfe0f) === 0x9000) {
+    /* LDS, 1001 000d dddd 0000 kkkk kkkk kkkk kkkk */
+    const value = cpu.readData(cpu.progMem[cpu.pc + 1]);
+    cpu.data[(opcode & 0x1f0) >> 4] = value;
+    cpu.pc++;
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x900c) {
+    /* LDX, 1001 000d dddd 1100 */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(cpu.dataView.getUint16(26, true));
+  } else if ((opcode & 0xfe0f) === 0x900d) {
+    /* LDX(INC), 1001 000d dddd 1101 */
+    const x = cpu.dataView.getUint16(26, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(x);
+    cpu.dataView.setUint16(26, x + 1, true);
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x900e) {
+    /* LDX(DEC), 1001 000d dddd 1110 */
+    const x = cpu.dataView.getUint16(26, true) - 1;
+    cpu.dataView.setUint16(26, x, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(x);
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x8008) {
+    /* LDY, 1000 000d dddd 1000 */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(cpu.dataView.getUint16(28, true));
+  } else if ((opcode & 0xfe0f) === 0x9009) {
+    /* LDY(INC), 1001 000d dddd 1001 */
+    const y = cpu.dataView.getUint16(28, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(y);
+    cpu.dataView.setUint16(28, y + 1, true);
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x900a) {
+    /* LDY(DEC), 1001 000d dddd 1010 */
+    const y = cpu.dataView.getUint16(28, true) - 1;
+    cpu.dataView.setUint16(28, y, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(y);
+    cpu.cycles += 2;
+  } else if (
+    (opcode & 0xd208) === 0x8008 &&
+    (opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)
+  ) {
+    /* LDDY, 10q0 qq0d dddd 1qqq */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(
+      cpu.dataView.getUint16(28, true) +
+        ((opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8))
+    );
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x8000) {
+    /* LDZ, 1000 000d dddd 0000 */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(cpu.dataView.getUint16(30, true));
+  } else if ((opcode & 0xfe0f) === 0x9001) {
+    /* LDZ(INC), 1001 000d dddd 0001 */
+    const z = cpu.dataView.getUint16(30, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(z);
+    cpu.dataView.setUint16(30, z + 1, true);
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x9002) {
+    /* LDZ(DEC), 1001 000d dddd 0010 */
+    const z = cpu.dataView.getUint16(30, true) - 1;
+    cpu.dataView.setUint16(30, z, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(z);
+    cpu.cycles += 2;
+  } else if (
+    (opcode & 0xd208) === 0x8000 &&
+    (opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)
+  ) {
+    /* LDDZ, 10q0 qq0d dddd 0qqq */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.readData(
+      cpu.dataView.getUint16(30, true) +
+        ((opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8))
+    );
+    cpu.cycles += 2;
+  } else if (opcode === 0x95c8) {
+    /* LPM, 1001 0101 1100 1000 */
+    cpu.data[0] = cpu.progBytes[cpu.dataView.getUint16(30, true)];
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9004) {
+    /* LPM(REG), 1001 000d dddd 0100 */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.progBytes[cpu.dataView.getUint16(30, true)];
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9005) {
+    /* LPM(INC), 1001 000d dddd 0101 */
+    const i = cpu.dataView.getUint16(30, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.progBytes[i];
+    cpu.dataView.setUint16(30, i + 1, true);
+    cpu.cycles += 2;
+  } else if ((opcode & 0xfe0f) === 0x9406) {
+    /* LSR, 1001 010d dddd 0110 */
+    const value = cpu.data[(opcode & 0x1f0) >> 4];
+    const R = value >>> 1;
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe0;
+    sreg |= R ? 0 : 2;
+    sreg |= value & 1;
+    sreg |= ((sreg >> 2) & 1) ^ (sreg & 1) ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x2c00) {
+    /* MOV, 0010 11rd dddd rrrr */
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+  } else if ((opcode & 0xff00) === 0x100) {
+    /* MOVW, 0000 0001 dddd rrrr */
+    const r2 = 2 * (opcode & 0xf);
+    const d2 = 2 * ((opcode & 0xf0) >> 4);
+    cpu.data[d2] = cpu.data[r2];
+    cpu.data[d2 + 1] = cpu.data[r2 + 1];
+  } else if ((opcode & 0xfc00) === 0x9c00) {
+    /* MUL, 1001 11rd dddd rrrr */
+    const R = cpu.data[(opcode & 0x1f0) >> 4] * cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    cpu.dataView.setUint16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 0 : 2) | (0x8000 & R ? 1 : 0);
+    cpu.cycles++;
+  } else if ((opcode & 0xff00) === 0x200) {
+    /* MULS, 0000 0010 dddd rrrr */
+    const R =
+      cpu.dataView.getInt8(((opcode & 0xf0) >> 4) + 16) * cpu.dataView.getInt8((opcode & 0xf) + 16);
+    cpu.dataView.setInt16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 0 : 2) | (0x8000 & R ? 1 : 0);
+    cpu.cycles++;
+  } else if ((opcode & 0xff88) === 0x300) {
+    /* MULSU, 0000 0011 0ddd 0rrr */
+    const R = cpu.dataView.getInt8(((opcode & 0x70) >> 4) + 16) * cpu.data[(opcode & 7) + 16];
+    cpu.dataView.setInt16(0, R, true);
+    cpu.data[95] = (cpu.data[95] & 0xfc) | (0xffff & R ? 0 : 2) | (0x8000 & R ? 1 : 0);
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x9401) {
+    /* NEG, 1001 010d dddd 0001 */
+    const d = (opcode & 0x1f0) >> 4;
+    const value = cpu.data[d];
+    const R = 0 - value;
+    cpu.data[d] = R;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= 128 === R ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= R ? 1 : 0;
+    sreg |= 1 & (R | value) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if (opcode === 0) {
+    /* NOP, 0000 0000 0000 0000 */
+    /* NOP */
+  } else if ((opcode & 0xfc00) === 0x2800) {
+    /* OR, 0010 10rd dddd rrrr */
+    const R = cpu.data[(opcode & 0x1f0) >> 4] | cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf000) === 0x6000) {
+    /* SBR, 0110 KKKK dddd KKKK */
+    const R = cpu.data[((opcode & 0xf0) >> 4) + 16] | ((opcode & 0xf) | ((opcode & 0xf00) >> 4));
+    cpu.data[((opcode & 0xf0) >> 4) + 16] = R;
+    let sreg = cpu.data[95] & 0xe1;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf800) === 0xb800) {
+    /* OUT, 1011 1AAr rrrr AAAA */
+    cpu.writeData(((opcode & 0xf) | ((opcode & 0x600) >> 5)) + 32, cpu.data[(opcode & 0x1f0) >> 4]);
+  } else if ((opcode & 0xfe0f) === 0x900f) {
+    /* POP, 1001 000d dddd 1111 */
+    const value = cpu.dataView.getUint16(93, true) + 1;
+    cpu.dataView.setUint16(93, value, true);
+    cpu.data[(opcode & 0x1f0) >> 4] = cpu.data[value];
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x920f) {
+    /* PUSH, 1001 001d dddd 1111 */
+    const value = cpu.dataView.getUint16(93, true);
+    cpu.data[value] = cpu.data[(opcode & 0x1f0) >> 4];
+    cpu.dataView.setUint16(93, value - 1, true);
+    cpu.cycles++;
+  } else if ((opcode & 0xf000) === 0xd000) {
+    /* RCALL, 1101 kkkk kkkk kkkk */
+    const k = (opcode & 0x7ff) - (opcode & 0x800 ? 0x800 : 0);
+    const retAddr = cpu.pc + 1;
+    const sp = cpu.dataView.getUint16(93, true);
+    cpu.data[sp] = 255 & retAddr;
+    cpu.data[sp - 1] = (retAddr >> 8) & 255;
+    cpu.dataView.setUint16(93, sp - 2, true);
+    cpu.pc += k;
+    cpu.cycles += 3;
+  } else if (opcode === 0x9508) {
+    /* RET, 1001 0101 0000 1000 */
+    const i = cpu.dataView.getUint16(93, true) + 2;
+    cpu.dataView.setUint16(93, i, true);
+    cpu.pc = (cpu.data[i - 1] << 8) + cpu.data[i] - 1;
+    cpu.cycles += 4;
+  } else if (opcode === 0x9518) {
+    /* RETI, 1001 0101 0001 1000 */
+    const i = cpu.dataView.getUint16(93, true) + 2;
+    cpu.dataView.setUint16(93, i, true);
+    cpu.pc = (cpu.data[i - 1] << 8) + cpu.data[i] - 1;
+    cpu.cycles += 4;
+    cpu.data[95] |= 0x80; // Enable interrupts
+  } else if ((opcode & 0xf000) === 0xc000) {
+    /* RJMP, 1100 kkkk kkkk kkkk */
+    cpu.pc = cpu.pc + ((opcode & 0x7ff) - (opcode & 0x800 ? 0x800 : 0));
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x9407) {
+    /* ROR, 1001 010d dddd 0111 */
+    const d = cpu.data[(opcode & 0x1f0) >> 4];
+    const r = (d >>> 1) | ((cpu.data[95] & 1) << 7);
+    cpu.data[(opcode & 0x1f0) >> 4] = r;
+    let sreg = cpu.data[95] & 0xe0;
+    sreg |= r ? 0 : 2;
+    sreg |= 128 & r ? 4 : 0;
+    sreg |= 1 & d ? 1 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ (sreg & 1) ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfc00) === 0x800) {
+    /* SBC, 0000 10rd dddd rrrr */
+    const val1 = cpu.data[(opcode & 0x1f0) >> 4];
+    const val2 = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    let sreg = cpu.data[95];
+    const R = val1 - val2 - (sreg & 1);
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    sreg = (sreg & 0xc0) | (!R && (sreg >> 1) & 1 ? 2 : 0) | (val2 + (sreg & 1) > val1 ? 1 : 0);
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (val1 ^ val2) & (val1 ^ R) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= 1 & ((~val1 & val2) | (val2 & R) | (R & ~val1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf000) === 0x4000) {
+    /* SBCI, 0100 KKKK dddd KKKK */
+    const val1 = cpu.data[((opcode & 0xf0) >> 4) + 16];
+    const val2 = (opcode & 0xf) | ((opcode & 0xf00) >> 4);
+    let sreg = cpu.data[95];
+    const R = val1 - val2 - (sreg & 1);
+    cpu.data[((opcode & 0xf0) >> 4) + 16] = R;
+    sreg = (sreg & 0xc0) | (!R && (sreg >> 1) & 1 ? 2 : 0) | (val2 + (sreg & 1) > val1 ? 1 : 0);
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (val1 ^ val2) & (val1 ^ R) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= 1 & ((~val1 & val2) | (val2 & R) | (R & ~val1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xff00) === 0x9a00) {
+    /* SBI, 1001 1010 AAAA Abbb */
+    const target = ((opcode & 0xf8) >> 3) + 32;
+    cpu.writeData(target, cpu.readData(target) | (1 << (opcode & 7)));
+    cpu.cycles++;
+  } else if ((opcode & 0xff00) === 0x9900) {
+    /* SBIC, 1001 1001 AAAA Abbb */
+    const value = cpu.readData(((opcode & 0xf8) >> 3) + 32);
+    if (!(value & (1 << (opcode & 7)))) {
+      const nextOpcode = cpu.progMem[cpu.pc + 1];
+      const skipSize = isTwoWordInstruction(nextOpcode) ? 2 : 1;
+      cpu.cycles += skipSize;
+      cpu.pc += skipSize;
+    }
+  } else if ((opcode & 0xff00) === 0x9b00) {
+    /* SBIS, 1001 1011 AAAA Abbb */
+    const value = cpu.readData(((opcode & 0xf8) >> 3) + 32);
+    if (value & (1 << (opcode & 7))) {
+      const nextOpcode = cpu.progMem[cpu.pc + 1];
+      const skipSize = isTwoWordInstruction(nextOpcode) ? 2 : 1;
+      cpu.cycles += skipSize;
+      cpu.pc += skipSize;
+    }
+  } else if ((opcode & 0xff00) === 0x9700) {
+    /* SBIW, 1001 0111 KKdd KKKK */
+    const i = 2 * ((opcode & 0x30) >> 4) + 24;
+    const a = cpu.dataView.getUint16(i, true);
+    const l = (opcode & 0xf) | ((opcode & 0xc0) >> 2);
+    const R = a - l;
+    cpu.dataView.setUint16(i, R, true);
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 0x8000 & R ? 4 : 0;
+    sreg |= a & ~R & 0x8000 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= l > a ? 1 : 0;
+    sreg |= 1 & ((~a & l) | (l & R) | (R & ~a)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+    cpu.cycles++;
+  } else if ((opcode & 0xfe08) === 0xfc00) {
+    /* SBRC, 1111 110r rrrr 0bbb */
+    if (!(cpu.data[(opcode & 0x1f0) >> 4] & (1 << (opcode & 7)))) {
+      const nextOpcode = cpu.progMem[cpu.pc + 1];
+      const skipSize = isTwoWordInstruction(nextOpcode) ? 2 : 1;
+      cpu.cycles += skipSize;
+      cpu.pc += skipSize;
+    }
+  } else if ((opcode & 0xfe08) === 0xfe00) {
+    /* SBRS, 1111 111r rrrr 0bbb */
+    if (cpu.data[(opcode & 0x1f0) >> 4] & (1 << (opcode & 7))) {
+      const nextOpcode = cpu.progMem[cpu.pc + 1];
+      const skipSize = isTwoWordInstruction(nextOpcode) ? 2 : 1;
+      cpu.cycles += skipSize;
+      cpu.pc += skipSize;
+    }
+  } else if (opcode === 0x9588) {
+    /* SLEEP, 1001 0101 1000 1000 */
+    /* not implemented */
+  } else if (opcode === 0x95e8) {
+    /* SPM, 1001 0101 1110 1000 */
+    /* not implemented */
+  } else if (opcode === 0x95f8) {
+    /* SPM(INC), 1001 0101 1111 1000 */
+    /* not implemented */
+  } else if ((opcode & 0xfe0f) === 0x9200) {
+    /* STS, 1001 001d dddd 0000 kkkk kkkk kkkk kkkk */
+    const value = cpu.data[(opcode & 0x1f0) >> 4];
+    const addr = cpu.progMem[cpu.pc + 1];
+    cpu.writeData(addr, value);
+    cpu.pc++;
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x920c) {
+    /* STX, 1001 001r rrrr 1100 */
+    cpu.writeData(cpu.dataView.getUint16(26, true), cpu.data[(opcode & 0x1f0) >> 4]);
+  } else if ((opcode & 0xfe0f) === 0x920d) {
+    /* STX(INC), 1001 001r rrrr 1101 */
+    const x = cpu.dataView.getUint16(26, true);
+    cpu.writeData(x, cpu.data[(opcode & 0x1f0) >> 4]);
+    cpu.dataView.setUint16(26, x + 1, true);
+  } else if ((opcode & 0xfe0f) === 0x920e) {
+    /* STX(DEC), 1001 001r rrrr 1110 */
+    const i = cpu.data[(opcode & 0x1f0) >> 4];
+    const x = cpu.dataView.getUint16(26, true) - 1;
+    cpu.dataView.setUint16(26, x, true);
+    cpu.writeData(x, i);
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x8208) {
+    /* STY, 1000 001r rrrr 1000 */
+    cpu.writeData(cpu.dataView.getUint16(28, true), cpu.data[(opcode & 0x1f0) >> 4]);
+  } else if ((opcode & 0xfe0f) === 0x9209) {
+    /* STY(INC), 1001 001r rrrr 1001 */
+    const i = cpu.data[(opcode & 0x1f0) >> 4];
+    const y = cpu.dataView.getUint16(28, true);
+    cpu.writeData(y, i);
+    cpu.dataView.setUint16(28, y + 1, true);
+  } else if ((opcode & 0xfe0f) === 0x920a) {
+    /* STY(DEC), 1001 001r rrrr 1010 */
+    const i = cpu.data[(opcode & 0x1f0) >> 4];
+    const y = cpu.dataView.getUint16(28, true) - 1;
+    cpu.dataView.setUint16(28, y, true);
+    cpu.writeData(y, i);
+    cpu.cycles++;
+  } else if (
+    (opcode & 0xd208) === 0x8208 &&
+    (opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)
+  ) {
+    /* STDY, 10q0 qq1r rrrr 1qqq */
+    cpu.writeData(
+      cpu.dataView.getUint16(28, true) +
+        ((opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)),
+      cpu.data[(opcode & 0x1f0) >> 4]
+    );
+    cpu.cycles++;
+  } else if ((opcode & 0xfe0f) === 0x8200) {
+    /* STZ, 1000 001r rrrr 0000 */
+    cpu.writeData(cpu.dataView.getUint16(30, true), cpu.data[(opcode & 0x1f0) >> 4]);
+  } else if ((opcode & 0xfe0f) === 0x9201) {
+    /* STZ(INC), 1001 001r rrrr 0001 */
+    const z = cpu.dataView.getUint16(30, true);
+    cpu.writeData(z, cpu.data[(opcode & 0x1f0) >> 4]);
+    cpu.dataView.setUint16(30, z + 1, true);
+  } else if ((opcode & 0xfe0f) === 0x9202) {
+    /* STZ(DEC), 1001 001r rrrr 0010 */
+    const i = cpu.data[(opcode & 0x1f0) >> 4];
+    const z = cpu.dataView.getUint16(30, true) - 1;
+    cpu.dataView.setUint16(30, z, true);
+    cpu.writeData(z, i);
+    cpu.cycles++;
+  } else if (
+    (opcode & 0xd208) === 0x8200 &&
+    (opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)
+  ) {
+    /* STDZ, 10q0 qq1r rrrr 0qqq */
+    cpu.writeData(
+      cpu.dataView.getUint16(30, true) +
+        ((opcode & 7) | ((opcode & 0xc00) >> 7) | ((opcode & 0x2000) >> 8)),
+      cpu.data[(opcode & 0x1f0) >> 4]
+    );
+    cpu.cycles++;
+  } else if ((opcode & 0xfc00) === 0x1800) {
+    /* SUB, 0001 10rd dddd rrrr */
+    const val1 = cpu.data[(opcode & 0x1f0) >> 4];
+    const val2 = cpu.data[(opcode & 0xf) | ((opcode & 0x200) >> 5)];
+    const R = val1 - val2;
+
+    cpu.data[(opcode & 0x1f0) >> 4] = R;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (val1 ^ val2) & (val1 ^ R) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= val2 > val1 ? 1 : 0;
+    sreg |= 1 & ((~val1 & val2) | (val2 & R) | (R & ~val1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xf000) === 0x5000) {
+    /* SUBI, 0101 KKKK dddd KKKK */
+    const val1 = cpu.data[((opcode & 0xf0) >> 4) + 16];
+    const val2 = (opcode & 0xf) | ((opcode & 0xf00) >> 4);
+    const R = val1 - val2;
+    cpu.data[((opcode & 0xf0) >> 4) + 16] = R;
+    let sreg = cpu.data[95] & 0xc0;
+    sreg |= R ? 0 : 2;
+    sreg |= 128 & R ? 4 : 0;
+    sreg |= (val1 ^ val2) & (val1 ^ R) & 128 ? 8 : 0;
+    sreg |= ((sreg >> 2) & 1) ^ ((sreg >> 3) & 1) ? 0x10 : 0;
+    sreg |= val2 > val1 ? 1 : 0;
+    sreg |= 1 & ((~val1 & val2) | (val2 & R) | (R & ~val1)) ? 0x20 : 0;
+    cpu.data[95] = sreg;
+  } else if ((opcode & 0xfe0f) === 0x9402) {
+    /* SWAP, 1001 010d dddd 0010 */
+    const d = (opcode & 0x1f0) >> 4;
+    const i = cpu.data[d];
+    cpu.data[d] = ((15 & i) << 4) | ((240 & i) >>> 4);
+  } else if (opcode === 0x95a8) {
+    /* WDR, 1001 0101 1010 1000 */
+    /* not implemented */
+  } else if ((opcode & 0xfe0f) === 0x9204) {
+    /* XCH, 1001 001r rrrr 0100 */
+    const r = (opcode & 0x1f0) >> 4;
+    const val1 = cpu.data[r];
+    const val2 = cpu.data[cpu.dataView.getUint16(30, true)];
+    cpu.data[cpu.dataView.getUint16(30, true)] = val1;
+    cpu.data[r] = val2;
+  }
+
+  cpu.pc = (cpu.pc + 1) % cpu.progMem.length;
+  cpu.cycles++;
+}
diff --git a/src/cpu/interrupt.spec.ts b/src/cpu/interrupt.spec.ts
new file mode 100644
index 0000000..cc54e3c
--- /dev/null
+++ b/src/cpu/interrupt.spec.ts
@@ -0,0 +1,19 @@
+import { CPU } from './cpu';
+import { avrInterrupt } from './interrupt';
+
+describe('avrInterrupt', () => {
+  it('should execute interrupt handler', () => {
+    const cpu = new CPU(new Uint16Array(0x8000));
+    cpu.pc = 0x520;
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x80; // SP <- 0x80
+    cpu.data[95] = 0b10000001; // SREG <- I------C
+    avrInterrupt(cpu, 5);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.pc).toEqual(5);
+    expect(cpu.data[93]).toEqual(0x7e); // SP
+    expect(cpu.data[0x80]).toEqual(0x20); // Return addr low
+    expect(cpu.data[0x7f]).toEqual(0x5); // Return addr high
+    expect(cpu.data[95]).toEqual(0b00000001); // SREG: -------C
+  });
+});
diff --git a/src/cpu/interrupt.ts b/src/cpu/interrupt.ts
new file mode 100644
index 0000000..1c7d835
--- /dev/null
+++ b/src/cpu/interrupt.ts
@@ -0,0 +1,19 @@
+/**
+ * AVR-8 Interrupt Handling
+ * Part of AVR8js
+ * Reference: http://ww1.microchip.com/downloads/en/devicedoc/atmel-0856-avr-instruction-set-manual.pdf
+ *
+ * Copyright (C) 2019, Uri Shaked
+ */
+
+import { ICPU } from './cpu';
+
+export function avrInterrupt(cpu: ICPU, addr: number) {
+  const sp = cpu.dataView.getUint16(93, true);
+  cpu.data[sp] = cpu.pc & 0xff;
+  cpu.data[sp - 1] = (cpu.pc >> 8) & 0xff;
+  cpu.dataView.setUint16(93, sp - 2, true);
+  cpu.data[95] &= 0x7f; // clear global interrupt flag
+  cpu.cycles += 2;
+  cpu.pc = addr;
+}