From: notaz <notasas@gmail.com>
Date: Thu, 18 Nov 2021 22:52:31 +0000 (+0200)
Subject: drc/gte: add some stall handling
X-Git-Tag: r23~70
X-Git-Url: https://notaz.gp2x.de/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=81dbbf4cbb16fc6c9a82a5b91e102c8005c5726a;p=pcsx_rearmed.git

drc/gte: add some stall handling
---

diff --git a/frontend/menu.c b/frontend/menu.c
index f3049d92..1d21dacf 100644
--- a/frontend/menu.c
+++ b/frontend/menu.c
@@ -1564,6 +1564,7 @@ static menu_entry e_menu_speed_hacks[] =
 	mee_onoff_h   ("Disable SMC checks",       0, new_dynarec_hacks, NDHACK_NO_SMC_CHECK, h_cfg_nosmc),
 	mee_onoff_h   ("Assume GTE regs unneeded", 0, new_dynarec_hacks, NDHACK_GTE_UNNEEDED, h_cfg_gteunn),
 	mee_onoff_h   ("Disable GTE flags",        0, new_dynarec_hacks, NDHACK_GTE_NO_FLAGS, h_cfg_gteflgs),
+	mee_onoff_h   ("Disable GTE stalls",       0, new_dynarec_hacks, NDHACK_GTE_NO_STALL, h_cfg_gtestll),
 	mee_end,
 };
 
diff --git a/libpcsxcore/gte.c b/libpcsxcore/gte.c
index e05f33d2..d3428225 100644
--- a/libpcsxcore/gte.c
+++ b/libpcsxcore/gte.c
@@ -275,6 +275,32 @@ INLINE u32 DIVIDE(u16 n, u16 d) {
 
 #ifndef FLAGLESS
 
+const char gte_cycletab[64] = {
+	/*   1   2   3   4   5   6   7   8   9   a   b   c   d   e   f */
+	 0, 15,  0,  0,  0,  0,  8,  0,  0,  0,  0,  0,  6,  0,  0,  0,
+	 8,  8,  8, 19, 13,  0, 44,  0,  0,  0,  0, 17, 11,  0, 14,  0,
+	30,  0,  0,  0,  0,  0,  0,  0,  5,  8, 17,  0,  0,  5,  6,  0,
+	23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  5,  5, 39,
+};
+
+// warning: called by the dynarec
+int gteCheckStallRaw(u32 op_cycles, psxRegisters *regs) {
+	u32 left = regs->gteBusyCycle - regs->cycle;
+	int stall = 0;
+
+	if (left <= 44) {
+		//printf("c %2u stall %2u %u\n", op_cycles, left, regs->cycle);
+		regs->cycle = regs->gteBusyCycle;
+		stall = left;
+	}
+	regs->gteBusyCycle = regs->cycle + op_cycles;
+	return stall;
+}
+
+void gteCheckStall(u32 op) {
+	gteCheckStallRaw(gte_cycletab[op], &psxRegs);
+}
+
 static inline u32 MFC2(int reg) {
 	psxCP2Regs *regs = &psxRegs.CP2;
 	switch (reg) {
@@ -403,6 +429,7 @@ void gteLWC2() {
 }
 
 void gteSWC2() {
+	gteCheckStall(0);
 	psxMemWrite32(_oB_, MFC2(_Rt_));
 }
 
diff --git a/libpcsxcore/gte.h b/libpcsxcore/gte.h
index 76462263..9ad73d57 100644
--- a/libpcsxcore/gte.h
+++ b/libpcsxcore/gte.h
@@ -67,6 +67,11 @@ extern "C" {
 
 struct psxCP2Regs;
 
+extern const char gte_cycletab[64];
+
+int  gteCheckStallRaw(u32 op_cycles, psxRegisters *regs);
+void gteCheckStall(u32 op);
+
 void gteMFC2();
 void gteCFC2();
 void gteMTC2();
diff --git a/libpcsxcore/misc.c b/libpcsxcore/misc.c
index cd16c41a..02d1761b 100644
--- a/libpcsxcore/misc.c
+++ b/libpcsxcore/misc.c
@@ -21,6 +21,7 @@
 * Miscellaneous functions, including savestates and CD-ROM loading.
 */
 
+#include <stddef.h>
 #include "misc.h"
 #include "cdrom.h"
 #include "mdec.h"
@@ -602,7 +603,8 @@ int SaveState(const char *file) {
 	SaveFuncs.write(f, psxM, 0x00200000);
 	SaveFuncs.write(f, psxR, 0x00080000);
 	SaveFuncs.write(f, psxH, 0x00010000);
-	SaveFuncs.write(f, (void *)&psxRegs, sizeof(psxRegs));
+	// only partial save of psxRegisters to maintain savestate compat
+	SaveFuncs.write(f, &psxRegs, offsetof(psxRegisters, gteBusyCycle));
 
 	// gpu
 	gpufP = (GPUFreeze_t *)malloc(sizeof(GPUFreeze_t));
@@ -666,7 +668,8 @@ int LoadState(const char *file) {
 	SaveFuncs.read(f, psxM, 0x00200000);
 	SaveFuncs.read(f, psxR, 0x00080000);
 	SaveFuncs.read(f, psxH, 0x00010000);
-	SaveFuncs.read(f, (void *)&psxRegs, sizeof(psxRegs));
+	SaveFuncs.read(f, &psxRegs, offsetof(psxRegisters, gteBusyCycle));
+	psxRegs.gteBusyCycle = psxRegs.cycle;
 
 	if (Config.HLE)
 		psxBiosFreeze(0);
diff --git a/libpcsxcore/new_dynarec/assem_arm.c b/libpcsxcore/new_dynarec/assem_arm.c
index 9fe13a13..27803378 100644
--- a/libpcsxcore/new_dynarec/assem_arm.c
+++ b/libpcsxcore/new_dynarec/assem_arm.c
@@ -19,7 +19,6 @@
  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.          *
  * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
-#include "../gte.h"
 #define FLAGLESS
 #include "../gte.h"
 #undef FLAGLESS
@@ -1674,9 +1673,9 @@ static void do_readstub(int n)
   enum stub_type type=stubs[n].type;
   int i=stubs[n].a;
   int rs=stubs[n].b;
-  struct regstat *i_regs=(struct regstat *)stubs[n].c;
+  const struct regstat *i_regs=(struct regstat *)stubs[n].c;
   u_int reglist=stubs[n].e;
-  signed char *i_regmap=i_regs->regmap;
+  const signed char *i_regmap=i_regs->regmap;
   int rt;
   if(itype[i]==C1LS||itype[i]==C2LS||itype[i]==LOADLR) {
     rt=get_reg(i_regmap,FTEMP);
@@ -1747,7 +1746,8 @@ static void do_readstub(int n)
   emit_jmp(stubs[n].retaddr); // return address
 }
 
-static void inline_readstub(enum stub_type type, int i, u_int addr, signed char regmap[], int target, int adj, u_int reglist)
+static void inline_readstub(enum stub_type type, int i, u_int addr,
+  const signed char regmap[], int target, int adj, u_int reglist)
 {
   int rs=get_reg(regmap,target);
   int rt=get_reg(regmap,target);
@@ -1829,9 +1829,9 @@ static void do_writestub(int n)
   enum stub_type type=stubs[n].type;
   int i=stubs[n].a;
   int rs=stubs[n].b;
-  struct regstat *i_regs=(struct regstat *)stubs[n].c;
+  const struct regstat *i_regs=(struct regstat *)stubs[n].c;
   u_int reglist=stubs[n].e;
-  signed char *i_regmap=i_regs->regmap;
+  const signed char *i_regmap=i_regs->regmap;
   int rt,r;
   if(itype[i]==C1LS||itype[i]==C2LS) {
     rt=get_reg(i_regmap,r=FTEMP);
@@ -1902,7 +1902,8 @@ static void do_writestub(int n)
   emit_jmp(stubs[n].retaddr);
 }
 
-static void inline_writestub(enum stub_type type, int i, u_int addr, signed char regmap[], int target, int adj, u_int reglist)
+static void inline_writestub(enum stub_type type, int i, u_int addr,
+  const signed char regmap[], int target, int adj, u_int reglist)
 {
   int rs=get_reg(regmap,-1);
   int rt=get_reg(regmap,target);
@@ -1976,14 +1977,15 @@ static void do_dirty_stub_ds()
 
 /* Special assem */
 
-static void c2op_prologue(u_int op,u_int reglist)
+static void c2op_prologue(u_int op, int i, const struct regstat *i_regs, u_int reglist)
 {
   save_regs_all(reglist);
+  cop2_call_stall_check(op, i, i_regs, 0);
 #ifdef PCNT
-  emit_movimm(op,0);
+  emit_movimm(op, 0);
   emit_far_call(pcnt_gte_start);
 #endif
-  emit_addimm(FP,(int)&psxRegs.CP2D.r[0]-(int)&dynarec_local,0); // cop2 regs
+  emit_addimm(FP, (u_char *)&psxRegs.CP2D.r[0] - (u_char *)&dynarec_local, 0); // cop2 regs
 }
 
 static void c2op_epilogue(u_int op,u_int reglist)
@@ -2013,22 +2015,19 @@ static void c2op_call_rgb_func(void *func,int lm,int need_ir,int need_flags)
   emit_far_call(need_flags?gteMACtoRGB:gteMACtoRGB_nf);
 }
 
-static void c2op_assemble(int i,struct regstat *i_regs)
+static void c2op_assemble(int i, const struct regstat *i_regs)
 {
-  u_int c2op=source[i]&0x3f;
-  u_int hr,reglist_full=0,reglist;
-  int need_flags,need_ir;
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist_full|=1<<hr;
-  }
-  reglist=reglist_full&CALLER_SAVE_REGS;
+  u_int c2op = source[i] & 0x3f;
+  u_int reglist_full = get_host_reglist(i_regs->regmap);
+  u_int reglist = reglist_full & CALLER_SAVE_REGS;
+  int need_flags, need_ir;
 
   if (gte_handlers[c2op]!=NULL) {
     need_flags=!(gte_unneeded[i+1]>>63); // +1 because of how liveness detection works
     need_ir=(gte_unneeded[i+1]&0xe00)!=0xe00;
     assem_debug("gte op %08x, unneeded %016llx, need_flags %d, need_ir %d\n",
       source[i],gte_unneeded[i+1],need_flags,need_ir);
-    if(new_dynarec_hacks&NDHACK_GTE_NO_FLAGS)
+    if(HACK_ENABLED(NDHACK_GTE_NO_FLAGS))
       need_flags=0;
     int shift = (source[i] >> 19) & 1;
     int lm = (source[i] >> 10) & 1;
@@ -2040,7 +2039,7 @@ static void c2op_assemble(int i,struct regstat *i_regs)
         int cv = (source[i] >> 13) & 3;
         int mx = (source[i] >> 17) & 3;
         reglist=reglist_full&(CALLER_SAVE_REGS|0xf0); // +{r4-r7}
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         /* r4,r5 = VXYZ(v) packed; r6 = &MX11(mx); r7 = &CV1(cv) */
         if(v<3)
           emit_ldrd(v*8,0,4);
@@ -2076,7 +2075,7 @@ static void c2op_assemble(int i,struct regstat *i_regs)
           c2op_call_MACtoIR(lm,need_flags);
 #endif
 #else /* if not HAVE_ARMV5 */
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         emit_movimm(source[i],1); // opcode
         emit_writeword(1,&psxRegs.code);
         emit_far_call(need_flags?gte_handlers[c2op]:gte_handlers_nf[c2op]);
@@ -2084,7 +2083,7 @@ static void c2op_assemble(int i,struct regstat *i_regs)
         break;
       }
       case GTE_OP:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         emit_far_call(shift?gteOP_part_shift:gteOP_part_noshift);
         if(need_flags||need_ir) {
           emit_addimm(FP,(int)&psxRegs.CP2D.r[0]-(int)&dynarec_local,0);
@@ -2092,15 +2091,15 @@ static void c2op_assemble(int i,struct regstat *i_regs)
         }
         break;
       case GTE_DPCS:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         c2op_call_rgb_func(shift?gteDPCS_part_shift:gteDPCS_part_noshift,lm,need_ir,need_flags);
         break;
       case GTE_INTPL:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         c2op_call_rgb_func(shift?gteINTPL_part_shift:gteINTPL_part_noshift,lm,need_ir,need_flags);
         break;
       case GTE_SQR:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         emit_far_call(shift?gteSQR_part_shift:gteSQR_part_noshift);
         if(need_flags||need_ir) {
           emit_addimm(FP,(int)&psxRegs.CP2D.r[0]-(int)&dynarec_local,0);
@@ -2108,20 +2107,20 @@ static void c2op_assemble(int i,struct regstat *i_regs)
         }
         break;
       case GTE_DCPL:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         c2op_call_rgb_func(gteDCPL_part,lm,need_ir,need_flags);
         break;
       case GTE_GPF:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         c2op_call_rgb_func(shift?gteGPF_part_shift:gteGPF_part_noshift,lm,need_ir,need_flags);
         break;
       case GTE_GPL:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
         c2op_call_rgb_func(shift?gteGPL_part_shift:gteGPL_part_noshift,lm,need_ir,need_flags);
         break;
 #endif
       default:
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op,i,i_regs,reglist);
 #ifdef DRC_DBG
         emit_movimm(source[i],1); // opcode
         emit_writeword(1,&psxRegs.code);
diff --git a/libpcsxcore/new_dynarec/assem_arm.h b/libpcsxcore/new_dynarec/assem_arm.h
index 6b3c672c..9b3a1e10 100644
--- a/libpcsxcore/new_dynarec/assem_arm.h
+++ b/libpcsxcore/new_dynarec/assem_arm.h
@@ -6,7 +6,6 @@
 #define HOST_IMM8 1
 #define HAVE_CMOV_IMM 1
 #define HAVE_CONDITIONAL_CALL 1
-#define RAM_SIZE 0x200000
 
 /* ARM calling convention:
    r0-r3, r12: caller-save
diff --git a/libpcsxcore/new_dynarec/assem_arm64.c b/libpcsxcore/new_dynarec/assem_arm64.c
index 303dcf00..e7df2b02 100644
--- a/libpcsxcore/new_dynarec/assem_arm64.c
+++ b/libpcsxcore/new_dynarec/assem_arm64.c
@@ -1448,7 +1448,8 @@ static void do_readstub(int n)
   emit_jmp(stubs[n].retaddr);
 }
 
-static void inline_readstub(enum stub_type type, int i, u_int addr, signed char regmap[], int target, int adj, u_int reglist)
+static void inline_readstub(enum stub_type type, int i, u_int addr,
+  const signed char regmap[], int target, int adj, u_int reglist)
 {
   int rs=get_reg(regmap,target);
   int rt=get_reg(regmap,target);
@@ -1607,7 +1608,8 @@ static void do_writestub(int n)
   emit_jmp(stubs[n].retaddr);
 }
 
-static void inline_writestub(enum stub_type type, int i, u_int addr, signed char regmap[], int target, int adj, u_int reglist)
+static void inline_writestub(enum stub_type type, int i, u_int addr,
+  const signed char regmap[], int target, int adj, u_int reglist)
 {
   int rs = get_reg(regmap,-1);
   int rt = get_reg(regmap,target);
@@ -1777,9 +1779,10 @@ static void get_bounds(void *addr, u_char **start, u_char **end)
 
 /* Special assem */
 
-static void c2op_prologue(u_int op,u_int reglist)
+static void c2op_prologue(u_int op, int i, const struct regstat *i_regs, u_int reglist)
 {
   save_load_regs_all(1, reglist);
+  cop2_call_stall_check(op, i, i_regs, 0);
 #ifdef PCNT
   emit_movimm(op, 0);
   emit_far_call(pcnt_gte_start);
@@ -1797,7 +1800,7 @@ static void c2op_epilogue(u_int op,u_int reglist)
   save_load_regs_all(0, reglist);
 }
 
-static void c2op_assemble(int i,struct regstat *i_regs)
+static void c2op_assemble(int i, const struct regstat *i_regs)
 {
   u_int c2op=source[i]&0x3f;
   u_int hr,reglist_full=0,reglist;
@@ -1819,7 +1822,7 @@ static void c2op_assemble(int i,struct regstat *i_regs)
     switch(c2op) {
       default:
         (void)need_ir;
-        c2op_prologue(c2op,reglist);
+        c2op_prologue(c2op, i, i_regs, reglist);
         emit_movimm(source[i],1); // opcode
         emit_writeword(1,&psxRegs.code);
         emit_far_call(need_flags?gte_handlers[c2op]:gte_handlers_nf[c2op]);
diff --git a/libpcsxcore/new_dynarec/assem_arm64.h b/libpcsxcore/new_dynarec/assem_arm64.h
index 1360bfad..1aeee0b9 100644
--- a/libpcsxcore/new_dynarec/assem_arm64.h
+++ b/libpcsxcore/new_dynarec/assem_arm64.h
@@ -3,7 +3,6 @@
 #define EXCLUDE_REG -1
 
 #define HOST_IMM8 1
-#define RAM_SIZE 0x200000
 
 /* calling convention:
    r0 -r17: caller-save
diff --git a/libpcsxcore/new_dynarec/emu_if.c b/libpcsxcore/new_dynarec/emu_if.c
index f660e7f1..0d6e58d3 100644
--- a/libpcsxcore/new_dynarec/emu_if.c
+++ b/libpcsxcore/new_dynarec/emu_if.c
@@ -218,15 +218,6 @@ const char *gte_regnames[64] = {
 	NULL  , NULL   , NULL   , NULL  , NULL , "GPF"  , "GPL"  , "NCCT", // 38
 };
 
-/* from gte.txt.. not sure if this is any good. */
-const char gte_cycletab[64] = {
-	/*   1   2   3   4   5   6   7   8   9   a   b   c   d   e   f */
-	 0, 15,  0,  0,  0,  0,  8,  0,  0,  0,  0,  0,  6,  0,  0,  0,
-	 8,  8,  8, 19, 13,  0, 44,  0,  0,  0,  0, 17, 11,  0, 14,  0,
-	30,  0,  0,  0,  0,  0,  0,  0,  5,  8, 17,  0,  0,  5,  6,  0,
-	23,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  5,  5, 39,
-};
-
 #define GCBIT(x) \
 	(1ll << (32+x))
 #define GDBIT(x) \
diff --git a/libpcsxcore/new_dynarec/emu_if.h b/libpcsxcore/new_dynarec/emu_if.h
index a6846e2f..36cc275f 100644
--- a/libpcsxcore/new_dynarec/emu_if.h
+++ b/libpcsxcore/new_dynarec/emu_if.h
@@ -52,7 +52,6 @@ extern int reg_cop2d[], reg_cop2c[];
 extern void *gte_handlers[64];
 extern void *gte_handlers_nf[64];
 extern const char *gte_regnames[64];
-extern const char gte_cycletab[64];
 extern const uint64_t gte_reg_reads[64];
 extern const uint64_t gte_reg_writes[64];
 
diff --git a/libpcsxcore/new_dynarec/linkage_arm.S b/libpcsxcore/new_dynarec/linkage_arm.S
index bbc52c3c..f18488ce 100644
--- a/libpcsxcore/new_dynarec/linkage_arm.S
+++ b/libpcsxcore/new_dynarec/linkage_arm.S
@@ -33,6 +33,7 @@
 #define clean_blocks		ESYM(clean_blocks)
 #define gen_interupt		ESYM(gen_interupt)
 #define invalidate_addr		ESYM(invalidate_addr)
+#define gteCheckStallRaw	ESYM(gteCheckStallRaw)
 #endif
 
 	.bss
@@ -820,4 +821,16 @@ FUNCTION(rcnt2_read_count_m1):
 	lsr	r0, #16			@ /= 8
 	bx	lr
 
+FUNCTION(call_gteStall):
+	/* r0 = op_cycles, r1 = cycles */
+	ldr	r2, [fp, #LO_last_count]
+	str	lr, [fp, #LO_saved_lr]
+	add	r1, r1, r2
+	str	r1, [fp, #LO_cycle]
+	add	r1, fp, #LO_psxRegs
+	bl	gteCheckStallRaw
+	ldr	lr, [fp, #LO_saved_lr]
+	add	r10, r10, r0
+	bx	lr
+
 @ vim:filetype=armasm
diff --git a/libpcsxcore/new_dynarec/linkage_arm64.S b/libpcsxcore/new_dynarec/linkage_arm64.S
index 444545ca..249fecbc 100644
--- a/libpcsxcore/new_dynarec/linkage_arm64.S
+++ b/libpcsxcore/new_dynarec/linkage_arm64.S
@@ -160,13 +160,13 @@ FUNCTION(cc_interrupt):
 FUNCTION(fp_exception):
 	mov	w2, #0x10000000
 0:
-	ldr	w1, [fp, #LO_reg_cop0+48] /* Status */
+	ldr	w1, [rFP, #LO_reg_cop0+48] /* Status */
 	mov	w3, #0x80000000
-	str	w0, [fp, #LO_reg_cop0+56] /* EPC */
+	str	w0, [rFP, #LO_reg_cop0+56] /* EPC */
 	orr	w1, w1, #2
 	add	w2, w2, #0x2c
-	str	w1, [fp, #LO_reg_cop0+48] /* Status */
-	str	w2, [fp, #LO_reg_cop0+52] /* Cause */
+	str	w1, [rFP, #LO_reg_cop0+48] /* Status */
+	str	w2, [rFP, #LO_reg_cop0+52] /* Cause */
 	add	w0, w3, #0x80
 	bl	get_addr_ht
 	br	x0
@@ -179,13 +179,13 @@ FUNCTION(fp_exception_ds):
 
 	.align	2
 FUNCTION(jump_syscall):
-	ldr	w1, [fp, #LO_reg_cop0+48] /* Status */
+	ldr	w1, [rFP, #LO_reg_cop0+48] /* Status */
 	mov	w3, #0x80000000
-	str	w0, [fp, #LO_reg_cop0+56] /* EPC */
+	str	w0, [rFP, #LO_reg_cop0+56] /* EPC */
 	orr	w1, w1, #2
 	mov	w2, #0x20
-	str	w1, [fp, #LO_reg_cop0+48] /* Status */
-	str	w2, [fp, #LO_reg_cop0+52] /* Cause */
+	str	w1, [rFP, #LO_reg_cop0+48] /* Status */
+	str	w2, [rFP, #LO_reg_cop0+52] /* Cause */
 	add	w0, w3, #0x80
 	bl	get_addr_ht
 	br	x0
@@ -195,11 +195,11 @@ FUNCTION(jump_syscall):
 	/* note: psxException might do recursive recompiler call from it's HLE code,
 	 * so be ready for this */
 FUNCTION(jump_to_new_pc):
-	ldr	w1, [fp, #LO_next_interupt]
-	ldr	rCC, [fp, #LO_cycle]
-	ldr	w0, [fp, #LO_pcaddr]
+	ldr	w1, [rFP, #LO_next_interupt]
+	ldr	rCC, [rFP, #LO_cycle]
+	ldr	w0, [rFP, #LO_pcaddr]
 	sub	rCC, rCC, w1
-	str	w1, [fp, #LO_last_count]
+	str	w1, [rFP, #LO_last_count]
 	bl	get_addr_ht
 	br	x0
 	.size	jump_to_new_pc, .-jump_to_new_pc
@@ -331,7 +331,7 @@ handler_write_end:
 
 FUNCTION(jump_handle_swl):
 	/* w0 = address, w1 = data, w2 = cycles */
-	ldr	x3, [fp, #LO_mem_wtab]
+	ldr	x3, [rFP, #LO_mem_wtab]
 	mov	w4, w0, lsr #12
 	ldr	x3, [x3, w4, uxtw #3]
 	adds	x3, x3, x3
@@ -366,7 +366,7 @@ FUNCTION(jump_handle_swl):
 
 FUNCTION(jump_handle_swr):
 	/* w0 = address, w1 = data, w2 = cycles */
-	ldr	x3, [fp, #LO_mem_wtab]
+	ldr	x3, [rFP, #LO_mem_wtab]
 	mov	w4, w0, lsr #12
 	ldr	x3, [x3, w4, uxtw #3]
 	adds	x3, x3, x3
@@ -396,3 +396,15 @@ FUNCTION(jump_handle_swr):
 	bl	abort
 	ret
 
+FUNCTION(call_gteStall):
+	/* w0 = op_cycles, w1 = cycles */
+	ldr	w2, [rFP, #LO_last_count]
+	str	lr, [rFP, #LO_saved_lr]
+	add	w1, w1, w2
+	str	w1, [rFP, #LO_cycle]
+	add	x1, rFP, #LO_psxRegs
+	bl	gteCheckStallRaw
+	ldr	lr, [rFP, #LO_saved_lr]
+	add	rCC, rCC, w0
+	ret
+
diff --git a/libpcsxcore/new_dynarec/linkage_offsets.h b/libpcsxcore/new_dynarec/linkage_offsets.h
index 82d27bd4..4c75e6c0 100644
--- a/libpcsxcore/new_dynarec/linkage_offsets.h
+++ b/libpcsxcore/new_dynarec/linkage_offsets.h
@@ -22,7 +22,9 @@
 #define LO_cycle		(LO_code + 4)
 #define LO_interrupt		(LO_cycle + 4)
 #define LO_intCycle		(LO_interrupt + 4)
-#define LO_psxRegs_end		(LO_intCycle + 256)
+#define LO_gteBusyCycle		(LO_intCycle + 256)
+#define LO_psxRegs_reserved	(LO_gteBusyCycle + 4)
+#define LO_psxRegs_end		(LO_psxRegs_reserved + 4*3)
 #define LO_rcnts		(LO_psxRegs_end)
 #define LO_rcnts_end		(LO_rcnts + 7*4*4)
 #define LO_inv_code_start	(LO_rcnts_end)
@@ -33,8 +35,9 @@
 #define LO_zeromem_ptr		(LO_psxH_ptr + PTRSZ)
 #define LO_invc_ptr		(LO_zeromem_ptr + PTRSZ)
 #define LO_scratch_buf_ptr	(LO_invc_ptr + PTRSZ)
-#define LO_align1		(LO_scratch_buf_ptr + PTRSZ)
-#define LO_mini_ht		(LO_align1 + PTRSZ*2)
+#define LO_saved_lr		(LO_scratch_buf_ptr + PTRSZ)
+#define LO_align1		(LO_saved_lr + PTRSZ)
+#define LO_mini_ht		(LO_align1 + PTRSZ)
 #define LO_restore_candidate	(LO_mini_ht + PTRSZ*32*2)
 #define LO_dynarec_local_size	(LO_restore_candidate + 512)
 
diff --git a/libpcsxcore/new_dynarec/new_dynarec.c b/libpcsxcore/new_dynarec/new_dynarec.c
index 964c07ba..1a91c3a1 100644
--- a/libpcsxcore/new_dynarec/new_dynarec.c
+++ b/libpcsxcore/new_dynarec/new_dynarec.c
@@ -37,7 +37,8 @@ static int sceBlock;
 #include "new_dynarec_config.h"
 #include "../psxhle.h"
 #include "../psxinterpreter.h"
-#include "emu_if.h" //emulator interface
+#include "../gte.h"
+#include "emu_if.h" // emulator interface
 
 #define noinline __attribute__((noinline,noclone))
 #ifndef ARRAY_SIZE
@@ -63,6 +64,7 @@ static int sceBlock;
 #include "assem_arm64.h"
 #endif
 
+#define RAM_SIZE 0x200000
 #define MAXBLOCK 4096
 #define MAX_OUTPUT_BLOCK_SIZE 262144
 
@@ -308,6 +310,7 @@ void cc_interrupt();
 void fp_exception();
 void fp_exception_ds();
 void jump_to_new_pc();
+void call_gteStall();
 void new_dyna_leave();
 
 // Needed by assembler
@@ -318,17 +321,19 @@ static void load_all_regs(signed char i_regmap[]);
 static void load_needed_regs(signed char i_regmap[],signed char next_regmap[]);
 static void load_regs_entry(int t);
 static void load_all_consts(signed char regmap[],u_int dirty,int i);
+static u_int get_host_reglist(const signed char *regmap);
 
 static int verify_dirty(const u_int *ptr);
 static int get_final_value(int hr, int i, int *value);
 static void add_stub(enum stub_type type, void *addr, void *retaddr,
   u_int a, uintptr_t b, uintptr_t c, u_int d, u_int e);
 static void add_stub_r(enum stub_type type, void *addr, void *retaddr,
-  int i, int addr_reg, struct regstat *i_regs, int ccadj, u_int reglist);
+  int i, int addr_reg, const struct regstat *i_regs, int ccadj, u_int reglist);
 static void add_to_linker(void *addr, u_int target, int ext);
 static void *emit_fastpath_cmp_jump(int i,int addr,int *addr_reg_override);
 static void *get_direct_memhandler(void *table, u_int addr,
   enum stub_type type, uintptr_t *addr_host);
+static void cop2_call_stall_check(u_int op, int i, const struct regstat *i_regs, u_int reglist);
 static void pass_args(int a0, int a1);
 static void emit_far_jump(const void *f);
 static void emit_far_call(const void *f);
@@ -917,6 +922,7 @@ static const struct {
   FUNCNAME(jump_handler_write32),
   FUNCNAME(invalidate_addr),
   FUNCNAME(jump_to_new_pc),
+  FUNCNAME(call_gteStall),
   FUNCNAME(new_dyna_leave),
   FUNCNAME(pcsx_mtc0),
   FUNCNAME(pcsx_mtc0_ds),
@@ -1918,19 +1924,19 @@ void cop0_alloc(struct regstat *current,int i)
   minimum_free_regs[i]=HOST_REGS;
 }
 
-static void cop12_alloc(struct regstat *current,int i)
+static void cop2_alloc(struct regstat *current,int i)
 {
-  alloc_reg(current,i,CSREG); // Load status
-  if(opcode2[i]<3) // MFC1/CFC1
+  if (opcode2[i] < 3) // MFC2/CFC2
   {
+    alloc_cc(current,i); // for stalls
+    dirty_reg(current,CCREG);
     if(rt1[i]){
       clear_const(current,rt1[i]);
       alloc_reg(current,i,rt1[i]);
       dirty_reg(current,rt1[i]);
     }
-    alloc_reg_temp(current,i,-1);
   }
-  else if(opcode2[i]>3) // MTC1/CTC1
+  else if (opcode2[i] > 3) // MTC2/CTC2
   {
     if(rs1[i]){
       clear_const(current,rs1[i]);
@@ -1940,13 +1946,15 @@ static void cop12_alloc(struct regstat *current,int i)
       current->u&=~1LL;
       alloc_reg(current,i,0);
     }
-    alloc_reg_temp(current,i,-1);
   }
+  alloc_reg_temp(current,i,-1);
   minimum_free_regs[i]=1;
 }
 
 void c2op_alloc(struct regstat *current,int i)
 {
+  alloc_cc(current,i); // for stalls
+  dirty_reg(current,CCREG);
   alloc_reg_temp(current,i,-1);
 }
 
@@ -2003,8 +2011,9 @@ void delayslot_alloc(struct regstat *current,int i)
       cop0_alloc(current,i);
       break;
     case COP1:
+      break;
     case COP2:
-      cop12_alloc(current,i);
+      cop2_alloc(current,i);
       break;
     case C1LS:
       c1ls_alloc(current,i);
@@ -2070,7 +2079,7 @@ static void add_stub(enum stub_type type, void *addr, void *retaddr,
 }
 
 static void add_stub_r(enum stub_type type, void *addr, void *retaddr,
-  int i, int addr_reg, struct regstat *i_regs, int ccadj, u_int reglist)
+  int i, int addr_reg, const struct regstat *i_regs, int ccadj, u_int reglist)
 {
   add_stub(type, addr, retaddr, i, addr_reg, (uintptr_t)i_regs, ccadj, reglist);
 }
@@ -2647,20 +2656,36 @@ static void *get_direct_memhandler(void *table, u_int addr,
   }
 }
 
-static void load_assemble(int i,struct regstat *i_regs)
+static u_int get_host_reglist(const signed char *regmap)
+{
+  u_int reglist = 0, hr;
+  for (hr = 0; hr < HOST_REGS; hr++) {
+    if (hr != EXCLUDE_REG && regmap[hr] >= 0)
+      reglist |= 1 << hr;
+  }
+  return reglist;
+}
+
+static u_int reglist_exclude(u_int reglist, int r1, int r2)
+{
+  if (r1 >= 0)
+    reglist &= ~(1u << r1);
+  if (r2 >= 0)
+    reglist &= ~(1u << r2);
+  return reglist;
+}
+
+static void load_assemble(int i, const struct regstat *i_regs)
 {
   int s,tl,addr;
   int offset;
   void *jaddr=0;
   int memtarget=0,c=0;
   int fastio_reg_override=-1;
-  u_int hr,reglist=0;
+  u_int reglist=get_host_reglist(i_regs->regmap);
   tl=get_reg(i_regs->regmap,rt1[i]);
   s=get_reg(i_regs->regmap,rs1[i]);
   offset=imm[i];
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist|=1<<hr;
-  }
   if(i_regs->regmap[HOST_CCREG]==CCREG) reglist&=~(1<<HOST_CCREG);
   if(s>=0) {
     c=(i_regs->wasconst>>s)&1;
@@ -2787,14 +2812,14 @@ static void load_assemble(int i,struct regstat *i_regs)
 }
 
 #ifndef loadlr_assemble
-static void loadlr_assemble(int i,struct regstat *i_regs)
+static void loadlr_assemble(int i, const struct regstat *i_regs)
 {
   int s,tl,temp,temp2,addr;
   int offset;
   void *jaddr=0;
   int memtarget=0,c=0;
   int fastio_reg_override=-1;
-  u_int hr,reglist=0;
+  u_int reglist=get_host_reglist(i_regs->regmap);
   tl=get_reg(i_regs->regmap,rt1[i]);
   s=get_reg(i_regs->regmap,rs1[i]);
   temp=get_reg(i_regs->regmap,-1);
@@ -2802,9 +2827,6 @@ static void loadlr_assemble(int i,struct regstat *i_regs)
   addr=get_reg(i_regs->regmap,AGEN1+(i&1));
   assert(addr<0);
   offset=imm[i];
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist|=1<<hr;
-  }
   reglist|=1<<temp;
   if(offset||s<0||c) addr=temp2;
   else addr=s;
@@ -2870,7 +2892,7 @@ static void loadlr_assemble(int i,struct regstat *i_regs)
 }
 #endif
 
-void store_assemble(int i,struct regstat *i_regs)
+void store_assemble(int i, const struct regstat *i_regs)
 {
   int s,tl;
   int addr,temp;
@@ -2880,7 +2902,7 @@ void store_assemble(int i,struct regstat *i_regs)
   int memtarget=0,c=0;
   int agr=AGEN1+(i&1);
   int fastio_reg_override=-1;
-  u_int hr,reglist=0;
+  u_int reglist=get_host_reglist(i_regs->regmap);
   tl=get_reg(i_regs->regmap,rs2[i]);
   s=get_reg(i_regs->regmap,rs1[i]);
   temp=get_reg(i_regs->regmap,agr);
@@ -2894,9 +2916,6 @@ void store_assemble(int i,struct regstat *i_regs)
   }
   assert(tl>=0);
   assert(temp>=0);
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist|=1<<hr;
-  }
   if(i_regs->regmap[HOST_CCREG]==CCREG) reglist&=~(1<<HOST_CCREG);
   if(offset||s<0||c) addr=temp;
   else addr=s;
@@ -2994,7 +3013,7 @@ void store_assemble(int i,struct regstat *i_regs)
   }
 }
 
-static void storelr_assemble(int i,struct regstat *i_regs)
+static void storelr_assemble(int i, const struct regstat *i_regs)
 {
   int s,tl;
   int temp;
@@ -3004,7 +3023,7 @@ static void storelr_assemble(int i,struct regstat *i_regs)
   void *done0, *done1, *done2;
   int memtarget=0,c=0;
   int agr=AGEN1+(i&1);
-  u_int hr,reglist=0;
+  u_int reglist=get_host_reglist(i_regs->regmap);
   tl=get_reg(i_regs->regmap,rs2[i]);
   s=get_reg(i_regs->regmap,rs1[i]);
   temp=get_reg(i_regs->regmap,agr);
@@ -3017,9 +3036,6 @@ static void storelr_assemble(int i,struct regstat *i_regs)
     }
   }
   assert(tl>=0);
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist|=1<<hr;
-  }
   assert(temp>=0);
   if(!c) {
     emit_cmpimm(s<0||offset?temp:s,RAM_SIZE);
@@ -3263,6 +3279,25 @@ static void do_cop1stub(int n)
   emit_far_jump(ds?fp_exception_ds:fp_exception);
 }
 
+// assumes callee-save regs are already saved
+static void cop2_call_stall_check(u_int op, int i, const struct regstat *i_regs, u_int reglist)
+{
+  if (HACK_ENABLED(NDHACK_GTE_NO_STALL))
+    return;
+  //assert(get_reg(i_regs->regmap, CCREG) == HOST_CCREG);
+  if (get_reg(i_regs->regmap, CCREG) != HOST_CCREG) {
+    // happens occasionally... cc evicted? Don't bother then
+    //printf("no cc %08x\n", start + i*4);
+    return;
+  }
+  assem_debug("cop2_call_stall_check\n");
+  save_regs(reglist);
+  emit_movimm(gte_cycletab[op], 0);
+  emit_addimm(HOST_CCREG, CLOCK_ADJUST(ccadj[i]), 1);
+  emit_far_call(call_gteStall);
+  restore_regs(reglist);
+}
+
 static void cop2_get_dreg(u_int copr,signed char tl,signed char temp)
 {
   switch (copr) {
@@ -3346,7 +3381,7 @@ static void cop2_put_dreg(u_int copr,signed char sl,signed char temp)
   }
 }
 
-static void c2ls_assemble(int i,struct regstat *i_regs)
+static void c2ls_assemble(int i, const struct regstat *i_regs)
 {
   int s,tl;
   int ar;
@@ -3356,7 +3391,7 @@ static void c2ls_assemble(int i,struct regstat *i_regs)
   enum stub_type type;
   int agr=AGEN1+(i&1);
   int fastio_reg_override=-1;
-  u_int hr,reglist=0;
+  u_int reglist=get_host_reglist(i_regs->regmap);
   u_int copr=(source[i]>>16)&0x1f;
   s=get_reg(i_regs->regmap,rs1[i]);
   tl=get_reg(i_regs->regmap,FTEMP);
@@ -3364,9 +3399,6 @@ static void c2ls_assemble(int i,struct regstat *i_regs)
   assert(rs1[i]>0);
   assert(tl>=0);
 
-  for(hr=0;hr<HOST_REGS;hr++) {
-    if(i_regs->regmap[hr]>=0) reglist|=1<<hr;
-  }
   if(i_regs->regmap[HOST_CCREG]==CCREG)
     reglist&=~(1<<HOST_CCREG);
 
@@ -3384,6 +3416,7 @@ static void c2ls_assemble(int i,struct regstat *i_regs)
   assert(ar>=0);
 
   if (opcode[i]==0x3a) { // SWC2
+    cop2_call_stall_check(0, i, i_regs, reglist_exclude(reglist, tl, -1));
     cop2_get_dreg(copr,tl,-1);
     type=STOREW_STUB;
   }
@@ -3445,10 +3478,18 @@ static void c2ls_assemble(int i,struct regstat *i_regs)
   }
 }
 
-static void cop2_assemble(int i,struct regstat *i_regs)
+static void cop2_assemble(int i, const struct regstat *i_regs)
 {
-  u_int copr=(source[i]>>11)&0x1f;
-  signed char temp=get_reg(i_regs->regmap,-1);
+  u_int copr = (source[i]>>11) & 0x1f;
+  signed char temp = get_reg(i_regs->regmap, -1);
+
+  if (opcode2[i] == 0 || opcode2[i] == 2) { // MFC2/CFC2
+    if (!HACK_ENABLED(NDHACK_GTE_NO_STALL)) {
+      signed char tl = get_reg(i_regs->regmap, rt1[i]);
+      u_int reglist = reglist_exclude(get_host_reglist(i_regs->regmap), tl, temp);
+      cop2_call_stall_check(0, i, i_regs, reglist);
+    }
+  }
   if (opcode2[i]==0) { // MFC2
     signed char tl=get_reg(i_regs->regmap,rt1[i]);
     if(tl>=0&&rt1[i]!=0)
@@ -4341,11 +4382,9 @@ static void drc_dbg_emit_do_cmp(int i)
 {
   extern void do_insn_cmp();
   //extern int cycle;
-  u_int hr,reglist=0;
+  u_int hr, reglist = get_host_reglist(regs[i].regmap);
 
   assem_debug("//do_insn_cmp %08x\n", start+i*4);
-  for (hr = 0; hr < HOST_REGS; hr++)
-    if(regs[i].regmap[hr]>=0) reglist|=1<<hr;
   save_regs(reglist);
   // write out changed consts to match the interpreter
   if (i > 0 && !bt[i]) {
@@ -7651,8 +7690,9 @@ int new_recompile_block(u_int addr)
           cop0_alloc(&current,i);
           break;
         case COP1:
+          break;
         case COP2:
-          cop12_alloc(&current,i);
+          cop2_alloc(&current,i);
           break;
         case C1LS:
           c1ls_alloc(&current,i);
@@ -7945,12 +7985,10 @@ int new_recompile_block(u_int addr)
 #if !defined(DRC_DBG)
     else if(itype[i]==C2OP&&gte_cycletab[source[i]&0x3f]>2)
     {
-      // GTE runs in parallel until accessed, divide by 2 for a rough guess
-      cc+=gte_cycletab[source[i]&0x3f]/2;
-    }
-    else if(/*itype[i]==LOAD||itype[i]==STORE||*/itype[i]==C1LS) // load,store causes weird timing issues
-    {
-      cc+=2; // 2 cycle penalty (after CLOCK_DIVIDER)
+      // this should really be removed since the real stalls have been implemented,
+      // but doing so causes sizeable perf regression against the older version
+      u_int gtec = gte_cycletab[source[i] & 0x3f];
+      cc += HACK_ENABLED(NDHACK_GTE_NO_STALL) ? gtec/2 : 2;
     }
     else if(i>1&&itype[i]==STORE&&itype[i-1]==STORE&&itype[i-2]==STORE&&!bt[i])
     {
@@ -7958,7 +7996,8 @@ int new_recompile_block(u_int addr)
     }
     else if(itype[i]==C2LS)
     {
-      cc+=4;
+      // same as with C2OP
+      cc += HACK_ENABLED(NDHACK_GTE_NO_STALL) ? 4 : 2;
     }
 #endif
     else
diff --git a/libpcsxcore/new_dynarec/new_dynarec.h b/libpcsxcore/new_dynarec/new_dynarec.h
index bfb48838..bff1c164 100644
--- a/libpcsxcore/new_dynarec/new_dynarec.h
+++ b/libpcsxcore/new_dynarec/new_dynarec.h
@@ -11,6 +11,7 @@ extern int cycle_multiplier_override;
 #define NDHACK_GTE_UNNEEDED	(1<<1)
 #define NDHACK_GTE_NO_FLAGS	(1<<2)
 #define NDHACK_OVERRIDE_CYCLE_M	(1<<3)
+#define NDHACK_GTE_NO_STALL	(1<<4)
 extern int new_dynarec_hacks;
 extern int new_dynarec_hacks_pergame;
 
diff --git a/libpcsxcore/psxinterpreter.c b/libpcsxcore/psxinterpreter.c
index 61c60edb..b171b0a6 100644
--- a/libpcsxcore/psxinterpreter.c
+++ b/libpcsxcore/psxinterpreter.c
@@ -934,7 +934,10 @@ void psxCOP0() {
 }
 
 void psxCOP2() {
-	psxCP2[_Funct_]((struct psxCP2Regs *)&psxRegs.CP2D);
+	u32 f = _Funct_;
+	if (f != 0 || _Rs_ < 4) // not MTC2/CTC2
+		gteCheckStall(f);
+	psxCP2[f]((struct psxCP2Regs *)&psxRegs.CP2D);
 }
 
 void psxBASIC(struct psxCP2Regs *regs) {
diff --git a/libpcsxcore/r3000a.h b/libpcsxcore/r3000a.h
index 4b1ec9e0..54359159 100644
--- a/libpcsxcore/r3000a.h
+++ b/libpcsxcore/r3000a.h
@@ -193,6 +193,10 @@ typedef struct {
 	u32 cycle;
 	u32 interrupt;
 	struct { u32 sCycle, cycle; } intCycle[32];
+	u32 gteBusyCycle;
+	// warning: changing anything in psxRegisters requires update of all
+	// asm in libpcsxcore/new_dynarec/, but this member can be replaced
+	u32 reserved[3];
 } psxRegisters;
 
 extern psxRegisters psxRegs;