add exception.c and irq entry function

Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/boot.S

@@ -28,6 +28,7 @@ _start:
     bic     r0, #(1 << 2)           /* d cache */
     bic     r0, #(1 << 0)           /* mmu */
     mcr     p15, 0, r0, c1, c0, 0
+    
 
     ldr     r0, =stack_top
 

Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.S

@@ -12,7 +12,7 @@ ExceptionVectors:
   ldr pc, _IrqException
   ldr pc, _FiqException
 
-.globl Reset_Handler
+.globl _start
 .globl UndefInstrExceptionHandle
 .globl SwiExceptionHandle
 .globl PrefetchAbortExceptionHandle
@@ -22,7 +22,7 @@ ExceptionVectors:
 .globl FiqExceptionHandle
 
 _ResetException:
-  .word Reset_Handler
+  .word _start
 _UndefInstrException:
   .word UndefInstrExceptionHandle
 _SwiException:
@@ -38,29 +38,110 @@ _IrqException:
 _FiqException:
   .word FiqExceptionHandle
 
-.globl _start
+  .word 0 // extra word in RAM vectors
-Reset_Handler:
-    b     _start
 
+
+.macro push_svc_reg
+    sub     sp, sp, #17 * 4         @/* Sizeof(struct rt_hw_exp_stack)  */
+    stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
+    mov     r0, sp
+    mrs     r6, spsr                @/* Save CPSR                       */
+    str     lr, [r0, #15*4]         @/* Push PC                         */
+    str     r6, [r0, #16*4]         @/* Push CPSR                       */
+    cps     #Mode_SVC
+    str     sp, [r0, #13*4]         @/* Save calling SP                 */
+    str     lr, [r0, #14*4]         @/* Save calling PC                 */
+.endm
+
+.align  5
+.globl	UndefInstrExceptionHandle
 UndefInstrExceptionHandle:
-    b     UndefInstrIsrEntry
+1:
+  b       1b
 
+.align  5
+.globl	SwiExceptionHandle
 SwiExceptionHandle:
-    b     SvcIsrEntry
+  push_svc_reg
+  bl      rt_hw_trap_swi
+  b       .
 
+.align  5
+.globl	PrefetchAbortExceptionHandle
 PrefetchAbortExceptionHandle:
-    b     PrefetchAbortIsrEntry
+1:
+  b       1b
 
+.align  5
+.globl	DataAbortExceptionHandle
 DataAbortExceptionHandle:
-    b     DataAbortIsrEntry
+1:
+  b       1b
 
+.align  5
+.globl	ResvExceptionHandle
 ResvExceptionHandle:
-    b     ResvIsrEntry
+1:
+  b       1b
 
+.section .text.isr, "ax"
+.align  5
+.globl	ExceptionIsrEntry
 ExceptionIsrEntry:
+
     stmfd   sp!, {r0-r12,lr}
 
-    b      IsrEntry
+    bl      rt_interrupt_enter
+    bl      rt_hw_trap_irq
+    bl      rt_interrupt_leave
 
+    @ if rt_thread_switch_interrupt_flag set, jump to
+    @ rt_hw_context_switch_interrupt_do and don't return
+    ldr     r0, =rt_thread_switch_interrupt_flag
+    ldr     r1, [r0]
+    cmp     r1, #1
+    beq     rt_hw_context_switch_interrupt_do
+
+    ldmfd   sp!, {r0-r12,lr}
+    subs    pc,  lr, #4
+
+rt_hw_context_switch_interrupt_do:
+    mov     r1,  #0         @ clear flag
+    str     r1,  [r0]
+
+    mov     r1, sp          @ r1 point to {r0-r3} in stack
+    add     sp, sp, #4*4
+    ldmfd   sp!, {r4-r12,lr}@ reload saved registers
+    mrs     r0,  spsr       @ get cpsr of interrupt thread
+    sub     r2,  lr, #4     @ save old task's pc to r2
+
+    @ Switch to SVC mode with no interrupt. If the usr mode guest is
+    @ interrupted, this will just switch to the stack of kernel space.
+    @ save the registers in kernel space won't trigger data abort.
+    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
+
+    stmfd   sp!, {r2}       @ push old task's pc
+    stmfd   sp!, {r4-r12,lr}@ push old task's lr,r12-r4
+    ldmfd   r1,  {r1-r4}    @ restore r0-r3 of the interrupt thread
+    stmfd   sp!, {r1-r4}    @ push old task's r0-r3
+    stmfd   sp!, {r0}       @ push old task's cpsr
+
+    ldr     r4,  =rt_interrupt_from_thread
+    ldr     r5,  [r4]
+    str     sp,  [r5]       @ store sp in preempted tasks's TCB
+
+    ldr     r6,  =rt_interrupt_to_thread
+    ldr     r6,  [r6]
+    ldr     sp,  [r6]       @ get new task's stack pointer
+
+    ldmfd   sp!, {r4}       @ pop new task's cpsr to spsr
+    msr     spsr_cxsf, r4
+
+    ldmfd   sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
+ 
+
+.align  5
+.globl	FiqExceptionHandle
 FiqExceptionHandle:
-    b      FiqIsrEntry
+1:
+  b       1b

Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.c

@@ -0,0 +1,75 @@
+
+/**
+ * this function will show registers of CPU
+ *
+ * @param regs the registers point
+ */
+void PrintStackFrame(struct rt_hw_exp_stack *regs)
+{
+    rt_kprintf("Execption:\n");
+    rt_kprintf("r00:0x%08x r01:0x%08x r02:0x%08x r03:0x%08x\n", regs->r0, regs->r1, regs->r2, regs->r3);
+    rt_kprintf("r04:0x%08x r05:0x%08x r06:0x%08x r07:0x%08x\n", regs->r4, regs->r5, regs->r6, regs->r7);
+    rt_kprintf("r08:0x%08x r09:0x%08x r10:0x%08x\n", regs->r8, regs->r9, regs->r10);
+    rt_kprintf("fp :0x%08x ip :0x%08x\n", regs->fp, regs->ip);
+    rt_kprintf("sp :0x%08x lr :0x%08x pc :0x%08x\n", regs->sp, regs->lr, regs->pc);
+    rt_kprintf("cpsr:0x%08x\n", regs->cpsr);
+}
+
+
+/**
+ * The software interrupt instruction (SWI) is used for entering
+ * Supervisor mode, usually to request a particular supervisor
+ * function.
+ *
+ * @param regs system registers
+ *
+ * @note never invoke this function in application
+ */
+void rt_hw_trap_swi(struct rt_hw_exp_stack *regs)
+{
+
+}
+
+void rt_hw_trap_irq(void)
+{
+    void *param;
+    rt_isr_handler_t isr_func;
+    extern struct rt_irq_desc isr_table[];
+
+    // vectNum = RESERVED[31:13] | CPUID[12:10] | INTERRUPT_ID[9:0]
+    // send ack and get ID source
+    uint32_t vectNum = gic_read_irq_ack();
+
+    // Check that INT_ID isn't 1023 or 1022 (spurious interrupt)
+    if (vectNum & 0x0200)
+    {
+        gic_write_end_of_irq(vectNum);  // send end of irq
+    }
+    else
+    {
+        // copy the local value to the global image of CPUID
+        unsigned cpu = (vectNum >> 10) & 0x7;
+        unsigned irq = vectNum & 0x1FF;
+
+        /* skip warning */
+        cpu = cpu;
+
+        // Call the service routine stored in the handlers array. If there isn't
+        // one for this IRQ, then call the default handler.
+        /* get interrupt service routine */
+        isr_func = isr_table[irq].handler;
+#ifdef RT_USING_INTERRUPT_INFO
+        isr_table[irq].counter++;
+#endif
+        if (isr_func)
+        {
+            /* Interrupt for myself. */
+            param = isr_table[irq].param;
+            /* turn to interrupt service routine */
+            isr_func(irq, param);
+        }
+
+        // Signal the end of the irq.
+        gic_write_end_of_irq(vectNum);
+    }
+}