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

fix #103

2 files changed, 42 insertions, 2 deletions

diff --git a/src/peripherals/gpio.spec.ts b/src/peripherals/gpio.spec.ts
index ed44264..67cd779 100644
--- a/src/peripherals/gpio.spec.ts
+++ b/src/peripherals/gpio.spec.ts
@@ -1,4 +1,5 @@
 import { CPU } from '../cpu/cpu';
+import { asmProgram, TestProgramRunner } from '../utils/test-utils';
 import { AVRIOPort, portBConfig, PinState, portDConfig, PinOverrideMode } from './gpio';
 
 // CPU registers
@@ -8,6 +9,9 @@ const SREG = 95;
 const PINB = 0x23;
 const DDRB = 0x24;
 const PORTB = 0x25;
+const PIND = 0x29;
+const DDRD = 0x2a;
+const PORTD = 0x2b;
 const EIFR = 0x3c;
 const EIMSK = 0x3d;
 const PCICR = 0x68;
@@ -76,6 +80,42 @@ describe('GPIO', () => {
     expect(cpu.data[PINB]).toEqual(0x4); // PINB should return port value
   });
 
+  it('should only affect one pin when writing to PIN using SBI (issue #103)', () => {
+    const { program } = asmProgram(`
+    ; register addresses
+    _REPLACE DDRD, ${DDRD - 0x20}
+    _REPLACE PIND, ${PIND - 0x20}
+    _REPLACE PORTD, ${PORTD - 0x20}
+
+    ; Setup
+    ldi r24, 0x48
+    out DDRD, r24
+    out PORTD, r24
+
+    ; Now toggle pin 6 with SBI
+    sbi PIND, 6
+
+    break
+  `);
+    const cpu = new CPU(program);
+    const portD = new AVRIOPort(cpu, portDConfig);
+    const runner = new TestProgramRunner(cpu);
+
+    const listener = jest.fn();
+    portD.addListener(listener);
+
+    // Setup: pins 6, 3 are output, set to HIGH
+    runner.runInstructions(3);
+    expect(listener).toHaveBeenCalledWith(0x48, 0x0);
+    expect(cpu.data[PORTD]).toEqual(0x48);
+    listener.mockReset();
+
+    // Now we toggle pin 6
+    runner.runInstructions(1);
+    expect(listener).toHaveBeenCalledWith(0x08, 0x48);
+    expect(cpu.data[PORTD]).toEqual(0x8);
+  });
+
   it('should update the PIN register on output compare (OCR) match (issue #102)', () => {
     const cpu = new CPU(new Uint16Array(1024));
     const port = new AVRIOPort(cpu, portBConfig);
diff --git a/src/peripherals/gpio.ts b/src/peripherals/gpio.ts
index 6d1dae8..07667d9 100644
--- a/src/peripherals/gpio.ts
+++ b/src/peripherals/gpio.ts
@@ -229,11 +229,11 @@ export class AVRIOPort {
       this.updatePinRegister(ddrMask);
       return true;
     };
-    cpu.writeHooks[portConfig.PIN] = (value: u8) => {
+    cpu.writeHooks[portConfig.PIN] = (value: u8, oldValue, addr, mask) => {
       // Writing to 1 PIN toggles PORT bits
       const oldPortValue = cpu.data[portConfig.PORT];
       const ddrMask = cpu.data[portConfig.DDR];
-      const portValue = oldPortValue ^ value;
+      const portValue = oldPortValue ^ (value & mask);
       cpu.data[portConfig.PORT] = portValue;
       this.writeGpio(portValue, ddrMask);
       this.updatePinRegister(ddrMask);