13 files changed, 286 insertions, 92 deletions

diff --git a/demo/src/execute.ts b/demo/src/execute.ts
index 40cc996..3a065d1 100644
--- a/demo/src/execute.ts
+++ b/demo/src/execute.ts
@@ -54,6 +54,7 @@ export class AVRRunner {
       this.timer1.tick();
       this.timer2.tick();
       this.usart.tick();
+      this.cpu.tick();
     }
 
     callback(this.cpu);
diff --git a/src/cpu/cpu.ts b/src/cpu/cpu.ts
index ed88e71..4b70efb 100644
--- a/src/cpu/cpu.ts
+++ b/src/cpu/cpu.ts
@@ -5,7 +5,8 @@
  * Copyright (C) 2019, Uri Shaked
  */
 
-import { u32, u16, u8 } from '../types';
+import { u32, u16, u8, i16 } from '../types';
+import { avrInterrupt } from './interrupt';
 
 const registerSpace = 0x100;
 
@@ -45,6 +46,15 @@ export interface CPUMemoryReadHooks {
   [key: number]: CPUMemoryReadHook;
 }
 
+export interface AVRInterruptConfig {
+  address: u8;
+  enableRegister: u16;
+  enableMask: u8;
+  flagRegister: u16;
+  flagMask: u8;
+  constant?: boolean;
+}
+
 export class CPU implements ICPU {
   readonly data: Uint8Array = new Uint8Array(this.sramBytes + registerSpace);
   readonly data16 = new Uint16Array(this.data.buffer);
@@ -52,13 +62,15 @@ export class CPU implements ICPU {
   readonly progBytes = new Uint8Array(this.progMem.buffer);
   readonly readHooks: CPUMemoryReadHooks = [];
   readonly writeHooks: CPUMemoryHooks = [];
+  private readonly pendingInterrupts: AVRInterruptConfig[] = [];
   readonly pc22Bits = this.progBytes.length > 0x20000;
 
   // This lets the Timer Compare output override GPIO pins:
   readonly gpioTimerHooks: CPUMemoryHooks = [];
 
-  pc = 0;
-  cycles = 0;
+  pc: u32 = 0;
+  cycles: u32 = 0;
+  nextInterrupt: i16 = -1;
 
   constructor(public progMem: Uint16Array, private sramBytes = 8192) {
     this.reset();
@@ -67,6 +79,8 @@ export class CPU implements ICPU {
   reset() {
     this.data.fill(0);
     this.SP = this.data.length - 1;
+    this.pendingInterrupts.splice(0, this.pendingInterrupts.length);
+    this.nextInterrupt = -1;
   }
 
   readData(addr: number) {
@@ -101,4 +115,63 @@ export class CPU implements ICPU {
   get interruptsEnabled() {
     return this.SREG & 0x80 ? true : false;
   }
+
+  private updateNextInterrupt() {
+    this.nextInterrupt = this.pendingInterrupts.findIndex((item) => !!item);
+  }
+
+  setInterruptFlag(interrupt: AVRInterruptConfig) {
+    const { flagRegister, flagMask, enableRegister, enableMask } = interrupt;
+    if (interrupt.constant) {
+      this.data[flagRegister] &= ~flagMask;
+    } else {
+      this.data[flagRegister] |= flagMask;
+    }
+    if (this.data[enableRegister] & enableMask) {
+      this.queueInterrupt(interrupt);
+    }
+  }
+
+  updateInterruptEnable(interrupt: AVRInterruptConfig, registerValue: u8) {
+    const { enableMask, flagRegister, flagMask } = interrupt;
+    if (registerValue & enableMask) {
+      if (this.data[flagRegister] & flagMask) {
+        this.queueInterrupt(interrupt);
+      }
+    } else {
+      this.clearInterrupt(interrupt, false);
+    }
+  }
+
+  queueInterrupt(interrupt: AVRInterruptConfig) {
+    this.pendingInterrupts[interrupt.address] = interrupt;
+    this.updateNextInterrupt();
+  }
+
+  clearInterrupt({ address, flagRegister, flagMask }: AVRInterruptConfig, clearFlag = true) {
+    delete this.pendingInterrupts[address];
+    if (clearFlag) {
+      this.data[flagRegister] &= ~flagMask;
+    }
+    this.updateNextInterrupt();
+  }
+
+  clearInterruptByFlag(interrupt: AVRInterruptConfig, registerValue: number) {
+    const { flagRegister, flagMask } = interrupt;
+    if (registerValue & flagMask) {
+      this.data[flagRegister] &= ~flagMask;
+      this.clearInterrupt(interrupt);
+    }
+  }
+
+  tick() {
+    if (this.interruptsEnabled && this.nextInterrupt >= 0) {
+      const interrupt = this.pendingInterrupts[this.nextInterrupt];
+      // eslint-disable-next-line @typescript-eslint/no-non-null-assertion
+      avrInterrupt(this, interrupt.address);
+      if (!interrupt.constant) {
+        this.clearInterrupt(interrupt);
+      }
+    }
+  }
 }
diff --git a/src/peripherals/eeprom.spec.ts b/src/peripherals/eeprom.spec.ts
index e7cf62e..d293bd9 100644
--- a/src/peripherals/eeprom.spec.ts
+++ b/src/peripherals/eeprom.spec.ts
@@ -28,6 +28,7 @@ describe('EEPROM', () => {
       cpu.writeData(EEARH, 0);
       cpu.writeData(EECR, EERE);
       eeprom.tick();
+      cpu.tick();
       expect(cpu.cycles).toEqual(4);
       expect(cpu.data[EEDR]).toEqual(0xff);
     });
@@ -41,6 +42,7 @@ describe('EEPROM', () => {
       cpu.writeData(EEARH, 0x2);
       cpu.writeData(EECR, EERE);
       eeprom.tick();
+      cpu.tick();
       expect(cpu.data[EEDR]).toEqual(0x42);
     });
   });
@@ -56,6 +58,7 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
       expect(cpu.cycles).toEqual(2);
       expect(eepromBackend.memory[15]).toEqual(0x55);
       expect(cpu.data[EECR] & EEPE).toEqual(EEPE);
@@ -104,12 +107,14 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEPE);
       cpu.cycles += 1000;
       eeprom.tick();
+      cpu.tick();
       // At this point, write shouldn't be complete yet
       expect(cpu.data[EECR] & EEPE).toEqual(EEPE);
       expect(cpu.pc).toEqual(0);
       cpu.cycles += 10000000;
       // And now, 10 million cycles later, it should.
       eeprom.tick();
+      cpu.tick();
       expect(eepromBackend.memory[15]).toEqual(0x55);
       expect(cpu.data[EECR] & EEPE).toEqual(0);
       expect(cpu.pc).toEqual(0x2c); // EEPROM Ready interrupt
@@ -125,8 +130,10 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.cycles = 8; // waiting for more than 4 cycles should clear EEMPE
       eeprom.tick();
+      cpu.tick();
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
       // Ensure that nothing was written, and EEPE bit is clear
       expect(cpu.cycles).toEqual(8);
       expect(eepromBackend.memory[15]).toEqual(0xff);
@@ -145,6 +152,7 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
       expect(cpu.cycles).toEqual(2);
 
       // Write 0x66 to address 16 (first write is still in progress)
@@ -154,6 +162,7 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
 
       // Ensure that second write didn't happen
       expect(cpu.cycles).toEqual(2);
@@ -173,11 +182,13 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
       expect(cpu.cycles).toEqual(2);
 
       // wait long enough time for the first write to finish
       cpu.cycles += 10000000;
       eeprom.tick();
+      cpu.tick();
 
       // Write 0x66 to address 16
       cpu.writeData(EEDR, 0x66);
@@ -186,6 +197,7 @@ describe('EEPROM', () => {
       cpu.writeData(EECR, EEMPE);
       cpu.writeData(EECR, EEPE);
       eeprom.tick();
+      cpu.tick();
 
       // Ensure both writes took place
       expect(cpu.cycles).toEqual(10000004);
diff --git a/src/peripherals/eeprom.ts b/src/peripherals/eeprom.ts
index 0301701..bc386ca 100644
--- a/src/peripherals/eeprom.ts
+++ b/src/peripherals/eeprom.ts
@@ -1,6 +1,5 @@
-import { CPU } from '../cpu/cpu';
-import { avrInterrupt } from '../cpu/interrupt';
-import { u8, u16, u32 } from '../types';
+import { AVRInterruptConfig, CPU } from '../cpu/cpu';
+import { u16, u32, u8 } from '../types';
 
 export interface EEPROMBackend {
   readMemory(addr: u16): u8;
@@ -71,6 +70,16 @@ export class AVREEPROM {
 
   private writeCompleteCycles = 0;
 
+  // Interrupts
+  private EER: AVRInterruptConfig = {
+    address: this.config.eepromReadyInterrupt,
+    flagRegister: this.config.EECR,
+    flagMask: EEPE,
+    enableRegister: this.config.EECR,
+    enableMask: EERIE,
+    constant: true,
+  };
+
   constructor(
     private cpu: CPU,
     private backend: EEPROMBackend,
@@ -81,6 +90,10 @@ export class AVREEPROM {
 
       const addr = (this.cpu.data[EEARH] << 8) | this.cpu.data[EEARL];
 
+      if (eecr & EERE) {
+        this.cpu.clearInterrupt(this.EER);
+      }
+
       if (eecr & EEMPE) {
         this.writeEnabledCycles = this.cpu.cycles + 4;
       }
@@ -132,16 +145,13 @@ export class AVREEPROM {
   }
 
   tick() {
-    const { EECR, eepromReadyInterrupt } = this.config;
+    const { EECR } = this.config;
 
     if (this.writeEnabledCycles && this.cpu.cycles > this.writeEnabledCycles) {
       this.cpu.data[EECR] &= ~EEMPE;
     }
     if (this.writeCompleteCycles && this.cpu.cycles > this.writeCompleteCycles) {
-      this.cpu.data[EECR] &= ~EEPE;
-      if (this.cpu.interruptsEnabled && this.cpu.data[EECR] & EERIE) {
-        avrInterrupt(this.cpu, eepromReadyInterrupt);
-      }
+      this.cpu.setInterruptFlag(this.EER);
     }
   }
 }
diff --git a/src/peripherals/spi.spec.ts b/src/peripherals/spi.spec.ts
index 1bb099f..8e11b94 100644
--- a/src/peripherals/spi.spec.ts
+++ b/src/peripherals/spi.spec.ts
@@ -177,6 +177,7 @@ describe('SPI', () => {
     cpu.writeData(SPCR, SPE | MSTR);
     cpu.writeData(SPDR, 0x50);
     spi.tick();
+    cpu.tick();
     expect(cpu.readData(SPSR) & WCOL).toEqual(0);
 
     cpu.writeData(SPDR, 0x51);
@@ -194,11 +195,13 @@ describe('SPI', () => {
     // At this point, write shouldn't be complete yet
     cpu.cycles += 10;
     spi.tick();
+    cpu.tick();
     expect(cpu.pc).toEqual(0);
 
     // 100 cycles later, it should (8 bits * 8 cycles per bit = 64).
     cpu.cycles += 100;
     spi.tick();
+    cpu.tick();
     expect(cpu.data[SPSR] & SPIF).toEqual(0);
     expect(cpu.pc).toEqual(0x22); // SPI Ready interrupt
   });
@@ -213,10 +216,12 @@ describe('SPI', () => {
 
     cpu.cycles = 10;
     spi.tick();
+    cpu.tick();
     expect(cpu.readData(SPDR)).toEqual(0);
 
     cpu.cycles = 32; // 4 cycles per bit * 8 bits = 32
     spi.tick();
+    cpu.tick();
     expect(cpu.readData(SPDR)).toEqual(0x88);
   });
 });
diff --git a/src/peripherals/spi.ts b/src/peripherals/spi.ts
index 0c03c3f..f67143f 100644
--- a/src/peripherals/spi.ts
+++ b/src/peripherals/spi.ts
@@ -1,6 +1,5 @@
-import { CPU } from '../cpu/cpu';
+import { AVRInterruptConfig, CPU } from '../cpu/cpu';
 import { u8 } from '../types';
-import { avrInterrupt } from '../cpu/interrupt';
 
 export interface SPIConfig {
   spiInterrupt: u8;
@@ -42,6 +41,15 @@ export class AVRSPI {
   private transmissionCompleteCycles = 0;
   private receivedByte: u8 = 0;
 
+  // Interrupts
+  private SPI: AVRInterruptConfig = {
+    address: this.config.spiInterrupt,
+    flagRegister: this.config.SPSR,
+    flagMask: SPSR_SPIF,
+    enableRegister: this.config.SPCR,
+    enableMask: SPCR_SPIE,
+  };
+
   constructor(private cpu: CPU, private config: SPIConfig, private freqMHz: number) {
     const { SPCR, SPSR, SPDR } = config;
     cpu.writeHooks[SPDR] = (value: u8) => {
@@ -57,28 +65,26 @@ export class AVRSPI {
       }
 
       // Clear write collision / interrupt flags
-      cpu.data[SPSR] &= ~SPSR_WCOL & ~SPSR_SPIF;
+      cpu.data[SPSR] &= ~SPSR_WCOL;
+      this.cpu.clearInterrupt(this.SPI);
 
       this.receivedByte = this.onTransfer?.(value) ?? 0;
       this.transmissionCompleteCycles = this.cpu.cycles + this.clockDivider * bitsPerByte;
       return true;
     };
+    cpu.writeHooks[SPSR] = (value: u8) => {
+      this.cpu.data[SPSR] = value;
+      this.cpu.clearInterruptByFlag(this.SPI, value);
+    };
   }
 
   tick() {
     if (this.transmissionCompleteCycles && this.cpu.cycles >= this.transmissionCompleteCycles) {
-      const { SPSR, SPDR } = this.config;
-      this.cpu.data[SPSR] |= SPSR_SPIF;
+      const { SPDR } = this.config;
       this.cpu.data[SPDR] = this.receivedByte;
+      this.cpu.setInterruptFlag(this.SPI);
       this.transmissionCompleteCycles = 0;
     }
-    if (this.cpu.interruptsEnabled) {
-      const { SPSR, SPCR, spiInterrupt } = this.config;
-      if (this.cpu.data[SPCR] & SPCR_SPIE && this.cpu.data[SPSR] & SPSR_SPIF) {
-        avrInterrupt(this.cpu, spiInterrupt);
-        this.cpu.data[SPSR] &= ~SPSR_SPIF;
-      }
-    }
   }
 
   get isMaster() {
diff --git a/src/peripherals/timer.spec.ts b/src/peripherals/timer.spec.ts
index 2dc7d55..6b6a9eb 100644
--- a/src/peripherals/timer.spec.ts
+++ b/src/peripherals/timer.spec.ts
@@ -69,8 +69,10 @@ describe('timer', () => {
     const timer = new AVRTimer(cpu, timer0Config);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(1);
   });
@@ -80,8 +82,10 @@ describe('timer', () => {
     const timer = new AVRTimer(cpu, timer0Config);
     cpu.writeData(TCCR0B, CS01 | CS00); // Set prescaler to 64
     timer.tick();
+    cpu.tick();
     cpu.cycles = 64;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(1);
   });
@@ -92,6 +96,7 @@ describe('timer', () => {
     cpu.writeData(TCCR0B, 0); // No prescaler (timer disabled)
     cpu.cycles = 100000;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0); // TCNT should stay 0
   });
@@ -102,23 +107,42 @@ describe('timer', () => {
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0);
     expect(cpu.data[TIFR0]).toEqual(TOV0);
   });
 
+  it('should clear the TOV flag when writing 1 to the TOV bit, and not trigger the interrupt', () => {
+    const cpu = new CPU(new Uint16Array(0x1000));
+    const timer = new AVRTimer(cpu, timer0Config);
+    cpu.writeData(TCNT0, 0xff);
+    cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
+    timer.tick();
+    cpu.tick();
+    cpu.cycles = 1;
+    timer.tick();
+    cpu.tick();
+    expect(cpu.data[TIFR0]).toEqual(TOV0);
+    cpu.writeData(TIFR0, TOV0);
+    expect(cpu.data[TIFR0]).toEqual(0);
+  });
+
   it('should set TOV if timer overflows in FAST PWM mode', () => {
     const cpu = new CPU(new Uint16Array(0x1000));
     const timer = new AVRTimer(cpu, timer0Config);
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.writeData(OCR0A, 0x7f);
     cpu.writeData(TCCR0A, WGM01 | WGM00); // WGM: Fast PWM
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0);
     expect(cpu.data[TIFR0]).toEqual(TOV0);
@@ -130,10 +154,12 @@ describe('timer', () => {
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.data[TIMSK0] = TOIE0;
     cpu.data[SREG] = 0x80; // SREG: I-------
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(2); // TCNT should be 2 (one tick above + 2 cycles for interrupt)
     expect(cpu.data[TIFR0] & TOV0).toEqual(0);
@@ -157,10 +183,12 @@ describe('timer', () => {
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.data[TIMSK0] = 2;
     cpu.data[SREG] = 0x80; // SREG: I-------
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(2); // TCNT should be 2 (one tick above + 2 cycles for interrupt)
     expect(cpu.data[TIFR0] & 2).toEqual(0);
@@ -174,10 +202,12 @@ describe('timer', () => {
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.data[TIMSK0] = TOIE0;
     cpu.data[SREG] = 0x0; // SREG: --------
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     expect(cpu.data[TIFR0]).toEqual(TOV0);
     expect(cpu.pc).toEqual(0);
     expect(cpu.cycles).toEqual(1);
@@ -189,10 +219,12 @@ describe('timer', () => {
     cpu.writeData(TCNT0, 0xff);
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.data[TIMSK0] = 0;
     cpu.data[SREG] = 0x80; // SREG: I-------
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     expect(cpu.data[TIFR0]).toEqual(TOV0);
     expect(cpu.pc).toEqual(0);
     expect(cpu.cycles).toEqual(1);
@@ -206,8 +238,10 @@ describe('timer', () => {
     cpu.writeData(TCCR0A, 0x0); // WGM: Normal
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     expect(cpu.data[TIFR0]).toEqual(OCF0A);
     expect(cpu.pc).toEqual(0);
     expect(cpu.cycles).toEqual(1);
@@ -221,8 +255,10 @@ describe('timer', () => {
     cpu.writeData(TCCR0A, WGM01); // WGM: CTC
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0);
     expect(cpu.pc).toEqual(0);
@@ -237,8 +273,10 @@ describe('timer', () => {
     cpu.writeData(TCCR0A, 0x0); // WGM: (Normal)
     cpu.writeData(TCCR0B, CS00); // Set prescaler to 1
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     expect(cpu.data[TIFR0]).toEqual(OCF0B);
     expect(cpu.pc).toEqual(0);
     expect(cpu.cycles).toEqual(1);
@@ -253,8 +291,10 @@ describe('timer', () => {
     cpu.writeData(TIMSK0, OCIE0A);
     cpu.writeData(95, 0x80); // SREG: I-------
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0x23); // TCNT should be 0x23 (one tick above + 2 cycles for interrupt)
     expect(cpu.data[TIFR0] & OCF0A).toEqual(0);
@@ -271,8 +311,10 @@ describe('timer', () => {
     cpu.writeData(TIMSK0, 0);
     cpu.writeData(95, 0x80); // SREG: I-------
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0x21);
     expect(cpu.pc).toEqual(0);
@@ -288,8 +330,10 @@ describe('timer', () => {
     cpu.writeData(TIMSK0, OCIE0B);
     cpu.writeData(95, 0x80); // SREG: I-------
     timer.tick();
+    cpu.tick();
     cpu.cycles = 1;
     timer.tick();
+    cpu.tick();
     const tcnt = cpu.readData(TCNT0);
     expect(tcnt).toEqual(0x23); // TCNT should be 0x23 (0x23 + 2 cycles for interrupt)
     expect(cpu.data[TIFR0] & OCF0B).toEqual(0);
@@ -320,13 +364,16 @@ describe('timer', () => {
     const timer = new AVRTimer(cpu, timer2Config);
     cpu.writeData(TCCR2B, CS22 | CS21); // Set prescaler to 256
     timer.tick();
+    cpu.tick();
 
     cpu.cycles = 511;
     timer.tick();
+    cpu.tick();
     expect(cpu.readData(TCNT2)).toEqual(1);
 
     cpu.cycles = 512;
     timer.tick();
+    cpu.tick();
     expect(cpu.readData(TCNT2)).toEqual(2);
   });
 
@@ -457,8 +504,10 @@ describe('timer', () => {
       cpu.writeData(TCCR1A, 0x0); // WGM: Normal
       cpu.writeData(TCCR1B, CS10); // Set prescaler to 1
       timer.tick();
+      cpu.tick();
       cpu.cycles = 1;
       timer.tick();
+      cpu.tick();
       cpu.readData(TCNT1);
       expect(cpu.dataView.getUint16(TCNT1, true)).toEqual(0x2234); // TCNT1 should increment
     });
@@ -473,8 +522,10 @@ describe('timer', () => {
       cpu.writeData(TCCR1A, 0x0); // WGM: Normal
       cpu.writeData(TCCR1B, CS10); // Set prescaler to 1
       timer.tick();
+      cpu.tick();
       cpu.cycles = 1;
       timer.tick();
+      cpu.tick();
       expect(cpu.data[TIFR1]).toEqual(OCF1A); // TIFR1 should have OCF1A bit on
       expect(cpu.pc).toEqual(0);
       expect(cpu.cycles).toEqual(1);
@@ -490,8 +541,10 @@ describe('timer', () => {
       cpu.data[0x6f] = 0x1; // TIMSK1: TOIE1
       cpu.data[SREG] = 0x80; // SREG: I-------
       timer.tick();
+      cpu.tick();
       cpu.cycles = 1;
       timer.tick();
+      cpu.tick();
       cpu.readData(TCNT1); // Refresh TCNT1
       expect(cpu.dataView.getUint16(TCNT1, true)).toEqual(2);
       expect(cpu.data[TIFR1] & TOV1).toEqual(0);
@@ -508,8 +561,10 @@ describe('timer', () => {
       cpu.writeData(ICR1, 0x10); // ...
       cpu.writeData(TCCR1B, WGM13 | WGM12 | CS10); // Set prescaler to 1, WGM: CTC
       timer.tick();
+      cpu.tick();
       cpu.cycles = 2; // 2 cycles should increment timer twice, beyond ICR1
       timer.tick();
+      cpu.tick();
       cpu.readData(TCNT1); // Refresh TCNT1
       expect(cpu.dataView.getUint16(TCNT1, true)).toEqual(0); // TCNT should be 0
       expect(cpu.data[TIFR1] & TOV1).toEqual(0);
@@ -522,6 +577,7 @@ describe('timer', () => {
       cpu.writeData(TCNT1, 0x22);
       cpu.writeData(TCNT1H, 0x55);
       timer.tick();
+      cpu.tick();
       const timerLow = cpu.readData(TCNT1);
       const timerHigh = cpu.readData(TCNT1H);
       expect((timerHigh << 8) | timerLow).toEqual(0x22);
@@ -534,8 +590,10 @@ describe('timer', () => {
       cpu.writeData(TCNT1, 0xff);
       cpu.writeData(TCCR1B, WGM12 | CS10); // Set prescaler to 1, WGM: CTC
       timer.tick();
+      cpu.tick();
       cpu.cycles = 1;
       timer.tick();
+      cpu.tick();
       // We read the high byte before the low byte, so the high byte should still have
       // the previous value:
       const timerHigh = cpu.readData(TCNT1H);
diff --git a/src/peripherals/timer.ts b/src/peripherals/timer.ts
index e347212..81c1e47 100644
--- a/src/peripherals/timer.ts
+++ b/src/peripherals/timer.ts
@@ -6,9 +6,8 @@
  * Copyright (C) 2019, 2020, Uri Shaked
  */
 
-import { CPU } from '../cpu/cpu';
-import { avrInterrupt } from '../cpu/interrupt';
-import { portBConfig, portDConfig, PinOverrideMode } from './gpio';
+import { AVRInterruptConfig, CPU } from '../cpu/cpu';
+import { PinOverrideMode, portBConfig, portDConfig } from './gpio';
 
 const timer01Dividers = {
   0: 0,
@@ -251,11 +250,33 @@ export class AVRTimer {
   private tcntUpdated = false;
   private countingUp = true;
   private divider = 0;
-  private pendingInterrupt = false;
 
   // This is the temporary register used to access 16-bit registers (section 16.3 of the datasheet)
   private highByteTemp: u8 = 0;
 
+  // Interrupts
+  private OVF: AVRInterruptConfig = {
+    address: this.config.ovfInterrupt,
+    flagRegister: this.config.TIFR,
+    flagMask: this.config.TOV,
+    enableRegister: this.config.TIMSK,
+    enableMask: this.config.TOIE,
+  };
+  private OCFA: AVRInterruptConfig = {
+    address: this.config.compAInterrupt,
+    flagRegister: this.config.TIFR,
+    flagMask: this.config.OCFA,
+    enableRegister: this.config.TIMSK,
+    enableMask: this.config.OCIEA,
+  };
+  private OCFB: AVRInterruptConfig = {
+    address: this.config.compBInterrupt,
+    flagRegister: this.config.TIFR,
+    flagMask: this.config.OCFB,
+    enableRegister: this.config.TIMSK,
+    enableMask: this.config.OCIEB,
+  };
+
   constructor(private cpu: CPU, private config: AVRTimerConfig) {
     this.updateWGMConfig();
     this.cpu.readHooks[config.TCNT] = (addr: u8) => {
@@ -308,6 +329,18 @@ export class AVRTimer {
       this.updateWGMConfig();
       return true;
     };
+    cpu.writeHooks[config.TIFR] = (value) => {
+      this.cpu.data[config.TIFR] = value;
+      this.cpu.clearInterruptByFlag(this.OVF, value);
+      this.cpu.clearInterruptByFlag(this.OCFA, value);
+      this.cpu.clearInterruptByFlag(this.OCFB, value);
+      return true;
+    };
+    cpu.writeHooks[config.TIMSK] = (value) => {
+      this.cpu.updateInterruptEnable(this.OVF, value);
+      this.cpu.updateInterruptEnable(this.OCFA, value);
+      this.cpu.updateInterruptEnable(this.OCFB, value);
+    };
   }
 
   reset() {
@@ -317,15 +350,6 @@ export class AVRTimer {
     this.ocrB = 0;
   }
 
-  get TIFR() {
-    return this.cpu.data[this.config.TIFR];
-  }
-
-  set TIFR(value: u8) {
-    this.pendingInterrupt = value > 0;
-    this.cpu.data[this.config.TIFR] = value;
-  }
-
   get TCCRA() {
     return this.cpu.data[this.config.TCCRA];
   }
@@ -384,27 +408,10 @@ export class AVRTimer {
         this.timerUpdated();
       }
       if ((timerMode === TimerMode.Normal || timerMode === TimerMode.FastPWM) && val > newVal) {
-        this.TIFR |= this.config.TOV;
+        this.cpu.setInterruptFlag(this.OVF);
       }
     }
     this.tcntUpdated = false;
-    if (this.cpu.interruptsEnabled && this.pendingInterrupt) {
-      const { TIFR, TIMSK } = this;
-      const { TOV, OCFA, OCFB, TOIE, OCIEA, OCIEB } = this.config;
-      if (TIFR & TOV && TIMSK & TOIE) {
-        avrInterrupt(this.cpu, this.config.ovfInterrupt);
-        this.TIFR &= ~TOV;
-      }
-      if (TIFR & OCFA && TIMSK & OCIEA) {
-        avrInterrupt(this.cpu, this.config.compAInterrupt);
-        this.TIFR &= ~OCFA;
-      }
-      if (TIFR & OCFB && TIMSK & OCIEB) {
-        avrInterrupt(this.cpu, this.config.compBInterrupt);
-        this.TIFR &= ~OCFB;
-      }
-      this.pendingInterrupt = false;
-    }
   }
 
   private phasePwmCount(value: u16, delta: u8) {
@@ -418,7 +425,7 @@ export class AVRTimer {
         value--;
         if (!value && !this.tcntUpdated) {
           this.countingUp = true;
-          this.TIFR |= this.config.TOV;
+          this.cpu.setInterruptFlag(this.OVF);
         }
       }
       delta--;
@@ -430,19 +437,18 @@ export class AVRTimer {
     const value = this.tcnt;
 
     if (this.ocrA && value === this.ocrA) {
-      const { TOV, OCFA } = this.config;
-      this.TIFR |= OCFA;
+      this.cpu.setInterruptFlag(this.OCFA);
       if (this.timerMode === TimerMode.CTC) {
         // Clear Timer on Compare Match (CTC) Mode
         this.tcnt = 0;
-        this.TIFR |= TOV;
+        this.cpu.setInterruptFlag(this.OVF);
       }
       if (this.compA) {
         this.updateCompPin(this.compA, 'A');
       }
     }
     if (this.ocrB && value === this.ocrB) {
-      this.TIFR |= this.config.OCFB;
+      this.cpu.setInterruptFlag(this.OCFB);
       if (this.compB) {
         this.updateCompPin(this.compB, 'B');
       }
diff --git a/src/peripherals/twi.spec.ts b/src/peripherals/twi.spec.ts
index 1a2b2ae..e43ae38 100644
--- a/src/peripherals/twi.spec.ts
+++ b/src/peripherals/twi.spec.ts
@@ -48,9 +48,11 @@ describe('TWI', () => {
   it('should trigger data an interrupt if TWINT is set', () => {
     const cpu = new CPU(new Uint16Array(1024));
     const twi = new AVRTWI(cpu, twiConfig, FREQ_16MHZ);
-    cpu.writeData(TWCR, TWINT | TWIE);
+    cpu.writeData(TWCR, TWIE);
     cpu.data[SREG] = 0x80; // SREG: I-------
+    twi.completeStart(); // This will set the TWINT flag
     twi.tick();
+    cpu.tick();
     expect(cpu.pc).toEqual(0x30); // 2-wire Serial Interface Vector
     expect(cpu.cycles).toEqual(2);
     expect(cpu.data[TWCR] & TWINT).toEqual(0);
@@ -63,6 +65,7 @@ describe('TWI', () => {
       jest.spyOn(twi.eventHandler, 'start');
       cpu.writeData(TWCR, TWINT | TWSTA | TWEN);
       twi.tick();
+      cpu.tick();
       expect(twi.eventHandler.start).toHaveBeenCalledWith(false);
     });
 
diff --git a/src/peripherals/twi.ts b/src/peripherals/twi.ts
index 52258a6..914b67f 100644
--- a/src/peripherals/twi.ts
+++ b/src/peripherals/twi.ts
@@ -1,5 +1,4 @@
-import { CPU } from '../cpu/cpu';
-import { avrInterrupt } from '../cpu/interrupt';
+import { AVRInterruptConfig, CPU } from '../cpu/cpu';
 import { u8 } from '../types';
 
 export interface TWIEventHandler {
@@ -98,13 +97,22 @@ export class AVRTWI {
 
   private nextTick: (() => void) | null = null;
 
+  // Interrupts
+  private TWI: AVRInterruptConfig = {
+    address: this.config.twiInterrupt,
+    flagRegister: this.config.TWCR,
+    flagMask: TWCR_TWINT,
+    enableRegister: this.config.TWCR,
+    enableMask: TWCR_TWIE,
+  };
+
   constructor(private cpu: CPU, private config: TWIConfig, private freqMHz: number) {
     this.updateStatus(STATUS_TWI_IDLE);
     this.cpu.writeHooks[config.TWCR] = (value) => {
+      this.cpu.data[config.TWCR] = value;
       const clearInt = value & TWCR_TWINT;
-      if (clearInt) {
-        value &= ~TWCR_TWINT;
-      }
+      this.cpu.clearInterruptByFlag(this.TWI, value);
+      this.cpu.updateInterruptEnable(this.TWI, value);
       const { status } = this;
       if (clearInt && value & TWCR_TWEN) {
         const twdrValue = this.cpu.data[this.config.TWDR];
@@ -122,7 +130,6 @@ export class AVRTWI {
             this.eventHandler.readByte(ack);
           }
         };
-        this.cpu.data[config.TWCR] = value;
         return true;
       }
     };
@@ -133,13 +140,6 @@ export class AVRTWI {
       this.nextTick();
       this.nextTick = null;
     }
-    if (this.cpu.interruptsEnabled) {
-      const { TWCR, twiInterrupt } = this.config;
-      if (this.cpu.data[TWCR] & TWCR_TWIE && this.cpu.data[TWCR] & TWCR_TWINT) {
-        avrInterrupt(this.cpu, twiInterrupt);
-        this.cpu.data[TWCR] &= ~TWCR_TWINT;
-      }
-    }
   }
 
   get prescaler() {
@@ -193,8 +193,8 @@ export class AVRTWI {
   }
 
   private updateStatus(value: u8) {
-    const { TWCR, TWSR } = this.config;
+    const { TWSR } = this.config;
     this.cpu.data[TWSR] = (this.cpu.data[TWSR] & ~TWSR_TWS_MASK) | value;
-    this.cpu.data[TWCR] |= TWCR_TWINT;
+    this.cpu.setInterruptFlag(this.TWI);
   }
 }
diff --git a/src/peripherals/usart.spec.ts b/src/peripherals/usart.spec.ts
index 2728f9c..c1e1570 100644
--- a/src/peripherals/usart.spec.ts
+++ b/src/peripherals/usart.spec.ts
@@ -150,6 +150,7 @@ describe('USART', () => {
       cpu.writeData(UCSR0B, UDRIE | TXEN);
       cpu.data[SREG] = 0x80; // SREG: I-------
       usart.tick();
+      cpu.tick();
       expect(cpu.pc).toEqual(PC_INT_UDRE);
       expect(cpu.cycles).toEqual(2);
       expect(cpu.data[UCSR0A] & UDRE).toEqual(0);
@@ -163,6 +164,7 @@ describe('USART', () => {
       cpu.data[SREG] = 0x80; // SREG: I-------
       cpu.cycles = 1e6;
       usart.tick();
+      cpu.tick();
       expect(cpu.pc).toEqual(PC_INT_TXC);
       expect(cpu.cycles).toEqual(1e6 + 2);
       expect(cpu.data[UCSR0A] & TXC).toEqual(0);
@@ -174,6 +176,7 @@ describe('USART', () => {
       cpu.writeData(UCSR0B, TXCIE | TXEN);
       cpu.data[SREG] = 0x80; // SREG: I-------
       usart.tick();
+      cpu.tick();
       expect(cpu.pc).toEqual(0);
       expect(cpu.cycles).toEqual(0);
     });
@@ -186,6 +189,7 @@ describe('USART', () => {
       cpu.data[SREG] = 0; // SREG: 0 (disable interrupts)
       cpu.cycles = 1e6;
       usart.tick();
+      cpu.tick();
       expect(cpu.pc).toEqual(0);
       expect(cpu.cycles).toEqual(1e6);
       expect(cpu.data[UCSR0A]).toEqual(TXC | UDRE);
@@ -244,9 +248,11 @@ describe('USART', () => {
       cpu.writeData(UDR0, 0x48); // 'H'
       cpu.cycles += 16000; // 1ms
       usart.tick();
+      cpu.tick();
       expect(cpu.data[UCSR0A] & TXC).toEqual(0);
       cpu.cycles += 800; // 0.05ms
       usart.tick();
+      cpu.tick();
       expect(cpu.data[UCSR0A] & TXC).toEqual(TXC);
     });
   });
diff --git a/src/peripherals/usart.ts b/src/peripherals/usart.ts
index a2037a9..c1441ad 100644
--- a/src/peripherals/usart.ts
+++ b/src/peripherals/usart.ts
@@ -6,8 +6,7 @@
  * Copyright (C) 2019, 2020, Uri Shaked
  */
 
-import { CPU } from '../cpu/cpu';
-import { avrInterrupt } from '../cpu/interrupt';
+import { AVRInterruptConfig, CPU } from '../cpu/cpu';
 import { u8 } from '../types';
 
 export interface USARTConfig {
@@ -72,14 +71,38 @@ export class AVRUSART {
 
   private lineBuffer = '';
 
+  // Interrupts
+  private UDRE: AVRInterruptConfig = {
+    address: this.config.dataRegisterEmptyInterrupt,
+    flagRegister: this.config.UCSRA,
+    flagMask: UCSRA_UDRE,
+    enableRegister: this.config.UCSRB,
+    enableMask: UCSRB_UDRIE,
+  };
+  private TXC: AVRInterruptConfig = {
+    address: this.config.txCompleteInterrupt,
+    flagRegister: this.config.UCSRA,
+    flagMask: UCSRA_TXC,
+    enableRegister: this.config.UCSRB,
+    enableMask: UCSRB_TXCIE,
+  };
+
   private txCompleteCycles = 0;
 
   constructor(private cpu: CPU, private config: USARTConfig, private freqMHz: number) {
     this.reset();
+    this.cpu.writeHooks[config.UCSRA] = (value) => {
+      cpu.data[config.UCSRA] = value;
+      cpu.clearInterruptByFlag(this.UDRE, value);
+      cpu.clearInterruptByFlag(this.TXC, value);
+      return true;
+    };
     this.cpu.writeHooks[config.UCSRB] = (value, oldValue) => {
+      cpu.updateInterruptEnable(this.UDRE, value);
+      cpu.updateInterruptEnable(this.TXC, value);
       if (value & UCSRB_TXEN && !(oldValue & UCSRB_TXEN)) {
         // Enabling the transmission - mark UDR as empty
-        this.cpu.data[config.UCSRA] |= UCSRA_UDRE;
+        cpu.setInterruptFlag(this.UDRE);
       }
     };
     this.cpu.writeHooks[config.UDR] = (value) => {
@@ -97,7 +120,8 @@ export class AVRUSART {
       }
       const symbolsPerChar = 1 + this.bitsPerChar + this.stopBits + (this.parityEnabled ? 1 : 0);
       this.txCompleteCycles = this.cpu.cycles + (this.UBRR * this.multiplier + 1) * symbolsPerChar;
-      this.cpu.data[config.UCSRA] &= ~(UCSRA_TXC | UCSRA_UDRE);
+      this.cpu.clearInterrupt(this.TXC);
+      this.cpu.clearInterrupt(this.UDRE);
     };
   }
 
@@ -110,21 +134,10 @@ export class AVRUSART {
   tick() {
     const { txCompleteCycles, cpu } = this;
     if (txCompleteCycles && cpu.cycles >= txCompleteCycles) {
-      this.cpu.data[this.config.UCSRA] |= UCSRA_UDRE | UCSRA_TXC;
+      cpu.setInterruptFlag(this.UDRE);
+      cpu.setInterruptFlag(this.TXC);
       this.txCompleteCycles = 0;
     }
-    if (cpu.interruptsEnabled) {
-      const ucsra = cpu.data[this.config.UCSRA];
-      const ucsrb = cpu.data[this.config.UCSRB];
-      if (ucsra & UCSRA_UDRE && ucsrb & UCSRB_UDRIE) {
-        avrInterrupt(cpu, this.config.dataRegisterEmptyInterrupt);
-        cpu.data[this.config.UCSRA] &= ~UCSRA_UDRE;
-      }
-      if (ucsra & UCSRA_TXC && ucsrb & UCSRB_TXCIE) {
-        avrInterrupt(cpu, this.config.txCompleteInterrupt);
-        cpu.data[this.config.UCSRA] &= ~UCSRA_TXC;
-      }
-    }
   }
 
   private get UBRR() {
diff --git a/src/types.ts b/src/types.ts
index f87f298..850cd6b 100644
--- a/src/types.ts
+++ b/src/types.ts
@@ -1,3 +1,4 @@
 export type u8 = number;
 export type u16 = number;
+export type i16 = number;
 export type u32 = number;