Fix some bugs.

Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/imx6q-sabrelite/trap_common.c

@@ -47,8 +47,10 @@ static struct XiziTrapDriver xizi_trap_driver;
 
 void panic(char* s)
 {
-    xizi_trap_driver.cpu_irq_disable();
-    spinlock_unlock(&whole_kernel_lock);
+    // xizi_trap_driver.cpu_irq_disable();
+    if (is_spinlock_locked(&whole_kernel_lock) && whole_kernel_lock.owner_cpu == cur_cpuid()) {
+        spinlock_unlock(&whole_kernel_lock);
+    }
     KPrintf("panic: %s\n", s);
     for (;;)
         ;

Ubiquitous/XiZi_AIoT/hardkernel/intr/arm/armv7-a/cortex-a9/trampoline.S

@@ -47,43 +47,16 @@ trap_return:
 	ldmfd   r13!, 	{r14}         	
 	ldmfd   r13!, 	{r2}          	
 	msr     spsr_cxsf, r2
-	ldr 	r0, [r13], #4
-	ldr 	r1, [r13], #4
-	ldr 	r2, [r13], #4
-	ldr 	r3, [r13], #4
-	ldr 	r4, [r13], #4
-	ldr 	r5, [r13], #4
-	ldr 	r6, [r13], #4
-	ldr 	r7, [r13], #4
-	ldr 	r8, [r13], #4
-	ldr 	r9, [r13], #4
-	ldr 	r10, [r13], #4
-	ldr 	r11, [r13], #4
-	ldr 	r12, [r13], #4
-	ldm 	r13!, {pc}^
+	ldmfd   r13!,	{r0-r12, pc}^  	// restore context and return
 
 user_trap_swi_enter:
-    # save trapframe to swi stack
-    sub 	sp, sp, #56     
-	str 	r14, [sp, #52]  
-	str 	r12, [sp, #48] 
-	str 	r11, [sp, #44] 
-	str 	r10, [sp, #40]
-	str 	r9, [sp, #36] 
-	str 	r8, [sp, #32]  
-	str 	r7, [sp, #28]  
-	str 	r6, [sp, #24] 
-	str 	r5, [sp, #20] 
-	str 	r4, [sp, #16]
-	str 	r3, [sp, #12]
-	str 	r2, [sp, #8]
-	str 	r1, [sp, #4]
-	str 	r0, [sp]
+	# save trapframe to swi stack
+    stmfd   sp!, 	{r0-r12, r14}  	// save context
+	mrs     r2, 	spsr            // copy spsr to r2
+	stmfd   r13!, 	{r2}          	// save r2(spsr) to the stack
 
-	mrs     r2, 	spsr            
-	stmfd   r13!, 	{r2}       
-	stmfd   r13!, 	{r14}         	
-	stmfd   r13, 	{sp, lr}^    
+	stmfd   r13!, 	{r14}         	// save r14 again to have one uniform trapframe
+	stmfd   r13, 	{sp, lr}^      	// save user mode sp and lr
 	sub     r13, 	r13, #8
 
     # call syscall handler
@@ -92,17 +65,12 @@ user_trap_swi_enter:
 	b		trap_return
 
 trap_irq_enter:
-	# save context in irq stack
-	sub     r14, 	r14, #4
-	sub 	sp, sp, #16   
-	str 	r14, [sp, #12]
-	str 	r2, [sp, #8]
-	str 	r1, [sp, #4] 
-	str 	r0, [sp] 
-
-	mrs     r1, 	spsr
-	mov     r0, 	r13          	// irq stack stop
-	add     r13,	r13, #16    	// reset IRQ stack
+	# save it on the stack as r14 is banked
+	sub     r14, 	r14, #4       	// r14 (lr) contains the interrupted PC
+	stmfd   r13!, 	{r0-r2, r14}   	//
+	mrs     r1, 	spsr        	// save spsr_irq
+	mov     r0, 	r13          	// save stack stop (r13_irq)
+	add     r13,	r13, #16    	// reset the IRQ stack
 
 	# switch to the SVC mode
 	mrs     r2, 	cpsr
@@ -111,25 +79,15 @@ trap_irq_enter:
 	msr     cpsr_cxsf, r2
 
 	# build the trap frame
-	ldr     r2, 	[r0, #12]
+	ldr     r2, 	[r0, #12]    	// read the r14_irq, then save it
 	stmfd   r13!, 	{r2}
-	sub 	r13, r13, #40   
-	str 	r12, [r13, #36] 
-	str 	r11, [r13, #32]
-	str 	r10, [r13, #28]
-	str 	r9, [r13, #24]
-	str 	r8, [r13, #20]
-	str 	r7, [r13, #16]
-	str 	r6, [r13, #12]
-	str 	r5, [r13, #8]
-	str 	r4, [r13, #4]
-	str 	r3, [r13] 
-
-	ldmfd   r0, 	{r3-r5}     	
+	stmfd   r13!, 	{r3-r12}   		// r4-r12 are preserved (non-banked)
+	ldmfd   r0, 	{r3-r5}     	// copy r0-r2 over from irq stack
 	stmfd   r13!, 	{r3-r5}
-	stmfd   r13!, 	{r1}  
-	stmfd   r13!, 	{lr} 
-	stmfd   r13, 	{sp, lr}^ 
+	stmfd   r13!, 	{r1}         	// save spsr
+	stmfd   r13!, 	{lr}          	// save lr_svc
+
+	stmfd   r13, 	{sp, lr}^     	// save user mode sp and lr
 	sub     r13, 	r13, #8
 
 	mov     r0, 	r13           	// trapframe as parameters