Kernel/armv7 - Debugging, fixed TLB issues with a HACK!

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

diff --git a/Kernel/arch/armv7/start.S b/Kernel/arch/armv7/start.S
index 7c1be8c..8d9f3e4 100644 (file)
--- a/Kernel/arch/armv7/start.S
+++ b/Kernel/arch/armv7/start.S
@@ -8,7 +8,7 @@
 .section .init
 interrupt_vector_table:
 ivt_reset:     b _start        @ 0x00 Reset
-ivt_undef:     b .             @ 0x04 #UD
+ivt_undef:     b Undef_Handler @ 0x04 #UD
 ivt_svc:       b SVC_Handler   @ 0x08 SVC (used to be called SWI)
 ivt_prefetch:  b PrefetchAbort @ 0x0C Prefetch abort
 ivt_data:      b DataAbort     @ 0x10 Data abort
@@ -45,8 +45,13 @@ _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
+
        ldr r2, =0xF1000000
        mov r1, #'s'
        str r1, [r2]    
@@ -87,13 +92,27 @@ _start:
        str r1, [r2]    
        mov r1, #'m'
        str r1, [r2]    
+       mov r1, #13
+       str r1, [r2]    
+       mov r1, #10
+       str r1, [r2]    
 
-       ldr sp, =0x80000000-4   @ Set up stack (top of user range)
+.extern bss_start
+.extern bss_size_div_4
+.zero_bss:
+       ldr r0, =bss_start
+       ldr r1, =bss_end
+       mov r3, #0
+.zero_bss_loop:
+       str r3, [r0],#4
+       cmp r0, r1
+       bls .zero_bss_loop
+
+.goto_c:
+       ldr sp, =0x80000000-8   @ Set up stack (top of user range)
        ldr r0, =kmain
        mov pc, r0
 1:     b 1b    @ Infinite loop
-_ptr_kmain:
-       .long kmain
 
 .comm irqstack, 0x1000 @ ; 4KiB Stack
 .comm abortstack, 0x1000       @ ; 4KiB Stack
@@ -189,17 +208,27 @@ DataAbort:
 .globl PrefetchAbort
 PrefetchAbort:
        sub lr, #4      @ Adjust LR to the correct value
-@      srsdb sp!, #19  @ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD)
+       srsdb sp!, #23  @ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD)
 @      cpsid ifa, #19
-@      PUSH_GPRS
+       PUSH_GPRS
 
        ldr r0, =csAbort_Tag
        ldr r1, =csPrefetchAbort_Fmt
-       mov r2, lr
-       mrc p15, 0, r3, c5, c0, 0       @ Read IFSR (Instruction Fault Address Register) into R3
-       ldr r4, =Log_Error
-       blx r4
-       b .
+#      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
+.globl Undef_Handler
+Undef_Handler:
+       wfi
+       b Undef_Handler
+
+
 
 .section .rodata
 csIRQ_Tag:
@@ -334,5 +363,5 @@ kernel_exception_map:
 stack:
        .space MM_KSTACK_SIZE, 0        @ Original kernel stack
 
-// vim: ts=8, ft=armv7
+// vim: ts=8 ft=armv7