fix(timer): Output Compare in PWM modes #78

close #78

6 files changed, 353 insertions, 67 deletions

diff --git a/src/peripherals/spi.spec.ts b/src/peripherals/spi.spec.ts
index 635e657..f323b88 100644
--- a/src/peripherals/spi.spec.ts
+++ b/src/peripherals/spi.spec.ts
@@ -160,7 +160,7 @@ describe('SPI', () => {
       return 0x5b; // we copy this byte to
     };
 
-    const runner = new TestProgramRunner(cpu);
+    const runner = new TestProgramRunner(cpu, () => 0);
     runner.runToBreak();
 
     // 16 cycles per clock * 8 bits = 128
diff --git a/src/peripherals/timer.spec.ts b/src/peripherals/timer.spec.ts
index 3b5026c..1fa0dc6 100644
--- a/src/peripherals/timer.spec.ts
+++ b/src/peripherals/timer.spec.ts
@@ -246,6 +246,7 @@ describe('timer', () => {
     cpu.tick();
     cpu.cycles = 2;
     cpu.tick();
+    expect(cpu.readData(TCNT0)).toEqual(0);
     expect(cpu.data[TIFR0] & (OCF0A | OCF0B)).toEqual(OCF0A | OCF0B);
     expect(cpu.pc).toEqual(0);
     expect(cpu.cycles).toEqual(2);
@@ -417,7 +418,7 @@ describe('timer', () => {
     const { program, instructionCount } = asmProgram(`
       LDI r16, 0x1    ; TCCR0B = 1 << CS00;
       OUT 0x25, r16
-      LDI r16, 0x30   ; TCNT <- 0x30
+      LDI r16, 0x30   ; TCNT0 <- 0x30
       OUT 0x26, r16
       NOP
       IN r17, 0x26    ; r17 <- TCNT
@@ -495,6 +496,152 @@ describe('timer', () => {
     expect(cpu.readData(R17)).toEqual(2);
   });
 
+  describe('Fast PWM mode', () => {
+    it('should set OC0A on Compare Match, clear on Bottom (issue #78)', () => {
+      const { program, labels } = asmProgram(`
+        LDI r16, 0xfc   ; TCNT0 = 0xfc;
+        OUT 0x26, r16
+        LDI r16, 0xfe   ; OCR0A = 0xfe;
+        OUT 0x27, r16  
+        ; WGM: Fast PWM, enable OC0A mode 3 (set on Compare Match, clear on Bottom)
+        LDI r16, 0xc3   ; TCCR0A = (1 << COM0A1) | (1 << COM0A0) | (1 << WGM01) | (1 << WGM00);
+        OUT 0x24, r16
+        LDI r16, 0x1    ; TCCR0B = 1 << CS00;
+        OUT 0x25, r16
+
+        NOP             ; TCNT is now 0xfd
+      beforeMatch: 
+        NOP             ; TCNT is now 0xfe (Compare Match)
+      afterMatch:
+        NOP             ; TCNT is now 0xff
+      beforeBottom:     
+        NOP             ; TCNT is now 0x00 (BOTTOM)
+      afterBottom:
+        NOP
+      `);
+
+      const cpu = new CPU(program);
+      new AVRTimer(cpu, timer0Config);
+
+      // Listen to Port D's internal callback
+      const gpioCallback = jest.fn();
+      cpu.gpioTimerHooks[PORTD] = gpioCallback;
+
+      const runner = new TestProgramRunner(cpu);
+
+      runner.runToAddress(labels.beforeMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfd);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Enable, 0x2b); // OC0A: Enable
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.afterMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfe);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Set, 0x2b); // OC0A: Set
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.beforeBottom);
+      expect(cpu.readData(TCNT0)).toEqual(0xff);
+      expect(gpioCallback).toHaveBeenCalledTimes(0);
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.afterBottom);
+      expect(cpu.readData(TCNT0)).toEqual(0x0);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Clear, 0x2b); // OC0A: Clear
+    });
+
+    it('should toggle OC0A on Compare Match when COM0An = 1 (issue #78)', () => {
+      const { program, labels } = asmProgram(`
+        LDI r16, 0xfc   ; TCNT0 = 0xfc;
+        OUT 0x26, r16
+        LDI r16, 0xfe   ; OCR0A = 0xfe;
+        OUT 0x27, r16  
+        ; WGM: Fast PWM, enable OC0A mode 1 (Toggle)
+        LDI r16, 0x43   ; TCCR0A = (1 << COM0A0) | (1 << WGM01) | (1 << WGM00);
+        OUT 0x24, r16
+        LDI r16, 0x09   ; TCCR0B = (1 << WGM02) | (1 << CS00);
+        OUT 0x25, r16
+
+        NOP             ; TCNT is now 0xfd
+      beforeMatch: 
+        NOP             ; TCNT is now 0xfe (Compare Match, TOP)
+      afterMatch:
+        NOP             ; TCNT is now 0
+      afterOverflow:
+        NOP
+      `);
+
+      const cpu = new CPU(program);
+      new AVRTimer(cpu, timer0Config);
+
+      // Listen to Port D's internal callback
+      const gpioCallback = jest.fn();
+      cpu.gpioTimerHooks[PORTD] = gpioCallback;
+
+      const runner = new TestProgramRunner(cpu);
+
+      runner.runToAddress(labels.beforeMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfd);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Enable, 0x2b); // OC0A: Enable
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.afterMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfe);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Toggle, 0x2b); // OC0A: Toggle
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.afterOverflow);
+      expect(cpu.readData(TCNT0)).toEqual(0);
+      expect(gpioCallback).toHaveBeenCalledTimes(0);
+    });
+
+    it('should leave OC0A disconnected when COM0An = 1 and WGM02 = 0 (issue #78)', () => {
+      const { program, labels } = asmProgram(`
+        LDI r16, 0xfc   ; TCNT0 = 0xfc;
+        OUT 0x26, r16
+        LDI r16, 0xfe   ; OCR0A = 0xfe;
+        OUT 0x27, r16  
+        ; WGM: Fast PWM mode 7, enable OC0A mode 1 (Toggle)
+        LDI r16, 0x43   ; TCCR0A = (1 << COM0A0) | (1 << WGM01) | (1 << WGM00);
+        OUT 0x24, r16
+        LDI r16, 0x09   ; TCCR0B = (1 << WGM02) | (1 << CS00);
+        OUT 0x25, r16
+        
+      beforeClearWGM02:
+        LDI r16, 0x01   ; TCCR0B = (1 << CS00);
+        OUT 0x25, r16
+
+      afterClearWGM02:
+        NOP
+      `);
+
+      const cpu = new CPU(program);
+      new AVRTimer(cpu, timer0Config);
+
+      // Listen to Port D's internal callback
+      const gpioCallback = jest.fn();
+      cpu.gpioTimerHooks[PORTD] = gpioCallback;
+
+      const runner = new TestProgramRunner(cpu);
+
+      // First, run with the bit set and assert that the Pin Override was enabled (OC0A connected)
+      runner.runToAddress(labels.beforeClearWGM02);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Enable, 0x2b);
+      gpioCallback.mockClear();
+
+      // Now clear WGM02 and observe that Pin Override was disabled (OC0A disconnected)
+      runner.runToAddress(labels.afterClearWGM02);
+      expect(gpioCallback).toHaveBeenCalledTimes(1);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.None, 0x2b);
+      gpioCallback.mockClear();
+    });
+  });
+
   describe('Phase-correct PWM mode', () => {
     it('should count up to TOP, down to 0, and then set TOV flag', () => {
       const { program, instructionCount } = asmProgram(`
@@ -575,6 +722,71 @@ describe('timer', () => {
       expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Set, 0x2b);
     });
 
+    it('should toggle OC0A when TCNT0=OCR0A and COM0An=1 (issue #78)', () => {
+      const { program, labels } = asmProgram(`
+        LDI r16, 0xfe   ; OCR0A = 0xfe;   // <- TOP value
+        OUT 0x27, r16  
+        ; Set waveform generation mode (WGM) to PWM, Phase Correct (mode 5)
+        LDI r16, 0x41   ; TCCR0A = (1 << COM0A0) | (1 << WGM00);
+        OUT 0x24, r16  
+        LDI r16, 0x09   ; TCCR0B = (1 << WGM02) | (1 << CS00);
+        OUT 0x25, r16  
+        LDI r16, 0xfd   ; TCNT0 = 0xfd;
+        OUT 0x26, r16  
+
+      beforeMatch:
+        NOP             ; TCNT0 will be 0xfe
+      afterMatch:
+        NOP
+      `);
+
+      const cpu = new CPU(program);
+      new AVRTimer(cpu, timer0Config);
+
+      // Listen to Port D's internal callback
+      const gpioCallback = jest.fn();
+      cpu.gpioTimerHooks[PORTD] = gpioCallback;
+
+      const runner = new TestProgramRunner(cpu);
+
+      runner.runToAddress(labels.beforeMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfd);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Enable, 0x2b);
+      gpioCallback.mockClear();
+
+      runner.runToAddress(labels.afterMatch);
+      expect(cpu.readData(TCNT0)).toEqual(0xfe);
+      expect(gpioCallback).toHaveBeenCalledWith(6, PinOverrideMode.Toggle, 0x2b);
+      gpioCallback.mockClear();
+    });
+
+    it('should leave OC0A disconnected TCNT0=OCR0A and COM0An=1 in WGM mode 1 (issue #78)', () => {
+      const { program, instructionCount } = asmProgram(`
+        LDI r16, 0xfe   ; OCR0A = 0xfe;   // <- TOP value
+        OUT 0x27, r16  
+        ; Set waveform generation mode (WGM) to PWM, Phase Correct (mode 1)
+        LDI r16, 0x41   ; TCCR0A = (1 << COM0A0) | (1 << WGM00);
+        OUT 0x24, r16  
+        LDI r16, 0x01   ; TCCR0B = (1 << CS00);
+        OUT 0x25, r16  
+        LDI r16, 0xfd   ; TCNT0 = 0xfd;
+        OUT 0x26, r16  
+      `);
+
+      const cpu = new CPU(program);
+      new AVRTimer(cpu, timer0Config);
+
+      // Listen to Port D's internal callback
+      const gpioCallback = jest.fn();
+      cpu.gpioTimerHooks[PORTD] = gpioCallback;
+
+      const runner = new TestProgramRunner(cpu);
+      runner.runInstructions(instructionCount);
+
+      // Assert that the pin callback wasn't called (thus it's disconnected)
+      expect(gpioCallback).not.toHaveBeenCalled();
+    });
+
     it('should not miss Compare Match when executing multi-cycle instruction (issue #79)', () => {
       const { program, instructionCount } = asmProgram(`
         LDI r16, 0x10   ; OCR0A = 0x10;   // <- TOP value
@@ -793,7 +1005,7 @@ describe('timer', () => {
       expect(gpioCallback).toHaveBeenCalledWith(2, PinOverrideMode.Toggle, 0x25);
     });
 
-    it('should only update OCR0A when TCNT0=BOTTOM in PWM Phase/Frequency Correct mode (issue #76)', () => {
+    it('should only update OCR1A when TCNT1=BOTTOM in PWM Phase/Frequency Correct mode (issue #76)', () => {
       const { program, instructionCount } = asmProgram(`
         LDI r16, 0x0    ; OCR1AH = 0x0;
         STS 0x89, r16  
diff --git a/src/peripherals/timer.ts b/src/peripherals/timer.ts
index 6ea29e3..b1a64d9 100644
--- a/src/peripherals/timer.ts
+++ b/src/peripherals/timer.ts
@@ -189,37 +189,42 @@ const TopOCRA = 1;
 const TopICR = 2;
 type TimerTopValue = 0xff | 0x1ff | 0x3ff | 0xffff | typeof TopOCRA | typeof TopICR;
 
-type WGMConfig = [TimerMode, TimerTopValue, OCRUpdateMode, TOVUpdateMode];
+type WGMConfig = [TimerMode, TimerTopValue, OCRUpdateMode, TOVUpdateMode, number];
+
+// Enable Toggle mode for OCxA in PWM Wave Generation mode
+const OCToggle = 1;
+
+const { Normal, PWMPhaseCorrect, CTC, FastPWM, Reserved, PWMPhaseFrequencyCorrect } = TimerMode;
 
 const wgmModes8Bit: WGMConfig[] = [
-  /*0*/ [TimerMode.Normal, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*1*/ [TimerMode.PWMPhaseCorrect, 0xff, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*2*/ [TimerMode.CTC, TopOCRA, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*3*/ [TimerMode.FastPWM, 0xff, OCRUpdateMode.Bottom, TOVUpdateMode.Max],
-  /*4*/ [TimerMode.Reserved, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*5*/ [TimerMode.PWMPhaseCorrect, TopOCRA, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*6*/ [TimerMode.Reserved, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*7*/ [TimerMode.FastPWM, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
+  /*0*/ [Normal, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*1*/ [PWMPhaseCorrect, 0xff, OCRUpdateMode.Top, TOVUpdateMode.Bottom, 0],
+  /*2*/ [CTC, TopOCRA, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*3*/ [FastPWM, 0xff, OCRUpdateMode.Bottom, TOVUpdateMode.Max, 0],
+  /*4*/ [Reserved, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*5*/ [PWMPhaseCorrect, TopOCRA, OCRUpdateMode.Top, TOVUpdateMode.Bottom, OCToggle],
+  /*6*/ [Reserved, 0xff, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*7*/ [FastPWM, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Top, OCToggle],
 ];
 
 // Table 16-4 in the datasheet
 const wgmModes16Bit: WGMConfig[] = [
-  /*0 */ [TimerMode.Normal, 0xffff, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*1 */ [TimerMode.PWMPhaseCorrect, 0x00ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*2 */ [TimerMode.PWMPhaseCorrect, 0x01ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*3 */ [TimerMode.PWMPhaseCorrect, 0x03ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*4 */ [TimerMode.CTC, TopOCRA, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*5 */ [TimerMode.FastPWM, 0x00ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
-  /*6 */ [TimerMode.FastPWM, 0x01ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
-  /*7 */ [TimerMode.FastPWM, 0x03ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
-  /*8 */ [TimerMode.PWMPhaseFrequencyCorrect, TopICR, OCRUpdateMode.Bottom, TOVUpdateMode.Bottom],
-  /*9 */ [TimerMode.PWMPhaseFrequencyCorrect, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Bottom],
-  /*10*/ [TimerMode.PWMPhaseCorrect, TopICR, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*11*/ [TimerMode.PWMPhaseCorrect, TopOCRA, OCRUpdateMode.Top, TOVUpdateMode.Bottom],
-  /*12*/ [TimerMode.CTC, TopICR, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*13*/ [TimerMode.Reserved, 0xffff, OCRUpdateMode.Immediate, TOVUpdateMode.Max],
-  /*14*/ [TimerMode.FastPWM, TopICR, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
-  /*15*/ [TimerMode.FastPWM, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Top],
+  /*0 */ [Normal, 0xffff, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*1 */ [PWMPhaseCorrect, 0x00ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom, 0],
+  /*2 */ [PWMPhaseCorrect, 0x01ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom, 0],
+  /*3 */ [PWMPhaseCorrect, 0x03ff, OCRUpdateMode.Top, TOVUpdateMode.Bottom, 0],
+  /*4 */ [CTC, TopOCRA, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*5 */ [FastPWM, 0x00ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top, 0],
+  /*6 */ [FastPWM, 0x01ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top, 0],
+  /*7 */ [FastPWM, 0x03ff, OCRUpdateMode.Bottom, TOVUpdateMode.Top, 0],
+  /*8 */ [PWMPhaseFrequencyCorrect, TopICR, OCRUpdateMode.Bottom, TOVUpdateMode.Bottom, 0],
+  /*9 */ [PWMPhaseFrequencyCorrect, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Bottom, OCToggle],
+  /*10*/ [PWMPhaseCorrect, TopICR, OCRUpdateMode.Top, TOVUpdateMode.Bottom, 0],
+  /*11*/ [PWMPhaseCorrect, TopOCRA, OCRUpdateMode.Top, TOVUpdateMode.Bottom, OCToggle],
+  /*12*/ [CTC, TopICR, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*13*/ [Reserved, 0xffff, OCRUpdateMode.Immediate, TOVUpdateMode.Max, 0],
+  /*14*/ [FastPWM, TopICR, OCRUpdateMode.Bottom, TOVUpdateMode.Top, OCToggle],
+  /*15*/ [FastPWM, TopOCRA, OCRUpdateMode.Bottom, TOVUpdateMode.Top, OCToggle],
 ];
 
 type CompBitsValue = 0 | 1 | 2 | 3;
@@ -329,10 +334,6 @@ export class AVRTimer {
     }
     cpu.writeHooks[config.TCCRA] = (value) => {
       this.cpu.data[config.TCCRA] = value;
-      this.compA = ((value >> 6) & 0x3) as CompBitsValue;
-      this.updateCompA(this.compA ? PinOverrideMode.Enable : PinOverrideMode.None);
-      this.compB = ((value >> 4) & 0x3) as CompBitsValue;
-      this.updateCompB(this.compB ? PinOverrideMode.Enable : PinOverrideMode.None);
       this.updateWGMConfig();
       return true;
     };
@@ -406,12 +407,37 @@ export class AVRTimer {
   }
 
   private updateWGMConfig() {
-    const wgmModes = this.config.bits === 16 ? wgmModes16Bit : wgmModes8Bit;
-    const [timerMode, topValue, ocrUpdateMode, tovUpdateMode] = wgmModes[this.WGM];
+    const { config, WGM } = this;
+    const wgmModes = config.bits === 16 ? wgmModes16Bit : wgmModes8Bit;
+    const TCCRA = this.cpu.data[config.TCCRA];
+    const [timerMode, topValue, ocrUpdateMode, tovUpdateMode, flags] = wgmModes[WGM];
     this.timerMode = timerMode;
     this.topValue = topValue;
     this.ocrUpdateMode = ocrUpdateMode;
     this.tovUpdateMode = tovUpdateMode;
+
+    const pwmMode =
+      timerMode === FastPWM ||
+      timerMode === PWMPhaseCorrect ||
+      timerMode === PWMPhaseFrequencyCorrect;
+
+    const prevCompA = this.compA;
+    this.compA = ((TCCRA >> 6) & 0x3) as CompBitsValue;
+    if (this.compA === 1 && pwmMode && !(flags & OCToggle)) {
+      this.compA = 0;
+    }
+    if (!!prevCompA !== !!this.compA) {
+      this.updateCompA(this.compA ? PinOverrideMode.Enable : PinOverrideMode.None);
+    }
+
+    const prevCompB = this.compB;
+    this.compB = ((TCCRA >> 4) & 0x3) as CompBitsValue;
+    if (this.compB === 1 && pwmMode) {
+      this.compB = 0; // Reserved, according to the datasheet
+    }
+    if (!!prevCompB !== !!this.compB) {
+      this.updateCompB(this.compB ? PinOverrideMode.Enable : PinOverrideMode.None);
+    }
   }
 
   count = (reschedule = true) => {
@@ -423,8 +449,7 @@ export class AVRTimer {
       this.lastCycle += counterDelta * divider;
       const val = this.tcnt;
       const { timerMode, TOP } = this;
-      const phasePwm =
-        timerMode === TimerMode.PWMPhaseCorrect || timerMode === TimerMode.PWMPhaseFrequencyCorrect;
+      const phasePwm = timerMode === PWMPhaseCorrect || timerMode === PWMPhaseFrequencyCorrect;
       const newVal = phasePwm
         ? this.phasePwmCount(val, counterDelta)
         : (val + counterDelta) % (TOP + 1);
@@ -432,10 +457,22 @@ export class AVRTimer {
       // A CPU write overrides (has priority over) all counter clear or count operations.
       if (!this.tcntUpdated) {
         this.tcnt = newVal;
-        this.timerUpdated(newVal, val);
+        if (!phasePwm) {
+          this.timerUpdated(newVal, val);
+        }
       }
 
       if (!phasePwm) {
+        if (timerMode === FastPWM && overflow) {
+          const { compA, compB } = this;
+          if (compA) {
+            this.updateCompPin(compA, 'A', true);
+          }
+          if (compB) {
+            this.updateCompPin(compB, 'B', true);
+          }
+        }
+
         if (this.ocrUpdateMode == OCRUpdateMode.Bottom && overflow) {
           // OCRUpdateMode.Top only occurs in Phase Correct modes, handled by phasePwmCount()
           this.ocrA = this.nextOcrA;
@@ -476,10 +513,11 @@ export class AVRTimer {
   };
 
   private phasePwmCount(value: u16, delta: u8) {
+    const { ocrA, ocrB, TOP, tcntUpdated } = this;
     while (delta > 0) {
       if (this.countingUp) {
         value++;
-        if (value === this.TOP && !this.tcntUpdated) {
+        if (value === TOP && !tcntUpdated) {
           this.countingUp = false;
           if (this.ocrUpdateMode === OCRUpdateMode.Top) {
             this.ocrA = this.nextOcrA;
@@ -488,7 +526,7 @@ export class AVRTimer {
         }
       } else {
         value--;
-        if (!value && !this.tcntUpdated) {
+        if (!value && !tcntUpdated) {
           this.countingUp = true;
           this.cpu.setInterruptFlag(this.OVF);
           if (this.ocrUpdateMode === OCRUpdateMode.Bottom) {
@@ -497,28 +535,33 @@ export class AVRTimer {
           }
         }
       }
+      if (!tcntUpdated && value === ocrA) {
+        this.cpu.setInterruptFlag(this.OCFA);
+        if (this.compA) {
+          this.updateCompPin(this.compA, 'A');
+        }
+      }
+      if (!tcntUpdated && value === ocrB) {
+        this.cpu.setInterruptFlag(this.OCFB);
+        if (this.compB) {
+          this.updateCompPin(this.compB, 'B');
+        }
+      }
       delta--;
     }
     return value;
   }
 
   private timerUpdated(value: number, prevValue: number) {
-    const { ocrA, ocrB, countingUp } = this;
-    if (
-      (countingUp && prevValue < ocrA && value >= ocrA) ||
-      (countingUp && prevValue > value && ocrA >= value) ||
-      (!countingUp && prevValue > ocrA && value <= ocrA)
-    ) {
+    const { ocrA, ocrB } = this;
+    const overflow = prevValue > value;
+    if ((prevValue < ocrA || overflow) && value >= ocrA) {
       this.cpu.setInterruptFlag(this.OCFA);
       if (this.compA) {
         this.updateCompPin(this.compA, 'A');
       }
     }
-    if (
-      (countingUp && prevValue < ocrB && value >= ocrB) ||
-      (countingUp && prevValue > value && ocrB >= value) ||
-      (!countingUp && prevValue > ocrB && value <= ocrB)
-    ) {
+    if ((prevValue < ocrB || overflow) && value >= ocrB) {
       this.cpu.setInterruptFlag(this.OCFB);
       if (this.compB) {
         this.updateCompPin(this.compB, 'B');
@@ -526,20 +569,31 @@ export class AVRTimer {
     }
   }
 
-  private updateCompPin(compValue: CompBitsValue, pinName: 'A' | 'B') {
+  private updateCompPin(compValue: CompBitsValue, pinName: 'A' | 'B', bottom = false) {
     let newValue: PinOverrideMode = PinOverrideMode.None;
-    const inverted = compValue === 3;
-    const isSet = this.countingUp === inverted;
+    const invertingMode = compValue === 3;
+    const isSet = this.countingUp === invertingMode;
     switch (this.timerMode) {
-      case TimerMode.Normal:
-      case TimerMode.CTC:
-      case TimerMode.FastPWM:
+      case Normal:
+      case CTC:
         newValue = compToOverride(compValue);
         break;
 
-      case TimerMode.PWMPhaseCorrect:
-      case TimerMode.PWMPhaseFrequencyCorrect:
-        newValue = isSet ? PinOverrideMode.Set : PinOverrideMode.Clear;
+      case FastPWM:
+        if (compValue === 1) {
+          newValue = bottom ? PinOverrideMode.None : PinOverrideMode.Toggle;
+        } else {
+          newValue = invertingMode !== bottom ? PinOverrideMode.Set : PinOverrideMode.Clear;
+        }
+        break;
+
+      case PWMPhaseCorrect:
+      case PWMPhaseFrequencyCorrect:
+        if (compValue === 1) {
+          newValue = PinOverrideMode.Toggle;
+        } else {
+          newValue = isSet ? PinOverrideMode.Set : PinOverrideMode.Clear;
+        }
         break;
     }
 
diff --git a/src/utils/assembler.spec.ts b/src/utils/assembler.spec.ts
index f0daf64..836f388 100644
--- a/src/utils/assembler.spec.ts
+++ b/src/utils/assembler.spec.ts
@@ -14,6 +14,7 @@ describe('AVR assembler', () => {
       bytes: bytes('0b0d'),
       errors: [],
       lines: [{ byteOffset: 0, bytes: '0d0b', line: 1, text: 'ADD r16, r11' }],
+      labels: {},
     });
   });
 
@@ -36,6 +37,7 @@ describe('AVR assembler', () => {
       bytes: new Uint8Array(0),
       errors: [],
       lines: [],
+      labels: {},
     });
   });
 
@@ -44,6 +46,7 @@ describe('AVR assembler', () => {
       bytes: new Uint8Array(0),
       errors: ['Line 0: Rd out of range: 16<>31'],
       lines: [],
+      labels: {},
     });
   });
 
diff --git a/src/utils/assembler.ts b/src/utils/assembler.ts
index 4823937..278e944 100644
--- a/src/utils/assembler.ts
+++ b/src/utils/assembler.ts
@@ -29,7 +29,7 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-interface LabelTable {
+export interface LabelTable {
   [key: string]: number;
 }
 
@@ -938,7 +938,7 @@ function passTwo(lineTable: LineTablePass1[], labels: LabelTable) {
     }
   }
 
-  return { errors: errorTable, bytes: resultTable, lines: lineTable as LineTable[] };
+  return { errors: errorTable, bytes: resultTable, lines: lineTable as LineTable[], labels };
 }
 
 /**
@@ -951,6 +951,7 @@ export function assemble(input: string) {
       bytes: new Uint8Array(0),
       errors: mid.errors,
       lines: [],
+      labels: {},
     };
   }
   return passTwo(mid.lines, mid.labels);
diff --git a/src/utils/test-utils.ts b/src/utils/test-utils.ts
index 4b2efff..e620748 100644
--- a/src/utils/test-utils.ts
+++ b/src/utils/test-utils.ts
@@ -5,38 +5,54 @@ import { avrInstruction } from '../cpu/instruction';
 const BREAK_OPCODE = 0x9598;
 
 export function asmProgram(source: string) {
-  const { bytes, errors, lines } = assemble(source);
+  const { bytes, errors, lines, labels } = assemble(source);
   if (errors.length) {
     throw new Error('Assembly failed: ' + errors);
   }
-  return { program: new Uint16Array(bytes.buffer), lines, instructionCount: lines.length };
+  return { program: new Uint16Array(bytes.buffer), lines, instructionCount: lines.length, labels };
 }
 
+const defaultOnBreak = () => {
+  throw new Error('BREAK instruction encountered');
+};
+
 export class TestProgramRunner {
-  constructor(private readonly cpu: CPU, private readonly onBreak?: (cpu: CPU) => void) {}
+  constructor(
+    private readonly cpu: CPU,
+    private readonly onBreak: (cpu: CPU) => void = defaultOnBreak
+  ) {}
 
   runInstructions(count: number) {
     const { cpu, onBreak } = this;
     for (let i = 0; i < count; i++) {
       if (cpu.progMem[cpu.pc] === BREAK_OPCODE) {
         onBreak?.(cpu);
-        throw new Error('BREAK instruction encountered');
       }
       avrInstruction(cpu);
       cpu.tick();
     }
   }
 
-  runToBreak(maxIterations = 5000) {
+  runUntil(predicate: (cpu: CPU) => boolean, maxIterations = 5000) {
     const { cpu, onBreak } = this;
     for (let i = 0; i < maxIterations; i++) {
       if (cpu.progMem[cpu.pc] === BREAK_OPCODE) {
         onBreak?.(cpu);
+      }
+      if (predicate(cpu)) {
         return;
       }
       avrInstruction(cpu);
       cpu.tick();
     }
-    throw new Error('Program ran for too long without a BREAK instruction');
+    throw new Error('Test program ran for too long, check your predicate');
+  }
+
+  runToBreak() {
+    this.runUntil((cpu) => cpu.progMem[cpu.pc] === BREAK_OPCODE);
+  }
+
+  runToAddress(byteAddr: number) {
+    this.runUntil((cpu) => cpu.pc * 2 === byteAddr);
   }
 }