X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;ds=sidebyside;f=Kernel%2Farch%2Farmv7%2Fstart.S;h=5a8e4eb4acd331c23bf4cb70148b2e893902b694;hb=7852cb728c839810f4de80b60ba29f29a1fcfa10;hp=089ead5d03e880f9739bf485a8acf79bff28b4fa;hpb=b1e74a3e843f44c09c9c88b42db0efba99facb13;p=tpg%2Facess2.git

diff --git a/Kernel/arch/armv7/start.S b/Kernel/arch/armv7/start.S
index 089ead5d..5a8e4eb4 100644
--- a/Kernel/arch/armv7/start.S
+++ b/Kernel/arch/armv7/start.S
@@ -9,7 +9,7 @@
 interrupt_vector_table:
 ivt_reset:	b _start @ Reset
 ivt_undef:	b .	@ #UD
-ivt_svc:	b SyscallHandler @ SVC (SWI assume)
+ivt_svc:	b SVC_Handler @ SVC (used to be called SWI)
 ivt_prefetch:	b PrefetchAbort	@ Prefetch abort
 ivt_data:	b DataAbort	@ Data abort
 ivt_unused:	b .	@ Not Used
@@ -65,8 +65,72 @@ _ptr_kmain:
 .comm irqstack, 0x1000	@ ; 4KiB Stack
 .comm abortstack, 0x1000	@ ; 4KiB Stack
 
-SyscallHandler:
-	b .
+.extern SyscallHandler
+SVC_Handler:
+@	sub lr, #4
+	srsdb sp!, #19	@ Save state to stack
+	cpsie ifa, #19	@ Ensure we're in supervisor with interrupts enabled (should already be there)
+	push {r0-r12}
+
+	ldr r4, [lr,#-4]
+	mvn r5, #0xFF000000
+	and r4, r5
+
+	tst r4, #0x1000 
+	bne .arm_specifics
+
+	push {r4}
+
+	mov r0, sp
+	ldr r4, =SyscallHandler
+	blx r4
+	
+	pop {r2}	@ errno
+	pop {r0,r1}	@ Ret/RetHi
+	add sp, #2*4	@ Saved r2/r3
+	
+	pop {r4-r12}
+	rfeia sp!	@ Pop state (actually RFEFD)
+.arm_specifics:
+	and r4, #0xFF
+
+@
+@ Cache invalidation
+	cmp r4, #0x001
+	bne 1f
+	@ Page align
+	mov r2, #0x1000
+	sub r2, #1
+	add r1, r2
+	mvn r2, r2
+	and r0, r2
+	and r1, r2
+	cmp r0, #0x78000000
+	cmpls r1, #0x78000000
+	movge r0, #-1
+	movge r1, #0
+	movge r2, #1
+	bge .ret
+
+2:
+	cmp r0, r1
+	mcrlt p15, 0, r0, c7, c5, 1
+	mcrlt p15, 0, r0, c7, c6, 1
+	addlt r0, #0x1000
+	blt 2b
+	mov r0, #0
+	mov r1, #0
+	mov r2, #0
+	b .ret
+1:	
+	mov r0, #-1
+	mov r1, #0
+	mov r2, #-1
+.ret:
+	add sp, #4*4
+	pop {r4-r12}
+	rfeia sp!
+
 
 .globl gpIRQHandler
 gpIRQHandler:	.long	0
@@ -114,7 +178,6 @@ DataAbort:
 
 @	POP_GPRS
 @	rfeia sp!	@ Pop state (actually RFEFD)
-	bx lr
 
 .globl PrefetchAbort
 PrefetchAbort: