From 09f2385282b0c98a3fdc4b89091d0c62b61725f5 Mon Sep 17 00:00:00 2001
From: Uri Shaked <uri@urishaked.com>
Date: Wed, 2 Sep 2020 15:37:34 +0300
Subject: [PATCH] fix(interrupt): broken on ATmega2560

close #58
---
 src/cpu/interrupt.spec.ts | 19 +++++++++++++++++++
 src/cpu/interrupt.ts      |  5 ++++-
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/cpu/interrupt.spec.ts b/src/cpu/interrupt.spec.ts
index cc54e3c..c64724d 100644
--- a/src/cpu/interrupt.spec.ts
+++ b/src/cpu/interrupt.spec.ts
@@ -16,4 +16,23 @@ describe('avrInterrupt', () => {
     expect(cpu.data[0x7f]).toEqual(0x5); // Return addr high
     expect(cpu.data[95]).toEqual(0b00000001); // SREG: -------C
   });
+
+  it('should push a 3-byte return address when running in 22-bit PC mode (issue #58)', () => {
+    const cpu = new CPU(new Uint16Array(0x80000));
+    expect(cpu.pc22Bits).toEqual(true);
+
+    cpu.pc = 0x10520;
+    cpu.data[94] = 0;
+    cpu.data[93] = 0x80; // SP <- 0x80
+    cpu.data[95] = 0b10000001; // SREG <- I------C
+
+    avrInterrupt(cpu, 5);
+    expect(cpu.cycles).toEqual(2);
+    expect(cpu.pc).toEqual(5);
+    expect(cpu.data[93]).toEqual(0x7d); // SP should decrement by 3
+    expect(cpu.data[0x80]).toEqual(0x20); // Return addr low
+    expect(cpu.data[0x7f]).toEqual(0x05); // Return addr high
+    expect(cpu.data[0x7e]).toEqual(0x1); // Return addr extended
+    expect(cpu.data[95]).toEqual(0b00000001); // SREG: -------C
+  });
 });
diff --git a/src/cpu/interrupt.ts b/src/cpu/interrupt.ts
index 1c7d835..1bfa035 100644
--- a/src/cpu/interrupt.ts
+++ b/src/cpu/interrupt.ts
@@ -12,7 +12,10 @@ export function avrInterrupt(cpu: ICPU, addr: number) {
   const sp = cpu.dataView.getUint16(93, true);
   cpu.data[sp] = cpu.pc & 0xff;
   cpu.data[sp - 1] = (cpu.pc >> 8) & 0xff;
-  cpu.dataView.setUint16(93, sp - 2, true);
+  if (cpu.pc22Bits) {
+    cpu.data[sp - 2] = (cpu.pc >> 16) & 0xff;
+  }
+  cpu.dataView.setUint16(93, sp - (cpu.pc22Bits ? 3 : 2), true);
   cpu.data[95] &= 0x7f; // clear global interrupt flag
   cpu.cycles += 2;
   cpu.pc = addr;