From 159def753f719e18e65110de47c8a4385cb33e3f Mon Sep 17 00:00:00 2001 From: shaojinchun Date: Thu, 14 Mar 2019 15:45:20 +0800 Subject: [PATCH] =?UTF-8?q?arm926=E5=86=85=E5=AE=B9=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libcpu/arm/arm926/context_gcc.S | 1 + libcpu/arm/arm926/cpuport.c | 44 +-- libcpu/arm/arm926/machine.c | 41 +++ libcpu/arm/arm926/mmu.c | 40 +-- libcpu/arm/arm926/mmu.h | 5 +- libcpu/arm/arm926/stack.c | 2 +- libcpu/arm/arm926/start_gcc.S | 557 +++++++++++++++++--------------- libcpu/arm/arm926/trap.c | 91 +++--- 8 files changed, 420 insertions(+), 361 deletions(-) create mode 100644 libcpu/arm/arm926/machine.c diff --git a/libcpu/arm/arm926/context_gcc.S b/libcpu/arm/arm926/context_gcc.S index b6b786367..f69aa9f5b 100644 --- a/libcpu/arm/arm926/context_gcc.S +++ b/libcpu/arm/arm926/context_gcc.S @@ -10,6 +10,7 @@ #define NOINT 0xC0 +.text ;/* ; * rt_base_t rt_hw_interrupt_disable(); ; */ diff --git a/libcpu/arm/arm926/cpuport.c b/libcpu/arm/arm926/cpuport.c index 1efb23e89..035e97d54 100644 --- a/libcpu/arm/arm926/cpuport.c +++ b/libcpu/arm/arm926/cpuport.c @@ -23,30 +23,30 @@ rt_inline rt_uint32_t cp15_rd(void) { rt_uint32_t i; - __asm volatile("mrc p15, 0, %0, c1, c0, 0":"=r" (i)); + __asm volatile("mrc p15, 0, %0, c1, c0, 0":"=r"(i)); return i; } rt_inline void cache_enable(rt_uint32_t bit) { __asm volatile(\ - "mrc p15,0,r0,c1,c0,0\n\t" \ - "orr r0,r0,%0\n\t" \ - "mcr p15,0,r0,c1,c0,0" \ - : \ - :"r" (bit) \ - :"memory"); + "mrc p15,0,r0,c1,c0,0\n\t" \ + "orr r0,r0,%0\n\t" \ + "mcr p15,0,r0,c1,c0,0" \ + : \ + : "r"(bit) \ + : "memory"); } rt_inline void cache_disable(rt_uint32_t bit) { __asm volatile(\ - "mrc p15,0,r0,c1,c0,0\n\t" \ - "bic r0,r0,%0\n\t" \ - "mcr p15,0,r0,c1,c0,0" \ - : \ - :"r" (bit) \ - :"memory"); + "mrc p15,0,r0,c1,c0,0\n\t" \ + "bic r0,r0,%0\n\t" \ + "mcr p15,0,r0,c1,c0,0" \ + : \ + : "r"(bit) \ + : "memory"); } #endif @@ -152,7 +152,7 @@ void rt_hw_cpu_reset() rt_kprintf("Restarting system...\n"); machine_reset(); - while(1); /* loop forever and wait for reset to happen */ + while (1); /* loop forever and wait for reset to happen */ /* NEVER REACHED */ } @@ -206,21 +206,7 @@ int __rt_ffs(int value) #elif defined(__GNUC__) || defined(__ICCARM__) int __rt_ffs(int value) { - register rt_uint32_t x; - - if (value == 0) - return value; - - __asm - ( - "rsb %[temp], %[val], #0\n" - "and %[temp], %[temp], %[val]\n" - "clz %[temp], %[temp]\n" - "rsb %[temp], %[temp], #32\n" - :[temp] "=r"(x) - :[val] "r"(value) - ); - return x; + return __builtin_ffs(value); } #endif diff --git a/libcpu/arm/arm926/machine.c b/libcpu/arm/arm926/machine.c new file mode 100644 index 000000000..23953dea4 --- /dev/null +++ b/libcpu/arm/arm926/machine.c @@ -0,0 +1,41 @@ +/* + * File : cpu.c + * This file is part of RT-Thread RTOS + * COPYRIGHT (C) 2017, RT-Thread Development Team + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + * Change Logs: + * Date Author Notes + * 2018-02-08 RT-Thread the first version + */ + +#include +#include + +RT_WEAK void machine_reset(void) +{ + rt_kprintf("reboot system...\n"); + rt_hw_interrupt_disable(); + while (1); +} + +RT_WEAK void machine_shutdown(void) +{ + rt_kprintf("shutdown...\n"); + rt_hw_interrupt_disable(); + while (1); +} + diff --git a/libcpu/arm/arm926/mmu.c b/libcpu/arm/arm926/mmu.c index f1e475189..19d70b9b0 100644 --- a/libcpu/arm/arm926/mmu.c +++ b/libcpu/arm/arm926/mmu.c @@ -140,7 +140,7 @@ void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size) ptr = buffer & ~(CACHE_LINE_SIZE - 1); - while(ptr < buffer + size) + while (ptr < buffer + size) { __asm volatile { MCR p15, 0, ptr, c7, c14, 1 } ptr += CACHE_LINE_SIZE; @@ -211,18 +211,18 @@ void mmu_setttbase(register rt_uint32_t i) * set by page table entry */ value = 0; - asm volatile ("mcr p15, 0, %0, c8, c7, 0"::"r"(value)); + asm volatile("mcr p15, 0, %0, c8, c7, 0"::"r"(value)); value = 0x55555555; - asm volatile ("mcr p15, 0, %0, c3, c0, 0"::"r"(value)); + asm volatile("mcr p15, 0, %0, c3, c0, 0"::"r"(value)); - asm volatile ("mcr p15, 0, %0, c2, c0, 0"::"r"(i)); + asm volatile("mcr p15, 0, %0, c2, c0, 0"::"r"(i)); } void mmu_set_domain(register rt_uint32_t i) { - asm volatile ("mcr p15,0, %0, c3, c0, 0": :"r" (i)); + asm volatile("mcr p15,0, %0, c3, c0, 0": :"r"(i)); } void mmu_enable() @@ -321,7 +321,7 @@ void mmu_disable_alignfault() void mmu_clean_invalidated_cache_index(int index) { - asm volatile ("mcr p15, 0, %0, c7, c14, 2": :"r" (index)); + asm volatile("mcr p15, 0, %0, c7, c14, 2": :"r"(index)); } void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size) @@ -330,9 +330,9 @@ void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size) ptr = buffer & ~(CACHE_LINE_SIZE - 1); - while(ptr < buffer + size) + while (ptr < buffer + size) { - asm volatile ("mcr p15, 0, %0, c7, c14, 1": :"r" (ptr)); + asm volatile("mcr p15, 0, %0, c7, c14, 1": :"r"(ptr)); ptr += CACHE_LINE_SIZE; } @@ -347,7 +347,7 @@ void mmu_clean_dcache(rt_uint32_t buffer, rt_uint32_t size) while (ptr < buffer + size) { - asm volatile ("mcr p15, 0, %0, c7, c10, 1": :"r" (ptr)); + asm volatile("mcr p15, 0, %0, c7, c10, 1": :"r"(ptr)); ptr += CACHE_LINE_SIZE; } @@ -361,7 +361,7 @@ void mmu_invalidate_dcache(rt_uint32_t buffer, rt_uint32_t size) while (ptr < buffer + size) { - asm volatile ("mcr p15, 0, %0, c7, c6, 1": :"r" (ptr)); + asm volatile("mcr p15, 0, %0, c7, c6, 1": :"r"(ptr)); ptr += CACHE_LINE_SIZE; } @@ -369,19 +369,19 @@ void mmu_invalidate_dcache(rt_uint32_t buffer, rt_uint32_t size) void mmu_invalidate_tlb() { - asm volatile ("mcr p15, 0, %0, c8, c7, 0": :"r" (0)); + asm volatile("mcr p15, 0, %0, c8, c7, 0": :"r"(0)); } void mmu_invalidate_icache() { - asm volatile ("mcr p15, 0, %0, c7, c5, 0": :"r" (0)); + asm volatile("mcr p15, 0, %0, c7, c5, 0": :"r"(0)); } void mmu_invalidate_dcache_all() { - asm volatile ("mcr p15, 0, %0, c7, c6, 0": :"r" (0)); + asm volatile("mcr p15, 0, %0, c7, c6, 0": :"r"(0)); } #endif @@ -389,10 +389,10 @@ void mmu_invalidate_dcache_all() /* level1 page table */ #if defined(__ICCARM__) #pragma data_alignment=(16*1024) -static volatile rt_uint32_t _page_table[4*1024]; +static volatile rt_uint32_t _page_table[4 * 1024]; #else -static volatile rt_uint32_t _page_table[4*1024] \ - __attribute__((aligned(16*1024))); +static volatile rt_uint32_t _page_table[4 * 1024] \ +__attribute__((aligned(16 * 1024))); #endif void mmu_setmtt(rt_uint32_t vaddrStart, rt_uint32_t vaddrEnd, @@ -401,11 +401,11 @@ void mmu_setmtt(rt_uint32_t vaddrStart, rt_uint32_t vaddrEnd, volatile rt_uint32_t *pTT; volatile int nSec; int i = 0; - pTT=(rt_uint32_t *)_page_table+(vaddrStart>>20); - nSec=(vaddrEnd>>20)-(vaddrStart>>20); - for(i=0; i<=nSec; i++) + pTT = (rt_uint32_t *)_page_table + (vaddrStart >> 20); + nSec = (vaddrEnd >> 20) - (vaddrStart >> 20); + for (i = 0; i <= nSec; i++) { - *pTT = attr |(((paddrStart>>20)+i)<<20); + *pTT = attr | (((paddrStart >> 20) + i) << 20); pTT++; } } diff --git a/libcpu/arm/arm926/mmu.h b/libcpu/arm/arm926/mmu.h index 7b930f535..36fdbeacd 100644 --- a/libcpu/arm/arm926/mmu.h +++ b/libcpu/arm/arm926/mmu.h @@ -5,6 +5,7 @@ * * Change Logs: * Date Author Notes + * 2018-02-08 RT-Thread the first version */ #ifndef __MMU_H__ @@ -45,5 +46,7 @@ struct mem_desc }; void rt_hw_mmu_init(struct mem_desc *mdesc, rt_uint32_t size); - +void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size); +void mmu_clean_dcache(rt_uint32_t buffer, rt_uint32_t size); +void mmu_invalidate_dcache(rt_uint32_t buffer, rt_uint32_t size); #endif diff --git a/libcpu/arm/arm926/stack.c b/libcpu/arm/arm926/stack.c index fa98feb82..f90bfa8c2 100644 --- a/libcpu/arm/arm926/stack.c +++ b/libcpu/arm/arm926/stack.c @@ -38,7 +38,7 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, stack_addr += sizeof(rt_uint32_t); stack_addr = (rt_uint8_t *)RT_ALIGN_DOWN((rt_uint32_t)stack_addr, 8); - stk = (rt_uint32_t *)stack_addr; + stk = (rt_uint32_t *)stack_addr; *(--stk) = (rt_uint32_t)tentry; /* entry point */ *(--stk) = (rt_uint32_t)texit; /* lr */ diff --git a/libcpu/arm/arm926/start_gcc.S b/libcpu/arm/arm926/start_gcc.S index fee8e018c..35c1f1722 100644 --- a/libcpu/arm/arm926/start_gcc.S +++ b/libcpu/arm/arm926/start_gcc.S @@ -11,295 +11,320 @@ * 2015-06-04 aozima Align stack address to 8 byte. */ -#include "rt_low_level_init.h" - -#define S_FRAME_SIZE (18*4) //72 - -@#define S_SPSR (17*4) //SPSR -@#define S_CPSR (16*4) //CPSR -#define S_PC (15*4) //R15 -@#define S_LR (14*4) //R14 -@#define S_SP (13*4) //R13 - -@#define S_IP (12*4) //R12 -@#define S_FP (11*4) //R11 -@#define S_R10 (10*4) -@#define S_R9 (9*4) -@#define S_R8 (8*4) -@#define S_R7 (7*4) -@#define S_R6 (6*4) -@#define S_R5 (5*4) -@#define S_R4 (4*4) -@#define S_R3 (3*4) -@#define S_R2 (2*4) -@#define S_R1 (1*4) -@#define S_R0 (0*4) - -#define MODE_SYS 0x1F -#define MODE_FIQ 0x11 -#define MODE_IRQ 0x12 -#define MODE_SVC 0x13 -#define MODE_ABT 0x17 -#define MODE_UND 0x1B -#define MODEMASK 0x1F - -#define NOINT 0xC0 - -@;----------------------- Stack and Heap Definitions --------------------------- - .section .nobss, "w" - - .space UND_STK_SIZE +.equ MODE_USR, 0x10 +.equ MODE_FIQ, 0x11 +.equ MODE_IRQ, 0x12 +.equ MODE_SVC, 0x13 +.equ MODE_ABT, 0x17 +.equ MODE_UND, 0x1B +.equ MODE_SYS, 0x1F +.equ MODEMASK, 0x1F +.equ NOINT, 0xC0 + +.equ I_BIT, 0x80 +.equ F_BIT, 0x40 + +.equ UND_STACK_SIZE, 0x00000100 +.equ SVC_STACK_SIZE, 0x00000100 +.equ ABT_STACK_SIZE, 0x00000100 +.equ FIQ_STACK_SIZE, 0x00000100 +.equ IRQ_STACK_SIZE, 0x00000100 +.equ SYS_STACK_SIZE, 0x00000100 + + /* + *************************************** + * Interrupt vector table + *************************************** + */ +.section .vectors +.code 32 + +.global system_vectors +system_vectors: + ldr pc, _vector_reset + ldr pc, _vector_undef + ldr pc, _vector_swi + ldr pc, _vector_pabt + ldr pc, _vector_dabt + ldr pc, _vector_resv + ldr pc, _vector_irq + ldr pc, _vector_fiq + +_vector_reset: + .word reset +_vector_undef: + .word vector_undef +_vector_swi: + .word vector_swi +_vector_pabt: + .word vector_pabt +_vector_dabt: + .word vector_dabt +_vector_resv: + .word vector_resv +_vector_irq: + .word vector_irq +_vector_fiq: + .word vector_fiq + +.balignl 16,0xdeadbeef + + /* + *************************************** + * Stack and Heap Definitions + *************************************** + */ + .section .data + .space UND_STACK_SIZE .align 3 - .global UND_STACK_START -UND_STACK_START: + .global und_stack_start +und_stack_start: - .space ABT_STK_SIZE + .space ABT_STACK_SIZE .align 3 - .global ABT_STACK_START -ABT_STACK_START: + .global abt_stack_start +abt_stack_start: - .space FIQ_STK_SIZE + .space FIQ_STACK_SIZE .align 3 - .global FIQ_STACK_START -FIQ_STACK_START: + .global fiq_stack_start +fiq_stack_start: - .space IRQ_STK_SIZE + .space IRQ_STACK_SIZE .align 3 - .global IRQ_STACK_START -IRQ_STACK_START: + .global irq_stack_start +irq_stack_start: - .skip SYS_STK_SIZE + .skip SYS_STACK_SIZE .align 3 - .global SYS_STACK_START -SYS_STACK_START: + .global sys_stack_start +sys_stack_start: - .space SVC_STK_SIZE + .space SVC_STACK_SIZE .align 3 - .global SVC_STACK_START -SVC_STACK_START: - -@;--------------Jump vector table----------------------------------------------- - .section .init, "ax" - .arm - - .global start -start: - LDR PC, vector_reset - LDR PC, vector_undef - LDR PC, vector_swi - LDR PC, vector_pabt - LDR PC, vector_dabt - LDR PC, vector_resv - LDR PC, vector_irq - LDR PC, vector_fiq - -vector_reset: - .word Reset_Handler -vector_undef: - .word Undef_Handler -vector_swi: - .word SWI_Handler -vector_pabt: - .word PAbt_Handler -vector_dabt: - .word DAbt_Handler -vector_resv: - .word Resv_Handler -vector_irq: - .word IRQ_Handler -vector_fiq: - .word FIQ_Handler - - .balignl 16,0xdeadbeef - -@;----------------- Reset Handler --------------------------------------------- - .global rt_low_level_init - .global main - .global Reset_Handler -Reset_Handler: - @; Set the cpu to SVC32 mode - MRS R0, CPSR - BIC R0, R0, #MODEMASK - ORR R0, R0, #MODE_SVC|NOINT - MSR CPSR_cxsf, R0 + .global svc_stack_start +svc_stack_start: + +/* + *************************************** + * Startup Code + *************************************** + */ + .section .text + .global reset +reset: + /* Enter svc mode and mask interrupts */ + mrs r0, cpsr + bic r0, r0, #MODEMASK + orr r0, r0, #MODE_SVC|NOINT + msr cpsr_cxsf, r0 + + /* init cpu */ + bl cpu_init_crit + + /* todo:copyself to link address */ + + /* Copy vector to the correct address */ + ldr r0, =system_vectors + mrc p15, 0, r2, c1, c0, 0 + ands r2, r2, #(1 << 13) + ldreq r1, =0x00000000 + ldrne r1, =0xffff0000 + ldmia r0!, {r2-r8, r10} + stmia r1!, {r2-r8, r10} + ldmia r0!, {r2-r8, r10} + stmia r1!, {r2-r8, r10} + + /* turn off the watchdog */ + ldr r0, =0x01C20CB8 + mov r1, #0x0 + str r1, [r0] + + /* mask all IRQs source */ + ldr r1, =0xffffffff + ldr r0, =0x01C20430 + str r1, [r0], #0x04 + str r1, [r0] + + /* Call low level init function */ + ldr sp, =svc_stack_start + ldr r0, =rt_low_level_init + blx r0 + + /* init stack */ + bl stack_setup - @; Set CO-Processor - @; little-end锛宒isbale I/D Cache MMU, vector table is 0x00000000 - MRC P15, 0, R0, C1, C0, 0 @; Read CP15 - LDR R1, =0x00003085 @; set clear bits - BIC R0, R0, R1 - MCR P15, 0, R0, C1, C0, 0 @; Write CP15 - - @; Call low level init function, - @; disable and clear all IRQs, Init MMU, Init interrupt controller, etc. - LDR SP, =SVC_STACK_START - LDR R0, =rt_low_level_init - BLX R0 - -Setup_Stack: - @; Setup Stack for each mode - MRS R0, CPSR - BIC R0, R0, #MODEMASK - - ORR R1, R0, #MODE_UND|NOINT - MSR CPSR_cxsf, R1 @; Undef mode - LDR SP, =UND_STACK_START - - ORR R1, R0, #MODE_ABT|NOINT - MSR CPSR_cxsf, R1 @; Abort mode - LDR SP, =ABT_STACK_START - - ORR R1, R0, #MODE_IRQ|NOINT - MSR CPSR_cxsf, R1 @; IRQ mode - LDR SP, =IRQ_STACK_START - - ORR R1, R0, #MODE_FIQ|NOINT - MSR CPSR_cxsf, R1 @; FIQ mode - LDR SP, =FIQ_STACK_START - - ORR R1, R0, #MODE_SYS|NOINT - MSR CPSR_cxsf,R1 @; SYS/User mode - LDR SP, =SYS_STACK_START - - ORR R1, R0, #MODE_SVC|NOINT - MSR CPSR_cxsf, R1 @; SVC mode - LDR SP, =SVC_STACK_START - - @; clear .bss - MOV R0, #0 @; get a zero - LDR R1, =__bss_start__ @; bss start - LDR R2, =__bss_end__ @; bss end + /* clear bss */ + mov r0, #0 + ldr r1, =__bss_start + ldr r2, =__bss_end bss_clear_loop: - CMP R1, R2 @; check if data to clear - STRLO R0, [R1], #4 @; clear 4 bytes - BLO bss_clear_loop @; loop until done - - @; call C++ constructors of global objects - LDR R0, =__ctors_start__ - LDR R1, =__ctors_end__ + cmp r1, r2 + strlo r0, [r1], #4 + blo bss_clear_loop + + /* call c++ constructors of global objects */ + /* + ldr r0, =__ctors_start__ + ldr r1, =__ctors_end__ ctor_loop: - CMP R0, R1 - BEQ ctor_end - LDR R2, [R0], #4 - STMFD SP!, {R0-R1} - MOV LR, PC - BX R2 - LDMFD SP!, {R0-R1} - B ctor_loop + cmp r0, r1 + beq ctor_end + ldr r2, [r0], #4 + stmfd sp!, {r0-r1} + mov lr, pc + bx r2 + ldmfd sp!, {r0-r1} + b ctor_loop ctor_end: + */ + /* start RT-Thread Kernel */ + ldr pc, _rtthread_startup +_rtthread_startup: + .word rtthread_startup + + + +cpu_init_crit: + /* invalidate I/D caches */ + mov r0, #0 + mcr p15, 0, r0, c7, c7, 0 + mcr p15, 0, r0, c8, c7, 0 + + /* disable MMU stuff and caches */ + mrc p15, 0, r0, c1, c0, 0 + bic r0, r0, #0x00002300 + bic r0, r0, #0x00000087 + orr r0, r0, #0x00000002 + orr r0, r0, #0x00001000 + mcr p15, 0, r0, c1, c0, 0 + + bx lr + +stack_setup: + /* Setup Stack for each mode */ + mrs r0, cpsr + bic r0, r0, #MODEMASK + + orr r1, r0, #MODE_UND|NOINT + msr cpsr_cxsf, r1 + ldr sp, =und_stack_start + + orr r1, r0, #MODE_ABT|NOINT + msr cpsr_cxsf, r1 + ldr sp, =abt_stack_start + + orr r1, r0, #MODE_IRQ|NOINT + msr cpsr_cxsf, r1 + ldr sp, =irq_stack_start + + orr r1, r0, #MODE_FIQ|NOINT + msr cpsr_cxsf, r1 + ldr sp, =fiq_stack_start + + orr r1, r0, #MODE_SYS|NOINT + msr cpsr_cxsf,r1 + ldr sp, =sys_stack_start + + orr r1, r0, #MODE_SVC|NOINT + msr cpsr_cxsf, r1 + ldr sp, =svc_stack_start + + bx lr + +/* + *************************************** + * exception handlers + *************************************** + */ + /* Interrupt */ +vector_fiq: + stmfd sp!,{r0-r7,lr} + bl rt_hw_trap_fiq + ldmfd sp!,{r0-r7,lr} + subs pc, lr, #4 - @; Enter the C code - LDR R0, =rtthread_startup - BLX R0 - -@;----------------- Exception Handler ----------------------------------------- - .global rt_hw_trap_udef - .global rt_hw_trap_swi - .global rt_hw_trap_pabt - .global rt_hw_trap_dabt - .global rt_hw_trap_resv - .global rt_hw_trap_irq - .global rt_hw_trap_fiq - - .global rt_interrupt_enter - .global rt_interrupt_leave - .global rt_thread_switch_interrupt_flag - .global rt_interrupt_from_thread - .global rt_interrupt_to_thread - - .align 5 -Undef_Handler: - SUB SP, SP, #S_FRAME_SIZE - STMIA SP, {R0 - R12} @; Calling R0-R12 - ADD R8, SP, #S_PC - STMDB R8, {SP, LR} @; Calling SP, LR - STR LR, [R8, #0] @; Save calling PC - MRS R6, SPSR - STR R6, [R8, #4] @; Save CPSR - STR R0, [R8, #8] @; Save SPSR - MOV R0, SP - BL rt_hw_trap_udef - - .align 5 -SWI_Handler: - BL rt_hw_trap_swi - - .align 5 -PAbt_Handler: - BL rt_hw_trap_pabt - - .align 5 -DAbt_Handler: - SUB SP, SP, #S_FRAME_SIZE - STMIA SP, {R0 - R12} @; Calling R0-R12 - ADD R8, SP, #S_PC - STMDB R8, {SP, LR} @; Calling SP, LR - STR LR, [R8, #0] @; Save calling PC - MRS R6, SPSR - STR R6, [R8, #4] @; Save CPSR - STR R0, [R8, #8] @; Save SPSR - MOV R0, SP - BL rt_hw_trap_dabt - - .align 5 -Resv_Handler: - BL rt_hw_trap_resv - - .align 5 -FIQ_Handler: - STMFD SP!, {R0-R7,LR} - BL rt_hw_trap_fiq - LDMFD SP!, {R0-R7,LR} - SUBS PC, LR, #4 - - .align 5 -IRQ_Handler: - STMFD SP!, {R0-R12,LR} - 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 - -@;------ void rt_hw_context_switch_interrupt_do(rt_base_t flag) ----------------- -rt_hw_context_switch_interrupt_do: - MOV R1, #0 @; Clear flag - STR R1, [R0] @; Save to flag variable +vector_irq: + stmfd sp!, {r0-r12,lr} + + bl rt_interrupt_enter + bl rt_hw_trap_irq + bl rt_interrupt_leave - LDMFD SP!, {R0-R12,LR} @; Reload saved registers - STMFD SP, {R0-R2} @; Save R0-R2 - SUB R1, SP, #4*3 @; Save old task's SP to R1 - SUB R2, LR, #4 @; Save old task's PC to R2 + ldr r0, =rt_thread_switch_interrupt_flag + ldr r1, [r0] + cmp r1, #1 + beq rt_hw_context_switch_interrupt_do - MRS R0, SPSR @; Get CPSR of interrupt thread + ldmfd sp!, {r0-r12,lr} + subs pc, lr, #4 - MSR CPSR_c, #MODE_SVC|NOINT @; Switch to SVC mode and no interrupt +rt_hw_context_switch_interrupt_do: + mov r1, #0 + str r1, [r0] + + mov r1, sp + add sp, sp, #4*4 + ldmfd sp!, {r4-r12,lr} + mrs r0, spsr + sub r2, lr, #4 + + msr cpsr_c, #I_BIT|F_BIT|MODE_SVC + + stmfd sp!, {r2} + stmfd sp!, {r4-r12,lr} + ldmfd r1, {r1-r4} + stmfd sp!, {r1-r4} + stmfd sp!, {r0} + + ldr r4, =rt_interrupt_from_thread + ldr r5, [r4] + str sp, [r5] + + ldr r6, =rt_interrupt_to_thread + ldr r6, [r6] + ldr sp, [r6] + + ldmfd sp!, {r4} + msr spsr_cxsf, r4 + + ldmfd sp!, {r0-r12,lr,pc}^ + + /* Exception */ +.macro push_svc_reg + sub sp, sp, #17 * 4 + stmia sp, {r0 - r12} + mov r0, sp + mrs r6, spsr + str lr, [r0, #15*4] + str r6, [r0, #16*4] + str sp, [r0, #13*4] + str lr, [r0, #14*4] +.endm - STMFD SP!, {R2} @; Push old task's PC - STMFD SP!, {R3-R12,LR} @; Push old task's LR,R12-R3 - LDMFD R1, {R1-R3} - STMFD SP!, {R1-R3} @; Push old task's R2-R0 - STMFD SP!, {R0} @; Push old task's CPSR +vector_swi: + push_svc_reg + bl rt_hw_trap_swi + b . - LDR R4, =rt_interrupt_from_thread - LDR R5, [R4] @; R5 = stack ptr in old tasks's TCB - STR SP, [R5] @; Store SP in preempted tasks's TCB +vector_undef: + push_svc_reg + bl rt_hw_trap_udef + b . - LDR R6, =rt_interrupt_to_thread - LDR R6, [R6] @; R6 = stack ptr in new tasks's TCB - LDR SP, [R6] @; Get new task's stack pointer +vector_pabt: + push_svc_reg + bl rt_hw_trap_pabt + b . - LDMFD SP!, {R4} @; Pop new task's SPSR - MSR SPSR_cxsf, R4 +vector_dabt: + push_svc_reg + bl rt_hw_trap_dabt + b . - LDMFD SP!, {R0-R12,LR,PC}^ @; pop new task's R0-R12,LR & PC SPSR 2 CPSR +vector_resv: + push_svc_reg + bl rt_hw_trap_resv + b . diff --git a/libcpu/arm/arm926/trap.c b/libcpu/arm/arm926/trap.c index 054cc2741..de04c3f8c 100644 --- a/libcpu/arm/arm926/trap.c +++ b/libcpu/arm/arm926/trap.c @@ -41,14 +41,18 @@ struct rt_hw_register rt_uint32_t cpsr; rt_uint32_t ORIG_r0; }; - +static rt_err_t (*rt_exception_hook)(void *context) = RT_NULL; +void rt_hw_exception_install(rt_err_t (*exception_handle)(void *context)) +{ + rt_exception_hook = exception_handle; +} /** * this function will show registers of CPU * * @param regs the registers point */ -void rt_hw_show_register (struct rt_hw_register *regs) +void rt_hw_show_register(struct rt_hw_register *regs) { rt_kprintf("Execption:\n"); rt_kprintf("r00:0x%08x r01:0x%08x r02:0x%08x r03:0x%08x\n", @@ -74,6 +78,13 @@ void rt_hw_show_register (struct rt_hw_register *regs) */ void rt_hw_trap_udef(struct rt_hw_register *regs) { + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; + + result = rt_exception_hook(regs); + if (result == RT_EOK) return; + } rt_hw_show_register(regs); rt_kprintf("undefined instruction\n"); @@ -96,6 +107,13 @@ void rt_hw_trap_udef(struct rt_hw_register *regs) */ void rt_hw_trap_swi(struct rt_hw_register *regs) { + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; + + result = rt_exception_hook(regs); + if (result == RT_EOK) return; + } rt_hw_show_register(regs); rt_kprintf("software interrupt\n"); @@ -112,6 +130,13 @@ void rt_hw_trap_swi(struct rt_hw_register *regs) */ void rt_hw_trap_pabt(struct rt_hw_register *regs) { + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; + + result = rt_exception_hook(regs); + if (result == RT_EOK) return; + } rt_hw_show_register(regs); rt_kprintf("prefetch abort\n"); @@ -133,6 +158,13 @@ void rt_hw_trap_pabt(struct rt_hw_register *regs) */ void rt_hw_trap_dabt(struct rt_hw_register *regs) { + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; + + result = rt_exception_hook(regs); + if (result == RT_EOK) return; + } rt_hw_show_register(regs); rt_kprintf("data abort\n"); @@ -153,55 +185,26 @@ void rt_hw_trap_dabt(struct rt_hw_register *regs) */ void rt_hw_trap_resv(struct rt_hw_register *regs) { + if (rt_exception_hook != RT_NULL) + { + rt_err_t result; + + result = rt_exception_hook(regs); + if (result == RT_EOK) return; + } rt_kprintf("not used\n"); rt_hw_show_register(regs); rt_hw_cpu_shutdown(); } -extern struct rt_irq_desc irq_desc[]; -extern rt_uint32_t rt_hw_interrupt_get_active(rt_uint32_t fiq_irq); -extern void rt_hw_interrupt_ack(rt_uint32_t fiq_irq, rt_uint32_t id); - -void rt_hw_trap_irq() -{ - rt_isr_handler_t isr_func; - rt_uint32_t irq; - void *param; - - /* get irq number */ - irq = rt_hw_interrupt_get_active(INT_IRQ); +extern void rt_interrupt_dispatch(void); - /* get interrupt service routine */ - isr_func = irq_desc[irq].handler; - param = irq_desc[irq].param; - - /* turn to interrupt service routine */ - isr_func(irq, param); - - rt_hw_interrupt_ack(INT_IRQ, irq); -#ifdef RT_USING_INTERRUPT_INFO - irq_desc[irq].counter ++; -#endif +void rt_hw_trap_irq(void) +{ + rt_interrupt_dispatch(); } -void rt_hw_trap_fiq() +void rt_hw_trap_fiq(void) { - rt_isr_handler_t isr_func; - rt_uint32_t irq; - void *param; - - /* get irq number */ - irq = rt_hw_interrupt_get_active(INT_FIQ); - - /* get interrupt service routine */ - isr_func = irq_desc[irq].handler; - param = irq_desc[irq].param; - - /* turn to interrupt service routine */ - isr_func(irq, param); - - rt_hw_interrupt_ack(INT_FIQ, irq); -#ifdef RT_USING_INTERRUPT_INFO - irq_desc[irq].counter ++; -#endif + rt_interrupt_dispatch(); } -- GitLab