diff --git a/Core/display.c b/Core/display.c
index fd502ec..927e30c 100755
--- a/Core/display.c
+++ b/Core/display.c
@@ -292,12 +292,12 @@ static void update_display_state(GB_gameboy_t *gb, uint8_t cycles)
        Todo: Investigate what causes the difference between our findings */
     uint8_t stat_delay = gb->cgb_double_speed? 2 : 4; // (gb->cgb_mode? 0 : 4);
     /* Todo: Is this correct for DMG mode CGB? */
-    uint8_t scx_delay = gb->effective_scx;
+    uint8_t scx_delay = gb->effective_scx & 7;
     if (gb->cgb_double_speed) {
-        scx_delay = (scx_delay + 1) & ~1 & 7;
+        scx_delay = (scx_delay + 1) & ~1;
     }
     else {
-        scx_delay = (scx_delay + (gb->first_scanline ? 2 : 0)) & ~3 & 7;
+        scx_delay = (scx_delay + (gb->first_scanline ? 2 : 0)) & ~3;
     }
     
     /* Todo: These are correct for DMG, DMG-mode CGB, and single speed CGB. Is is correct for double speed CGB? */
diff --git a/Core/z80_cpu.c b/Core/z80_cpu.c
index e7b1b5c..072b1a7 100644
--- a/Core/z80_cpu.c
+++ b/Core/z80_cpu.c
@@ -1361,16 +1361,34 @@ 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);
+        interrupt_queue = gb->interrupt_enable;
+        GB_advance_cycles(gb, 4);
+        GB_write_memory(gb, gb->registers[GB_REGISTER_SP], (gb->pc) & 0xFF);
+        interrupt_queue &= (gb->io_registers[GB_IO_IF] | gb->future_interrupts) & 0x1F;
+        gb->io_registers[GB_IO_IF] |= gb->future_interrupts;
+        gb->future_interrupts = 0;
+
+        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++);