X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FKernel%2Farch%2Farmv7%2Fstart.S;h=8193b31c2edf6c9382ac3a3e29b72e6d368b5600;hb=25398a7f24d189ee7a70fcf0e64827cbc8999028;hp=8d9f3e4d2a815510a1ebb12c8b7deeba52fb3d3d;hpb=48743e39650eb1ef988380e9d95f27fd40d3a9ce;p=tpg%2Facess2.git

diff --git a/KernelLand/Kernel/arch/armv7/start.S b/KernelLand/Kernel/arch/armv7/start.S
index 8d9f3e4d..8193b31c 100644
--- a/KernelLand/Kernel/arch/armv7/start.S
+++ b/KernelLand/Kernel/arch/armv7/start.S
@@ -45,12 +45,11 @@ _start:
 	mrc p15, 0, r0, c1, c0, 0
 	orr r0, r0, #1
 	orr r0, r0, #1 << 23
-	mvn r1, #1 << 2
-	and r0, r0, r1
 	mcr p15, 0, r0, c1, c0, 0
 
-	@ HACK! Disable caching
-	mrc p15, 0, r1, c1, c0, 0
+	@ HACK: Set ASID to non zero
+	mov r0, #1
+	MCR p15,0,r0,c13,c0,1
 
 	ldr r2, =0xF1000000
 	mov r1, #'s'
@@ -68,13 +67,17 @@ _start:
 	@
 	mrc p15, 0, r0, c0, c1, 1
 	and r0, #0xF0
+	beq .no_sec_ext
 	@ - Present
-	ldrne r0,=KERNEL_BASE
-	mcrne p15, 0, r0, c12, c0, 0	@ Set the VBAR (brings exceptions into high memory)
+	ldr r0,=KERNEL_BASE
+	mcr p15, 0, r0, c12, c0, 0	@ Set the VBAR (brings exceptions into high memory)
+	b .exceptions_vectored
+.no_sec_ext:
 	@ - Absent
-	mrceq p15, 0, r0, c1, c0, 0	@ Set SCTLR.V
-	orreq r0, #0x2000
-	mcreq p15, 0, r0, c1, c0, 0
+	mrc p15, 0, r0, c1, c0, 0	@ Set SCTLR.V
+	orr r0, #0x2000
+	mcr p15, 0, r0, c1, c0, 0
+.exceptions_vectored:
 
 	mov r1, #'-'
 	str r1, [r2]	
@@ -195,12 +198,21 @@ DataAbort:
 @	cpsid ifa, #19
 	PUSH_GPRS
 
+	@ Get the user's LR value (and push to stack)
+	cps #31	@ Go to system mode
+	mov r1, lr
+	cps #19	@ Go to supervisor
+	mov r0, lr
+	cps #23	@ back to exception
+	push {r0,r1}
+	
 	mov r3, #0	@ not a prefetch abort
 	mrc p15, 0, r2, c5, c0, 0	@ Read DFSR (Data Fault Status Register) to R2
 	mrc p15, 0, r1, c6, c0, 0	@ Read DFAR (Data Fault Address Register) into R1
 	mov r0, lr	@ PC
 	ldr r4, =MM_PageFault
 	blx r4
+	add sp, #8	@ Undo push of the user/system LR
 
 	POP_GPRS
 	rfeia sp!	@ Pop state (actually RFEFD)
@@ -212,17 +224,24 @@ PrefetchAbort:
 @	cpsid ifa, #19
 	PUSH_GPRS
 
-	ldr r0, =csAbort_Tag
-	ldr r1, =csPrefetchAbort_Fmt
-#	mov r2, lr
-	mrc p15, 0, r2, c6, c0, 2	@ Read IFAR (Instruction Fault Address Register) into R3
-	mrc p15, 0, r3, c5, c0, 1	@ Read IFSR (Instruction Fault Status Register) into R3
-	ldr r5, =Log_Error
-	blx r5
 
-.loop:
-	wfi
-	b .loop
+	@ Get the user's LR value (and push to stack)
+	cps #31	@ Go to system mode
+	mov r0, lr
+	cps #23	@ back to supervisor
+	push {r0}
+	
+	mrc p15, 0, r2, c5, c0, 1	@ Read IFSR (Instruction Fault Status Register) into R3
+	mrc p15, 0, r1, c6, c0, 2	@ Read IFAR (Instruction Fault Address Register) into R3
+	mov r0, r1
+	mov r3, #1	@ IS a prefetch abort
+	ldr r4, =MM_PageFault
+	blx r4
+	add sp, #4	@ Undo push of the user LR
+
+	POP_GPRS
+	rfeia sp!	@ Pop state (actually RFEFD)
+
 .globl Undef_Handler
 Undef_Handler:
 	wfi
@@ -239,7 +258,7 @@ csIRQ_Fmt:
 csDataAbort_Fmt:
 	.asciz "Data Abort - %p accessed %p, DFSR=%x Unk:%x Unk:%x"
 csPrefetchAbort_Fmt:
-	.asciz "Prefetch Abort at %p, IFSR=%x"
+	.asciz "Prefetch Abort at %p, IFSR=%x, UserLR:0x%x"
 csSyscallPrintRetAddr:
 	.asciz "Syscall ret to %p"