From 5bc85ec07ac00013de4681cb5d268e4567709be0 Mon Sep 17 00:00:00 2001
From: Uri Shaked
Date: Tue, 7 Sep 2021 00:42:42 +0300
Subject: fix(gpio): CBI/SBI handling in writes to PIN register #103

fix #103
---
 src/cpu/cpu.ts               |  8 ++++----
 src/cpu/instruction.ts       |  6 ++++--
 src/peripherals/gpio.spec.ts | 40 ++++++++++++++++++++++++++++++++++++++++
 src/peripherals/gpio.ts      |  4 ++--
 4 files changed, 50 insertions(+), 8 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 */
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);
-- 
cgit v1.2.3