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

@ vim:filetype=armasm

.if 0
#include "compiler.h"
.endif

.global tcache

.global flush_inval_caches
.global ssp_drc_entry
.global ssp_drc_next
.global ssp_drc_next_patch
.global ssp_drc_end
.global ssp_hle_800
.global ssp_hle_902

@ translation cache buffer
.text
.align 12 @ 4096
.size tcache, TCACHE_SIZE
tcache:
 .space TCACHE_SIZE


.text
.align 2


flush_inval_caches:
    mov     r2, #0x0  @ must be 0
    swi     0x9f0002
    bx      lr


@       SSP_GR0, SSP_X,     SSP_Y,   SSP_A,
@       SSP_ST,  SSP_STACK, SSP_PC,  SSP_P,
@       SSP_PM0, SSP_PM1,   SSP_PM2, SSP_XST,
@       SSP_PM4, SSP_gr13,  SSP_PMC, SSP_AL

@ register map:
@ r4:  XXYY
@ r5:  A
@ r6:  STACK and emu flags: sss0 * .uu. .lll NZCV (NZCV is PSR bits from ARM)
@ r7:  SSP context
@ r8:  r0-r2 (.210)
@ r9:  r4-r6 (.654)
@ r10: P
@ r11: cycles


#define SSP_OFFS_GR         0x400
#define SSP_PC                  6
#define SSP_P                   7
#define SSP_PM0                 8
#define SSP_PMC                14
#define SSP_OFFS_PM_WRITE   0x46c // pmac_write[]
#define SSP_OFFS_EMUSTAT    0x484 // emu_status
#define SSP_OFFS_IRAM_ROM   0x48c // ptr_iram_rom
#define SSP_OFFS_IRAM_DIRTY 0x494
#define SSP_OFFS_IRAM_CTX   0x498 // iram_context
#define SSP_OFFS_BLTAB      0x49c // block_table
#define SSP_OFFS_BLTAB_IRAM 0x4a0
#define SSP_OFFS_TMP0       0x4a4 // for entry PC
#define SSP_OFFS_TMP1       0x4a8
#define SSP_OFFS_TMP2       0x4ac
#define SSP_WAIT_PM0       0x2000


.macro ssp_drc_do_next patch_jump=0
.if \patch_jump
    str     lr, [r7, #SSP_OFFS_TMP2]		@ jump instr. (actually call) address + 4
.endif
    mov     r0, r0, lsl #16
    mov     r0, r0, lsr #16
    str     r0, [r7, #SSP_OFFS_TMP0]
    cmp     r0, #0x400
    blt     0f @ ssp_de_iram

    ldr     r2, [r7, #SSP_OFFS_BLTAB]
    ldr     r2, [r2, r0, lsl #2]
    tst     r2, r2
.if \patch_jump
    bne     ssp_drc_do_patch
.else
    bxne    r2
.endif
    bl      ssp_translate_block
    mov     r2, r0
    ldr     r0, [r7, #SSP_OFFS_TMP0]		@ entry PC
    ldr     r1, [r7, #SSP_OFFS_BLTAB]
    str     r2, [r1, r0, lsl #2]
.if \patch_jump
    b       ssp_drc_do_patch
.else
    bx      r2
.endif

0: @ ssp_de_iram:
    ldr     r1, [r7, #SSP_OFFS_IRAM_DIRTY]
    tst     r1, r1
    ldreq   r1, [r7, #SSP_OFFS_IRAM_CTX]
    beq     1f @ ssp_de_iram_ctx

    bl      ssp_get_iram_context
    mov     r1, #0
    str     r1, [r7, #SSP_OFFS_IRAM_DIRTY]
    mov     r1, r0
    str     r1, [r7, #SSP_OFFS_IRAM_CTX]
    ldr     r0, [r7, #SSP_OFFS_TMP0]		@ entry PC
    
1: @ ssp_de_iram_ctx:
    ldr     r2, [r7, #SSP_OFFS_BLTAB_IRAM]
    add     r2, r2, r1, lsl #12			@ block_tab_iram + iram_context * 0x800/2*4
    add     r1, r2, r0, lsl #2
    ldr     r2, [r1]
    tst     r2, r2
.if \patch_jump
    bne     ssp_drc_do_patch
.else
    bxne    r2
.endif
    str     r1, [r7, #SSP_OFFS_TMP1]
    bl      ssp_translate_block
    mov     r2, r0
    ldr     r0, [r7, #SSP_OFFS_TMP0]		@ entry PC
    ldr     r1, [r7, #SSP_OFFS_TMP1]		@ &block_table_iram[iram_context][rPC]
    str     r2, [r1]
.if \patch_jump
    b       ssp_drc_do_patch
.else
    bx      r2
.endif
.endm @ ssp_drc_do_next


ssp_drc_entry:
    stmfd   sp!, {r4-r11, lr}
    mov     r11, r0
ssp_regfile_load:
    ldr     r7, =ssp
    ldr     r7, [r7]
    add     r2, r7, #0x400
    add     r2, r2, #4
    ldmia   r2, {r3,r4,r5,r6,r8}
    mov     r3, r3, lsr #16
    mov     r3, r3, lsl #16
    orr     r4, r3, r4, lsr #16         @ XXYY

    and     r8, r8, #0x0f0000
    mov     r8, r8, lsl #13             @ sss0 *
    and     r9, r6, #0x670000
    tst     r6,     #0x80000000
    orrne   r8, r8, #0x8
    tst     r6,     #0x20000000
    orrne   r8, r8, #0x4                @ sss0 *           NZ..
    orr     r6, r8, r9, lsr #12         @ sss0 * .uu. .lll NZ..

    ldr     r8, [r7, #0x440]            @ r0-r2
    ldr     r9, [r7, #0x444]            @ r4-r6
    ldr     r10,[r7, #(0x400+SSP_P*4)]  @ P

    ldr     r0, [r7, #(SSP_OFFS_GR+SSP_PC*4)]
    mov     r0, r0, lsr #16


ssp_drc_next:
    ssp_drc_do_next 0


ssp_drc_next_patch:
    ssp_drc_do_next 1

ssp_drc_do_patch:
    ldr     r1, [r7, #SSP_OFFS_TMP2]	@ jump instr. (actually call) address + 4
    subs    r12,r2, r1
    moveq   r3,     #0xe1000000
    orreq   r3, r3, #0x00a00000		@ nop
    streq   r3, [r1, #-4]
    beq     ssp_drc_dp_end

    cmp     r12,#4
    ldreq   r3, [r1]
    addeq   r3, r3, #1
    streq   r3, [r1, #-4]               @ move the other cond up
    moveq   r3,     #0xe1000000
    orreq   r3, r3, #0x00a00000
    streq   r3, [r1]                    @ fill it's place with nop
    beq     ssp_drc_dp_end

    ldr     r3, [r1, #-4]
    sub     r12,r12,#4
    mov     r3, r3, lsr #24
    bic     r3, r3, #1			@ L bit
    orr     r3, r3, r12,lsl #6
    mov     r3, r3, ror #8              @ patched branch instruction
    str     r3, [r1, #-4]

ssp_drc_dp_end:
    str     r2, [r7, #SSP_OFFS_TMP1]
    sub     r0, r1, #4
    add     r1, r1, #4
    bl      flush_inval_caches
    ldr     r2, [r7, #SSP_OFFS_TMP1]
    ldr     r0, [r7, #SSP_OFFS_TMP0]
    bx      r2


ssp_drc_end:
    mov     r0, r0, lsl #16
    str     r0, [r7, #(SSP_OFFS_GR+SSP_PC*4)]

ssp_regfile_store:
    str     r10,[r7, #(0x400+SSP_P*4)]  @ P
    str     r8, [r7, #0x440]            @ r0-r2
    str     r9, [r7, #0x444]            @ r4-r6

    mov     r9, r6, lsr #13
    and     r9, r9, #(7<<16)            @ STACK
    mov     r3, r6, lsl #28
    msr     cpsr_flg, r3                @ to to ARM PSR
    and     r6, r6, #0x670
    mov     r6, r6, lsl #12
    orrmi   r6, r6, #0x80000000         @ N
    orreq   r6, r6, #0x20000000         @ Z

    mov     r3, r4, lsl #16             @ Y
    mov     r2, r4, lsr #16
    mov     r2, r2, lsl #16             @ X
    add     r8, r7, #0x400
    add     r8, r8, #4
    stmia   r8, {r2,r3,r5,r6,r9}

    mov     r0, r11
    ldmfd   sp!, {r4-r11, lr}
    bx      lr


@ ld      A, PM0
@ andi    2
@ bra     z=1, gloc_0800
ssp_hle_800:
    ldr     r0, [r7, #(SSP_OFFS_GR+SSP_PM0*4)]
    ldr     r1, [r7, #SSP_OFFS_EMUSTAT]
    tst     r0, #0x20000
    orreq   r1, r1,  #SSP_WAIT_PM0
    subeq   r11,r11, #1024
    streq   r1, [r7, #SSP_OFFS_EMUSTAT]
    mov     r0,     #0x400
    beq     ssp_drc_end
    orrne   r0, r0, #0x004
    b       ssp_drc_next


ssp_hle_902:
    cmp     r11, #0
    ble     ssp_drc_end

    add     r1, r7, #0x200
    ldrh    r0, [r1]
    ldr     r3, [r7, #SSP_OFFS_IRAM_ROM]
    add     r2, r3, r0, lsl #1			@ (r7|00)
    ldrh    r0, [r2], #2
    mov     r5, r5, lsl #16
    mov     r5, r5, lsr #16
    bic     r0, r0, #0xfc00
    add     r3, r3, r0, lsl #1 			@ IRAM dest
    ldrh    r12,[r2], #2			@ length
    bic     r3, r3, #3				@ always seen aligned
@    orr     r5, r5, #0x08000000
@    orr     r5, r5, #0x00880000
@    sub     r5, r5, r12, lsl #16
    bic     r6, r6, #0xf
    add     r12,r12,#1
    mov     r0, #1
    str     r0, [r7, #SSP_OFFS_IRAM_DIRTY]
    sub     r11,r11,r12,lsl #1
    sub     r11,r11,r12				@ -= length*3

ssp_hle_902_loop:
    ldrh    r0, [r2], #2
    ldrh    r1, [r2], #2
    subs    r12,r12,#2
    orr     r0, r0, r1, lsl #16
    str     r0, [r3], #4
    bgt     ssp_hle_902_loop

    tst     r12, #1
    ldrneh  r0, [r2], #2
    strneh  r0, [r3], #2

    ldr     r0, [r7, #SSP_OFFS_IRAM_ROM]
    add     r1, r7, #0x200
    sub     r2, r2, r0
    mov     r2, r2, lsr #1
    strh    r2, [r1]				@ (r7|00)

    sub     r0, r3, r0
    mov     r0, r0, lsr #1
    orr     r0, r0, #0x08000000
    orr     r0, r0, #0x001c8000
    str     r0, [r7, #(SSP_OFFS_GR+SSP_PMC*4)]
    str     r0, [r7, #(SSP_OFFS_PM_WRITE+4*4)]

    sub     r6, r6, #0x20000000
    add     r1, r7, #0x400
    add     r1, r1, #0x048			@ stack
    add     r1, r1, r6, lsr #28
    ldrh    r0, [r1]
    subs    r11,r11,#16				@ timeslice is likely to end
    ble     ssp_drc_end
    b       ssp_drc_next
Commit	Line	Data
e807ac75	1	@ vim:filetype=armasm
	2
	3	.if 0
	4	#include "compiler.h"
	5	.endif
	6
	7	.global tcache
	8
	9	.global flush_inval_caches
71bb1b7b	10	.global ssp_drc_entry
71bb1b7b	11	.global ssp_drc_next
45883918	12	.global ssp_drc_next_patch
45883918	13	.global ssp_drc_end
d5276282	14	.global ssp_hle_800
f5d1115f	15	.global ssp_hle_902
e807ac75	16
	17	@ translation cache buffer
	18	.text
	19	.align 12 @ 4096
	20	.size tcache, TCACHE_SIZE
	21	tcache:
	22	.space TCACHE_SIZE
	23
	24
	25	.text
	26	.align 2
	27
	28
	29	flush_inval_caches:
	30	mov r2, #0x0 @ must be 0
	31	swi 0x9f0002
	32	bx lr
	33
	34
	35	@ SSP_GR0, SSP_X, SSP_Y, SSP_A,
	36	@ SSP_ST, SSP_STACK, SSP_PC, SSP_P,
	37	@ SSP_PM0, SSP_PM1, SSP_PM2, SSP_XST,
	38	@ SSP_PM4, SSP_gr13, SSP_PMC, SSP_AL
	39
	40	@ register map:
	41	@ r4: XXYY
	42	@ r5: A
b9c1d012	43	@ r6: STACK and emu flags: sss0 * .uu. .lll NZCV (NZCV is PSR bits from ARM)
e807ac75	44	@ r7: SSP context
5d817c91	45	@ r8: r0-r2 (.210)
5d817c91	46	@ r9: r4-r6 (.654)
e807ac75	47	@ r10: P
	48	@ r11: cycles
	49
e807ac75	50
45883918	51	#define SSP_OFFS_GR 0x400
	52	#define SSP_PC 6
	53	#define SSP_P 7
	54	#define SSP_PM0 8
f5d1115f	55	#define SSP_PMC 14
f5d1115f	56	#define SSP_OFFS_PM_WRITE 0x46c // pmac_write[]
45883918	57	#define SSP_OFFS_EMUSTAT 0x484 // emu_status
f5d1115f	58	#define SSP_OFFS_IRAM_ROM 0x48c // ptr_iram_rom
45883918	59	#define SSP_OFFS_IRAM_DIRTY 0x494
	60	#define SSP_OFFS_IRAM_CTX 0x498 // iram_context
	61	#define SSP_OFFS_BLTAB 0x49c // block_table
	62	#define SSP_OFFS_BLTAB_IRAM 0x4a0
	63	#define SSP_OFFS_TMP0 0x4a4 // for entry PC
	64	#define SSP_OFFS_TMP1 0x4a8
	65	#define SSP_OFFS_TMP2 0x4ac
	66	#define SSP_WAIT_PM0 0x2000
	67
	68
	69	.macro ssp_drc_do_next patch_jump=0
	70	.if \patch_jump
	71	str lr, [r7, #SSP_OFFS_TMP2] @ jump instr. (actually call) address + 4
	72	.endif
	73	mov r0, r0, lsl #16
	74	mov r0, r0, lsr #16
	75	str r0, [r7, #SSP_OFFS_TMP0]
	76	cmp r0, #0x400
	77	blt 0f @ ssp_de_iram
	78
	79	ldr r2, [r7, #SSP_OFFS_BLTAB]
	80	ldr r2, [r2, r0, lsl #2]
	81	tst r2, r2
	82	.if \patch_jump
	83	bne ssp_drc_do_patch
	84	.else
	85	bxne r2
	86	.endif
	87	bl ssp_translate_block
	88	mov r2, r0
	89	ldr r0, [r7, #SSP_OFFS_TMP0] @ entry PC
	90	ldr r1, [r7, #SSP_OFFS_BLTAB]
	91	str r2, [r1, r0, lsl #2]
	92	.if \patch_jump
	93	b ssp_drc_do_patch
	94	.else
	95	bx r2
	96	.endif
	97
	98	0: @ ssp_de_iram:
	99	ldr r1, [r7, #SSP_OFFS_IRAM_DIRTY]
	100	tst r1, r1
	101	ldreq r1, [r7, #SSP_OFFS_IRAM_CTX]
	102	beq 1f @ ssp_de_iram_ctx
	103
	104	bl ssp_get_iram_context
	105	mov r1, #0
	106	str r1, [r7, #SSP_OFFS_IRAM_DIRTY]
	107	mov r1, r0
	108	str r1, [r7, #SSP_OFFS_IRAM_CTX]
	109	ldr r0, [r7, #SSP_OFFS_TMP0] @ entry PC
	110
	111	1: @ ssp_de_iram_ctx:
	112	ldr r2, [r7, #SSP_OFFS_BLTAB_IRAM]
	113	add r2, r2, r1, lsl #12 @ block_tab_iram + iram_context * 0x800/2*4
	114	add r1, r2, r0, lsl #2
	115	ldr r2, [r1]
	116	tst r2, r2
	117	.if \patch_jump
	118	bne ssp_drc_do_patch
	119	.else
	120	bxne r2
	121	.endif
	122	str r1, [r7, #SSP_OFFS_TMP1]
123	bl ssp_translate_block
124	mov r2, r0
125	ldr r0, [r7, #SSP_OFFS_TMP0] @ entry PC
126	ldr r1, [r7, #SSP_OFFS_TMP1] @ &block_table_iram[iram_context][rPC]
127	str r2, [r1]
128	.if \patch_jump
129	b ssp_drc_do_patch
130	.else
131	bx r2
132	.endif
133	.endm @ ssp_drc_do_next
134
135
136	ssp_drc_entry:
137	stmfd sp!, {r4-r11, lr}
138	mov r11, r0
71bb1b7b	139	ssp_regfile_load:
e807ac75	140	ldr r7, =ssp
	141	ldr r7, [r7]
	142	add r2, r7, #0x400
	143	add r2, r2, #4
	144	ldmia r2, {r3,r4,r5,r6,r8}
	145	mov r3, r3, lsr #16
	146	mov r3, r3, lsl #16
	147	orr r4, r3, r4, lsr #16 @ XXYY
b9c1d012	148
	149	and r8, r8, #0x0f0000
	150	mov r8, r8, lsl #13 @ sss0 *
	151	and r9, r6, #0x670000
	152	tst r6, #0x80000000
	153	orrne r8, r8, #0x8
	154	tst r6, #0x20000000
	155	orrne r8, r8, #0x4 @ sss0 * NZ..
a6fb500b	156	orr r6, r8, r9, lsr #12 @ sss0 * .uu. .lll NZ..
b9c1d012	157
e807ac75	158	ldr r8, [r7, #0x440] @ r0-r2
e807ac75	159	ldr r9, [r7, #0x444] @ r4-r6
45883918	160	ldr r10,[r7, #(0x400+SSP_P*4)] @ P
	161
	162	ldr r0, [r7, #(SSP_OFFS_GR+SSP_PC*4)]
	163	mov r0, r0, lsr #16
	164
	165
	166	ssp_drc_next:
	167	ssp_drc_do_next 0
	168
	169
	170	ssp_drc_next_patch:
	171	ssp_drc_do_next 1
	172
	173	ssp_drc_do_patch:
	174	ldr r1, [r7, #SSP_OFFS_TMP2] @ jump instr. (actually call) address + 4
	175	subs r12,r2, r1
	176	moveq r3, #0xe1000000
	177	orreq r3, r3, #0x00a00000 @ nop
	178	streq r3, [r1, #-4]
	179	beq ssp_drc_dp_end
	180
	181	cmp r12,#4
	182	ldreq r3, [r1]
	183	addeq r3, r3, #1
	184	streq r3, [r1, #-4] @ move the other cond up
	185	moveq r3, #0xe1000000
	186	orreq r3, r3, #0x00a00000
	187	streq r3, [r1] @ fill it's place with nop
	188	beq ssp_drc_dp_end
	189
	190	ldr r3, [r1, #-4]
	191	sub r12,r12,#4
	192	mov r3, r3, lsr #24
	193	bic r3, r3, #1 @ L bit
	194	orr r3, r3, r12,lsl #6
	195	mov r3, r3, ror #8 @ patched branch instruction
	196	str r3, [r1, #-4]
	197
	198	ssp_drc_dp_end:
	199	str r2, [r7, #SSP_OFFS_TMP1]
	200	sub r0, r1, #4
	201	add r1, r1, #4
	202	bl flush_inval_caches
	203	ldr r2, [r7, #SSP_OFFS_TMP1]
	204	ldr r0, [r7, #SSP_OFFS_TMP0]
	205	bx r2
e807ac75	206
e807ac75	207
45883918	208	ssp_drc_end:
	209	mov r0, r0, lsl #16
	210	str r0, [r7, #(SSP_OFFS_GR+SSP_PC*4)]
	211
71bb1b7b	212	ssp_regfile_store:
45883918	213	str r10,[r7, #(0x400+SSP_P*4)] @ P
e807ac75	214	str r8, [r7, #0x440] @ r0-r2
e807ac75	215	str r9, [r7, #0x444] @ r4-r6
b9c1d012	216
b9c1d012	217	mov r9, r6, lsr #13
e807ac75	218	and r9, r9, #(7<<16) @ STACK
b9c1d012	219	mov r3, r6, lsl #28
	220	msr cpsr_flg, r3 @ to to ARM PSR
	221	and r6, r6, #0x670
	222	mov r6, r6, lsl #12
	223	orrmi r6, r6, #0x80000000 @ N
	224	orreq r6, r6, #0x20000000 @ Z
	225
e807ac75	226	mov r3, r4, lsl #16 @ Y
	227	mov r2, r4, lsr #16
	228	mov r2, r2, lsl #16 @ X
	229	add r8, r7, #0x400
	230	add r8, r8, #4
	231	stmia r8, {r2,r3,r5,r6,r9}
71bb1b7b	232
71bb1b7b	233	mov r0, r11
	234	ldmfd sp!, {r4-r11, lr}
	235	bx lr
	236
	237
d5276282	238
	239	@ ld A, PM0
	240	@ andi 2
	241	@ bra z=1, gloc_0800
	242	ssp_hle_800:
d5276282	243	ldr r0, [r7, #(SSP_OFFS_GR+SSP_PM0*4)]
71bb1b7b	244	ldr r1, [r7, #SSP_OFFS_EMUSTAT]
d5276282	245	tst r0, #0x20000
f5d1115f	246	orreq r1, r1, #SSP_WAIT_PM0
45883918	247	subeq r11,r11, #1024
71bb1b7b	248	streq r1, [r7, #SSP_OFFS_EMUSTAT]
45883918	249	mov r0, #0x400
	250	beq ssp_drc_end
	251	orrne r0, r0, #0x004
45883918	252	b ssp_drc_next
d5276282	253
e807ac75	254
f5d1115f	255	ssp_hle_902:
	256	cmp r11, #0
	257	ble ssp_drc_end
	258
	259	add r1, r7, #0x200
	260	ldrh r0, [r1]
	261	ldr r3, [r7, #SSP_OFFS_IRAM_ROM]
	262	add r2, r3, r0, lsl #1 @ (r7\|00)
	263	ldrh r0, [r2], #2
	264	mov r5, r5, lsl #16
	265	mov r5, r5, lsr #16
	266	bic r0, r0, #0xfc00
	267	add r3, r3, r0, lsl #1 @ IRAM dest
	268	ldrh r12,[r2], #2 @ length
	269	bic r3, r3, #3 @ always seen aligned
	270	@ orr r5, r5, #0x08000000
	271	@ orr r5, r5, #0x00880000
	272	@ sub r5, r5, r12, lsl #16
	273	bic r6, r6, #0xf
	274	add r12,r12,#1
	275	mov r0, #1
	276	str r0, [r7, #SSP_OFFS_IRAM_DIRTY]
	277	sub r11,r11,r12,lsl #1
	278	sub r11,r11,r12 @ -= length*3
	279
	280	ssp_hle_902_loop:
	281	ldrh r0, [r2], #2
	282	ldrh r1, [r2], #2
	283	subs r12,r12,#2
	284	orr r0, r0, r1, lsl #16
	285	str r0, [r3], #4
	286	bgt ssp_hle_902_loop
	287
	288	tst r12, #1
	289	ldrneh r0, [r2], #2
	290	strneh r0, [r3], #2
	291
	292	ldr r0, [r7, #SSP_OFFS_IRAM_ROM]
	293	add r1, r7, #0x200
	294	sub r2, r2, r0
	295	mov r2, r2, lsr #1
	296	strh r2, [r1] @ (r7\|00)
	297
	298	sub r0, r3, r0
	299	mov r0, r0, lsr #1
	300	orr r0, r0, #0x08000000
	301	orr r0, r0, #0x001c8000
	302	str r0, [r7, #(SSP_OFFS_GR+SSP_PMC*4)]
	303	str r0, [r7, #(SSP_OFFS_PM_WRITE+4*4)]
	304
	305	sub r6, r6, #0x20000000
	306	add r1, r7, #0x400
	307	add r1, r1, #0x048 @ stack
	308	add r1, r1, r6, lsr #28
	309	ldrh r0, [r1]
	310	subs r11,r11,#16 @ timeslice is likely to end
	311	ble ssp_drc_end
	312	b ssp_drc_next
	313