--- hatari/src/uae-cpu/hatari-glue.c	2019/04/09 08:54:37	1.1.1.19
+++ hatari/src/uae-cpu/hatari-glue.c	2019/04/09 08:59:34	1.1.1.22
@@ -111,6 +111,7 @@ int Init680x0(void)
 	currprefs.address_space_24 = changed_prefs.address_space_24 = true;
 
 	init_m68k();
+	Cart_PatchCpuTables();
 
 	return true;
 }
@@ -129,6 +130,18 @@ void Exit680x0(void)
 
 
 /**
+ * Execute a 'NOP' opcode (increment PC by 2 bytes and take care
+ * of prefetch at the CPU level depending on the current CPU mode)
+ * This is used to return from Gemdos / Natfeats interception, by ignoring
+ * the intercepted opcode and executing a NOP instead once the work has been done.
+ */
+static void	CpuDoNOP ( void )
+{
+	(*cpufunctbl[0X4E71])(0x4E71);
+}
+
+
+/**
  * Check if the CPU type has been changed
  */
 void check_prefs_changed_cpu(void)
@@ -140,11 +153,24 @@ void check_prefs_changed_cpu(void)
 		currprefs.cpu_compatible = changed_prefs.cpu_compatible;
 		set_special(SPCFLAG_MODE_CHANGE);
 		build_cpufunctbl ();
+		Cart_PatchCpuTables();
 	}
 }
 
 
 /**
+ * Check whether PC is currently in ROM cartridge space - used
+ * to test whether our "illegal" Hatari opcodes should be handled
+ * or whether they are just "normal" illegal opcodes.
+ */
+static bool is_cart_pc(void)
+{
+	Uint32 pc = M68000_GetPC() & 0x00ffffff;
+	return pc >= 0xfa0000 && pc < 0xfc0000;
+}
+
+
+/**
  * This function will be called at system init by the cartridge routine
  * (after gemdos init, before booting floppies).
  * The GEMDOS vector (#$84) is setup and we also initialize the connected
@@ -152,13 +178,13 @@ void check_prefs_changed_cpu(void)
  */
 unsigned long OpCode_SysInit(uae_u32 opcode)
 {
-	/* Add any drives mapped by TOS in the interim */
-	ConnectedDriveMask |= STMemory_ReadLong(0x4c2);
-	/* Initialize the connected drive mask */
-	STMemory_WriteLong(0x4c2, ConnectedDriveMask);
-
-	if (!bInitGemDOS)
+	if (is_cart_pc())
 	{
+		/* Add any drives mapped by TOS in the interim */
+		ConnectedDriveMask |= STMemory_ReadLong(0x4c2);
+		/* Initialize the connected drive mask */
+		STMemory_WriteLong(0x4c2, ConnectedDriveMask);
+
 		/* Init on boot - see cart.c */
 		GemDOS_Boot();
 
@@ -166,10 +192,23 @@ unsigned long OpCode_SysInit(uae_u32 opc
 		 * D0: LineA base, A1: Font base
 		 */
 		VDI_LineA(regs.regs[0], regs.regs[9]);
+
+		CpuDoNOP ();
+	}
+	else if (!bUseTos)
+	{
+		GemDOS_Boot();
+		CpuDoNOP();
+	}
+	else
+	{
+		LOG_TRACE(TRACE_OS_GEMDOS | TRACE_OS_BASE | TRACE_OS_VDI | TRACE_OS_AES,
+			  "SYSINIT opcode invoked outside of cartridge space\n");
+		/* illegal instruction */
+		op_illg(opcode);
+		fill_prefetch_0();
 	}
 
-	m68k_incpc(2);
-	fill_prefetch_0();
 	return 4;
 }
 
@@ -180,10 +219,19 @@ unsigned long OpCode_SysInit(uae_u32 opc
  */
 unsigned long OpCode_GemDos(uae_u32 opcode)
 {
-	GemDOS_OpCode();    /* handler code in gemdos.c */
+	if (is_cart_pc())
+	{
+		GemDOS_OpCode();    /* handler code in gemdos.c */
+		CpuDoNOP();
+	}
+	else
+	{
+		LOG_TRACE(TRACE_OS_GEMDOS, "GEMDOS opcode invoked outside of cartridge space\n");
+		/* illegal instruction */
+		op_illg(opcode);
+		fill_prefetch_0();
+	}
 
-	m68k_incpc(2);
-	fill_prefetch_0();
 	return 4;
 }
 
@@ -193,10 +241,8 @@ unsigned long OpCode_GemDos(uae_u32 opco
  */
 unsigned long OpCode_VDI(uae_u32 opcode)
 {
-	Uint32 pc = M68000_GetPC();
-
 	/* this is valid only after VDI trap, called from cartridge code */
-	if (VDI_OldPC && pc >= 0xfa0000 && pc < 0xfc0000)
+	if (VDI_OldPC && is_cart_pc())
 	{
 		VDI_Complete();
 
@@ -206,6 +252,7 @@ unsigned long OpCode_VDI(uae_u32 opcode)
 	}
 	else
 	{
+		LOG_TRACE(TRACE_OS_VDI, "VDI opcode invoked outside of cartridge space\n");
 		/* illegal instruction */
 		op_illg(opcode);
 	}
@@ -220,12 +267,9 @@ unsigned long OpCode_VDI(uae_u32 opcode)
 unsigned long OpCode_NatFeat_ID(uae_u32 opcode)
 {
 	Uint32 stack = Regs[REG_A7] + SIZE_LONG;	/* skip return address */
-	Uint16 SR = M68000_GetSR();
 
 	if (NatFeat_ID(stack, &(Regs[REG_D0]))) {
-		M68000_SetSR(SR);
-		m68k_incpc(2);
-		fill_prefetch_0();
+		CpuDoNOP ();
 	}
 	return 4;
 }
@@ -241,9 +285,7 @@ unsigned long OpCode_NatFeat_Call(uae_u3
 	
 	super = ((SR & SR_SUPERMODE) == SR_SUPERMODE);
 	if (NatFeat_Call(stack, super, &(Regs[REG_D0]))) {
-		M68000_SetSR(SR);
-		m68k_incpc(2);
-		fill_prefetch_0();
+		CpuDoNOP ();
 	}
 	return 4;
 }