Kernel/ARMv7 - Call timers on tegra2

[tpg/acess2.git] / KernelLand / Kernel / arch / armv7 / start.S

diff --git a/KernelLand/Kernel/arch/armv7/start.S b/KernelLand/Kernel/arch/armv7/start.S
index 8d9f3e4..1fe0fd8 100644 (file)
--- a/KernelLand/Kernel/arch/armv7/start.S
+++ b/KernelLand/Kernel/arch/armv7/start.S
@@ -16,8 +16,17 @@ ivt_unused:  b .             @ 0x14 Not Used
 ivt_irq:       b IRQHandler    @ 0x18 IRQ
 ivt_fiq:       b .             @ 0x1C FIQ (Fast interrupt)
 
+init_lock:     .long 0
+
+.extern SHORTLOCK
 .globl _start
 _start:
+       ldr r1, =init_lock
+       mov r0, #1
+       swp r0, r0, [r1]
+       tst r0,r0
+       bne .smp_halt
+       
        ldr r2, =UART0_PADDR
        mov r1, #'A'
        str r1, [r2]    
@@ -45,12 +54,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 +76,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]    
@@ -113,6 +125,8 @@ _start:
        ldr r0, =kmain
        mov pc, r0
 1:     b 1b    @ Infinite loop
+.smp_halt:
+1:     b 1b
 
 .comm irqstack, 0x1000 @ ; 4KiB Stack
 .comm abortstack, 0x1000       @ ; 4KiB Stack
@@ -195,12 +209,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 +235,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 +269,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"