fix(timer): OCRH masking #117

1 files changed, 18 insertions, 3 deletions

diff --git a/src/peripherals/timer.ts b/src/peripherals/timer.ts
index 53777e2..6167394 100644
--- a/src/peripherals/timer.ts
+++ b/src/peripherals/timer.ts
@@ -388,11 +388,16 @@ export class AVRTimer {
       const updateTempRegister = (value: u8) => {
         this.highByteTemp = value;
       };
+      const updateOCRHighRegister = (value: u8, old: u8, addr: u16) => {
+        this.highByteTemp = value & (this.ocrMask >> 8);
+        cpu.data[addr] = this.highByteTemp;
+        return true;
+      };
       this.cpu.writeHooks[config.TCNT + 1] = updateTempRegister;
-      this.cpu.writeHooks[config.OCRA + 1] = updateTempRegister;
-      this.cpu.writeHooks[config.OCRB + 1] = updateTempRegister;
+      this.cpu.writeHooks[config.OCRA + 1] = updateOCRHighRegister;
+      this.cpu.writeHooks[config.OCRB + 1] = updateOCRHighRegister;
       if (this.hasOCRC) {
-        this.cpu.writeHooks[config.OCRC + 1] = updateTempRegister;
+        this.cpu.writeHooks[config.OCRC + 1] = updateOCRHighRegister;
       }
       this.cpu.writeHooks[config.ICR + 1] = updateTempRegister;
     }
@@ -481,6 +486,16 @@ export class AVRTimer {
     }
   }
 
+  get ocrMask() {
+    switch (this.topValue) {
+      case TopOCRA:
+      case TopICR:
+        return 0xffff;
+      default:
+        return this.topValue;
+    }
+  }
+
   private updateWGMConfig() {
     const { config, WGM } = this;
     const wgmModes = config.bits === 16 ? wgmModes16Bit : wgmModes8Bit;