diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/boot.S b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/boot.S index 7d4d0da8d..3731ef6b8 100644 --- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/boot.S +++ b/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 diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.S b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.S index 7f52ee1ee..dc3eb6e2d 100644 --- a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.S +++ b/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 -Reset_Handler: - b _start + .word 0 // extra word in RAM vectors + +.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 diff --git a/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.c b/Ubiquitous/XiZi_AIoT/hardkernel/arch/arm/armv7-a/cortex-a9/exception.c new file mode 100644 index 000000000..de9a52e32 --- /dev/null +++ b/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); + } +}