fix(gpio): CBI/SBI handling in writes to PIN register #103

fix #103

2 files changed, 8 insertions, 6 deletions

diff --git a/src/cpu/cpu.ts b/src/cpu/cpu.ts
index 096d9eb..de5b294 100644
--- a/src/cpu/cpu.ts
+++ b/src/cpu/cpu.ts
@@ -34,10 +34,10 @@ export interface ICPU {
   cycles: number;
 
   readData(addr: u16): u8;
-  writeData(addr: u16, value: u8): void;
+  writeData(addr: u16, value: u8, mask?: u8): void;
 }
 
-export type CPUMemoryHook = (value: u8, oldValue: u8, addr: u16) => boolean | void;
+export type CPUMemoryHook = (value: u8, oldValue: u8, addr: u16, mask: u8) => boolean | void;
 export interface CPUMemoryHooks {
   [key: number]: CPUMemoryHook;
 }
@@ -102,10 +102,10 @@ export class CPU implements ICPU {
     return this.data[addr];
   }
 
-  writeData(addr: number, value: number) {
+  writeData(addr: number, value: number, mask = 0xff) {
     const hook = this.writeHooks[addr];
     if (hook) {
-      if (hook(value, this.data[addr], addr)) {
+      if (hook(value, this.data[addr], addr, mask)) {
         return;
       }
     }
diff --git a/src/cpu/instruction.ts b/src/cpu/instruction.ts
index bd9d2a2..c3cdb92 100644
--- a/src/cpu/instruction.ts
+++ b/src/cpu/instruction.ts
@@ -149,7 +149,8 @@ export function avrInstruction(cpu: ICPU) {
     const A = opcode & 0xf8;
     const b = opcode & 7;
     const R = cpu.readData((A >> 3) + 32);
-    cpu.writeData((A >> 3) + 32, R & ~(1 << b));
+    const mask = 1 << b;
+    cpu.writeData((A >> 3) + 32, R & ~mask, mask);
   } else if ((opcode & 0xfe0f) === 0x9400) {
     /* COM, 1001 010d dddd 0000 */
     const d = (opcode & 0x1f0) >> 4;
@@ -603,7 +604,8 @@ export function avrInstruction(cpu: ICPU) {
   } 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)));
+    const mask = 1 << (opcode & 7);
+    cpu.writeData(target, cpu.readData(target) | mask, mask);
     cpu.cycles++;
   } else if ((opcode & 0xff00) === 0x9900) {
     /* SBIC, 1001 1001 AAAA Abbb */