svp compiler: some fixes

[picodrive.git] / Pico / carthw / svp / stub_arm.S

diff --git a/Pico/carthw/svp/stub_arm.S b/Pico/carthw/svp/stub_arm.S
index b812b61..531536c 100644 (file)
--- a/Pico/carthw/svp/stub_arm.S
+++ b/Pico/carthw/svp/stub_arm.S
@@ -9,6 +9,7 @@
 .global flush_inval_caches
 .global regfile_load
 .global regfile_store
+.global ssp_hle_800
 
 @ translation cache buffer
 .text
@@ -62,7 +63,7 @@ regfile_load:
     orrne   r8, r8, #0x8
     tst     r6,     #0x20000000
     orrne   r8, r8, #0x4                @ sss0 *           NZ..
-    orr     r6, r8, r9, lsr #12         @ sss0 * .uu. .lll ....
+    orr     r6, r8, r9, lsr #12         @ sss0 * .uu. .lll NZ..
 
     ldr     r8, [r7, #0x440]            @ r0-r2
     ldr     r9, [r7, #0x444]            @ r4-r6
@@ -93,4 +94,34 @@ regfile_store:
     bx      lr
 
 
+#define SSP_OFFS_GR    0x400
+#define SSP_OFFS_EMUST 0x484
+#define SSP_PM0            8
+#define SSP_PC             6
+#define SSP_WAIT_PM0  0x2000
+
+@ ld      A, PM0
+@ andi    2
+@ bra     z=1, gloc_0800
+ssp_hle_800:
+    @ block prologue
+    stmfd   sp!, {r4-r11, lr}
+    bl      regfile_load
+    mov     r11, #0
+    
+    ldr     r0, [r7, #(SSP_OFFS_GR+SSP_PM0*4)]
+    ldr     r1, [r7, #SSP_OFFS_EMUST]
+    tst     r0, #0x20000
+    orreq   r1, r1, #SSP_WAIT_PM0
+    addeq   r11,r11, #1024
+    streq   r1, [r7, #SSP_OFFS_EMUST]
+    movne   r0,     #0x04000000
+    orrne   r0, r0, #0x00040000
+    strne   r0, [r7, #(SSP_OFFS_GR+SSP_PC*4)]
+
+    bl      regfile_store
+    add     r0, r11, #3
+    ldmfd   sp!, {r4-r11, lr}
+    bx      lr
+