From 57e7782ac4d8d88cfa3e584aeb30ddc4edad8bdd Mon Sep 17 00:00:00 2001
From: Lior Halphon <LIJI32@gmail.com>
Date: Wed, 20 Sep 2017 02:49:45 +0300
Subject: [PATCH] =?UTF-8?q?Interrupt=20servicing=20is=20now=20more=20accur?=
 =?UTF-8?q?ate.=20Fixes=20mooneye-gb=E2=80=99s=20ie=5Fpush=20(all=20models?=
 =?UTF-8?q?)=20and=20Pinball=20Deluxe=20(!!!)=20for=20CGB=20mode?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Core/z80_cpu.c | 33 ++++++++++++++++++++++++---------
 1 file changed, 24 insertions(+), 9 deletions(-)

diff --git a/Core/z80_cpu.c b/Core/z80_cpu.c
index e7b1b5c..f481921 100644
--- a/Core/z80_cpu.c
+++ b/Core/z80_cpu.c
@@ -1361,16 +1361,31 @@ void GB_cpu_run(GB_gameboy_t *gb)
     }
 
     if (effecitve_ime && interrupt_queue) {
-        uint8_t interrupt_bit = 0;
-        while (!(interrupt_queue & 1)) {
-            interrupt_queue >>= 1;
-            interrupt_bit++;
-        }
-        gb->io_registers[GB_IO_IF] &= ~(1 << interrupt_bit);
-        gb->ime = false;
         nop(gb, 0);
-        /* Run pseudo instructions rst 40-60*/
-        rst(gb, 0x87 + interrupt_bit * 8);
+        uint16_t call_addr = gb->pc - 1;
+        GB_advance_cycles(gb, 8);
+        gb->registers[GB_REGISTER_SP] -= 2;
+        GB_write_memory(gb, gb->registers[GB_REGISTER_SP] + 1, (gb->pc) >> 8);
+        GB_advance_cycles(gb, 4);
+        interrupt_queue = (gb->interrupt_enable | gb->future_interrupts) & gb->io_registers[GB_IO_IF] & 0x1F;
+        gb->io_registers[GB_IO_IF] |= gb->future_interrupts;
+        gb->future_interrupts = 0;
+        GB_write_memory(gb, gb->registers[GB_REGISTER_SP], (gb->pc) & 0xFF);
+        GB_advance_cycles(gb, 4);
+        if (interrupt_queue) {
+            uint8_t interrupt_bit = 0;
+            while (!(interrupt_queue & 1)) {
+                interrupt_queue >>= 1;
+                interrupt_bit++;
+            }
+            gb->io_registers[GB_IO_IF] &= ~(1 << interrupt_bit);
+            gb->pc = interrupt_bit * 8 + 0x40;
+        }
+        else {
+            gb->pc = 0;
+        }
+        gb->ime = false;
+        GB_debugger_call_hook(gb, call_addr);
     }
     else if(!gb->halted && !gb->stopped) {
         uint8_t opcode = GB_read_memory(gb, gb->pc++);