diff --git a/at91_init0.s b/at91_init0.s index b2ced47..7bd3a5e 100644 --- a/at91_init0.s +++ b/at91_init0.s @@ -16,13 +16,10 @@ * Free Software Foundation, Inc., * * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * ***************************************************************************/ - .equ AIC_IVR, (256) - .equ AIC_FVR, (260) - .equ AIC_EOICR, (304) .equ AT91C_BASE_AIC, (0xFFFFF000) .equ IRQ_Stack_Size, (3 * 8 * 4) - .equ FIQ_Stack_Size, (3 * 8 * 4) + .equ FIQ_Stack_Size, (0 * 8 * 4) .equ ABT_Stack_Size, 192 .equ ARM_MODE_FIQ, 0x11 @@ -46,15 +43,19 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */ irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ + .extern ABT_Handler + .extern IRQ_Handler + .extern FIQ_Handler + /* 0x80000000 will result in Prefetch Abort */ .word InitReset .word 0x80000000 .word 0x80000000 - .word ABT_Handler_Entry - .word ABT_Handler_Entry + .word ABT_Handler + .word ABT_Handler .word 0x80000000 - .word IRQ_Handler_Entry - .word FIQ_Handler_Entry + .word IRQ_Handler + .word FIQ_Handler .endfunc @@ -74,7 +75,7 @@ InitReset: mov sp, r0 sub r0, r0, #FIQ_Stack_Size - /* store AIC Base in ARM_MODE_FIQ:r8 for faster access */ + /* store AIC Base in r8_fiq for faster access */ ldr r8, =AT91C_BASE_AIC /* Setup IRQ Mode Stack */ @@ -120,122 +121,3 @@ LoopZI: cmp r1, r2 /* exit dummy for newlib */ exit: b . .endfunc - - .global ABT_Handler_Entry - .func ABT_Handler_Entry -ABT_Handler_Entry: - /* disable interrupts (F_BIT not set on entry) */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store all registers */ - stmfd sp!, { r0-r12 } - - /* saved cpsr (from aborted mode) */ - mrs r0, SPSR - - /* address of abort (pc) */ - mov r3, lr - - /* enter previous mode and get lr(r14), sp(r13) */ - /* TODO: interrupts might be enabled? */ - /* TODO: thumb mode enabled? */ - msr CPSR_c, r0 - mov r1, sp - mov r2, lr - - /* return to abort mode */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store remaining registers (r1-r3 == r13-r15) */ - stmfd sp!, { r1-r3 } - mov r1, sp - - /* execute C Handler (cpsr, registers) */ - ldr r5, =at91_abt_handler - mov lr, pc - bx r5 - b . - .endfunc - - .global FIQ_Handler_Entry - .func FIQ_Handler_Entry -FIQ_Handler_Entry: - /* Save r0 to ARM_MODE_FIQ:r9 */ - mov r9, r0 - - /* get FIQ Vector from AIC and thus clear FIQ */ - ldr r0, [r8, #AIC_FVR] - - /* Switch to ARM_MODE_SVC and save registers there */ - msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT - stmfd sp!, { r1-r3, r12, lr } - - /* execute FIQ in SVC_MODE */ - mov r14, pc - bx r0 - - /* restore registers and switch back to ARM_MODE_FIQ */ - ldmia sp!, { r1-r3, r12, lr } - msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT - - /* restore the ARM_MODE_SVC:r0 */ - mov r0, r9 - - /* restore PC using the LR_fiq directly */ - subs pc, lr, #4 - .endfunc - - .global IRQ_Handler_Entry - .func IRQ_Handler_Entry -IRQ_Handler_Entry: - /* Manage Exception Entry */ - /* Adjust and save LR_irq in IRQ stack */ - sub lr, lr, #4 - stmfd sp!, { lr } - - /* Save SPSR need to be saved for nested interrupt */ - mrs r14, SPSR - stmfd sp!, { r14 } - - /* Save and r0 in IRQ stack */ - stmfd sp!, { r0 } - - /* Write in the IVR to support Protect Mode */ - /* No effect in Normal Mode */ - /* De-assert the NIRQ and clear the source in Protect Mode */ - ldr r14, =AT91C_BASE_AIC - ldr r0, [r14, #AIC_IVR] - str r14, [r14, #AIC_IVR] - - /* Enable Interrupt and Switch in Supervisor Mode */ - msr CPSR_c, #ARM_MODE_SVC - - /* Save scratch/used registers and LR in User Stack */ - stmfd sp!, { r1-r3, r12, r14 } - - /* Branch to the routine pointed by the AIC_IVR */ - mov r14, pc - bx r0 - - /* Restore scratch/used registers and LR from User Stack */ - ldmia sp!, { r1-r3, r12, r14 } - - /* Disable Interrupt and switch back in IRQ mode */ - msr CPSR_c, #ARM_MODE_IRQ | I_BIT - - /* Mark the End of Interrupt on the AIC */ - ldr r14, =AT91C_BASE_AIC - str r14, [r14, #AIC_EOICR] - - /* Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, { r0 } - - /* Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, { r14 } - msr SPSR_cxsf, r14 - - /* Restore adjusted LR_irq from IRQ stack directly in the PC */ - ldmia sp!, { pc }^ - - .endfunc - .end diff --git a/at91_init1.c b/at91_init1.c index 32e5f72..cf07a06 100644 --- a/at91_init1.c +++ b/at91_init1.c @@ -31,33 +31,33 @@ void at91_init1(void) { /* disable watchdog */ *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS; - + /* enable user reset */ *AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN; /* Set Flash Waitstates */ *AT91C_MC_FMR = AT91C_MC_FWS_1FWS; - - /* + + /* * Enable main oscillator (MAINCK) * startup time: 8*6/32768 -> 1.46ms */ AT91S_PMC *pmc = AT91C_BASE_PMC; - pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | + pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | AT91C_CKGR_MOSCEN; while (!(pmc->PMC_SR & AT91C_PMC_MOSCS)); - /* + /* * PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124 * startup time: 32/32768 -> 976us - */ - pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | - (AT91C_CKGR_MUL & (124<<16)) | + */ + pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | + (AT91C_CKGR_MUL & (124<<16)) | (AT91C_CKGR_PLLCOUNT & (32<<8)) ; while (!(pmc->PMC_SR & AT91C_PMC_LOCK)); /* MCK = PLLCK / 2 = 48MHz */ - pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | + pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | AT91C_PMC_PRES_CLK_2; while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY)); @@ -79,3 +79,96 @@ void at91_init1(void) } aic->AIC_SPU = (uint32_t)empty_isr; } + +__attribute__((naked)) void IRQ_Handler(void) +{ + asm volatile ( + ".equ ARM_MODE_IRQ, 0x12 \n\t" + ".equ ARM_MODE_SVC, 0x13 \n\t" + ".equ I_BIT, 0x80 \n\t" + + ".equ AIC_IVR, (256) \n\t" + ".equ AIC_EOICR, (304) \n\t" + ".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t" + + /* Adjust and save lr_irq on IRQ stack */ + "sub lr, lr, #4 \n\t" + "stmfd sp!, { lr } \n\t" + + /* Save SPSR (for nested interrupts) */ + "mrs r14, SPSR \n\t" + "stmfd sp!, { r14 } \n\t" + + /* Save r0 on IRQ stack */ + "stmfd sp!, { r0 } \n\t" + + /* Write in the IVR to support Protect Mode */ + "ldr r14, =AT91C_BASE_AIC \n\t" + "ldr r0, [r14, #AIC_IVR] \n\t" + "str r14, [r14, #AIC_IVR] \n\t" + + /* Enable Interrupt and switch to SVC mode */ + "msr CPSR_c, #ARM_MODE_SVC \n\t" + + /* Save scratch/used registers and lr on SVC Stack */ + "stmfd sp!, { r1-r3, r12, r14 } \n\t" + + /* Branch to the routine pointed by the AIC_IVR */ + "mov r14, pc \n\t" + "bx r0 \n\t" + + /* Restore scratch/used registers and lr from SVC Stack */ + "ldmia sp!, { r1-r3, r12, r14 } \n\t" + + /* Disable Interrupt and switch back to IRQ mode */ + "msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t" + + /* Mark the End of Interrupt on the AIC */ + "ldr r14, =AT91C_BASE_AIC \n\t" + "str r14, [r14, #AIC_EOICR] \n\t" + + /* Restore SPSR_irq and r0 from IRQ stack */ + "ldmia sp!, { r0 } \n\t" + "ldmia sp!, { r14 } \n\t" + "msr SPSR_cxsf, r14 \n\t" + + /* Restore adjusted lr_irq from IRQ stack */ + "ldmia sp!, { pc }^ \n\t" + ); +} + +__attribute__((naked)) void FIQ_Handler(void) +{ + asm volatile ( + ".equ ARM_MODE_FIQ, 0x11 \n\t" + ".equ ARM_MODE_SVC, 0x13 \n\t" + ".equ I_BIT, 0x80 \n\t" + ".equ F_BIT, 0x40 \n\t" + + ".equ AIC_FVR, (260) \n\t" + + /* Save r0 to r9_fiq */ + "mov r9, r0 \n\t" + + /* get FIQ Vector from AIC and thus clear FIQ */ + "ldr r0, [r8, #AIC_FVR] \n\t" + + /* Switch to SVC and save registers there */ + "msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT \n\t" + "stmfd sp!, { r1-r3, r12, lr } \n\t" + + /* execute FIQ in SVC_MODE */ + "mov r14, pc \n\t" + "bx r0 \n\t" + + /* restore registers and switch back to FIQ */ + "ldmia sp!, { r1-r3, r12, lr } \n\t" + "msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT \n\t" + + /* restore the r0 from r9_fiq */ + "mov r0, r9 \n\t" + + /* restore PC using the lr_fiq directly */ + "subs pc, lr, #4 \n\t" + ); +} diff --git a/src/at91_exceptions.c b/src/at91_exceptions.c index 4b7517f..cd87668 100644 --- a/src/at91_exceptions.c +++ b/src/at91_exceptions.c @@ -39,7 +39,7 @@ static void dbgu_puts(const char *p) static void dbgu_hexvar(const char *name, uint32_t var) { dbgu_puts(name); - + char buf[11]; buf[0] = '0'; buf[1] = 'x'; @@ -47,7 +47,7 @@ static void dbgu_hexvar(const char *name, uint32_t var) for (i = 9; i > 1; i--) { buf[i] = (var & 0x0F); buf[i] += (buf[i] < 0xa) ? '0' : ('a' - 0x0a); - var >>= 4; + var >>= 4; } buf[10] = '\0'; dbgu_puts(buf); @@ -59,8 +59,8 @@ ARM7TDMI 32bit Undefined Address Instruction Fetch Abort --- Registerdump (cpsr:0xa0000013 - SVC): r0:0x00000002 r1:0x0000000c r2:0x00200038 r3:0x80000000 r4:0x0020fffc r5:0x00101788 r6:0x00000180 r7:0x41000000 - r8:0x00008400 r9:0x00100004 r10:0x03000294 fp:0x0020fe84 -r12:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004 + r8:0x00008400 r9:0x00100004 sl:0x03000294 fp:0x0020fe84 + ip:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004 --- Stackdump: 0x0020fe70: 0x0020fe7c 0x00101648 0x001015a8 0x0020feaf 0x0020fe80: 0x0020fec0 0x0020fe90 0x00101780 0x00101620 @@ -96,17 +96,17 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers) dbgu_puts("32bit "); break; } - + if (asr & AT91C_MC_UNDADD) dbgu_puts("Undefined Address "); else if (asr & AT91C_MC_MISADD) dbgu_puts("Misaliged Address "); - + switch (asr & AT91C_MC_ABTTYP) { case AT91C_MC_ABTTYP_DATAR: dbgu_puts("Data Read "); break; - + case AT91C_MC_ABTTYP_DATAW: dbgu_puts("Data Write "); break; @@ -115,7 +115,7 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers) dbgu_puts("Prefetch "); break; } - + dbgu_hexvar("Abort\n(ASR:", asr); dbgu_hexvar(" AASR:", *AT91C_MC_AASR); dbgu_puts(")\n"); @@ -142,9 +142,9 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers) dbgu_hexvar(" r7:", registers[10]); dbgu_hexvar("\n r8:", registers[11]); dbgu_hexvar(" r9:", registers[12]); - dbgu_hexvar(" r10:", registers[13]); + dbgu_hexvar(" sl:", registers[13]); dbgu_hexvar(" fp:", registers[14]); - dbgu_hexvar("\nr12:", registers[15]); + dbgu_hexvar("\n ip:", registers[15]); dbgu_hexvar(" sp:", registers[0]); dbgu_hexvar(" lr:", registers[1]); dbgu_hexvar(" pc:", registers[2]); @@ -160,3 +160,44 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers) } dbgu_putchar('\n'); } + +__attribute__((naked)) void ABT_Handler(void) +{ + asm volatile ( + ".equ ARM_MODE_ABT, 0x17 \n\t" + ".equ I_BIT, 0x80 \n\t" + ".equ F_BIT, 0x40 \n\t" + + /* disable interrupts (F_BIT not set on entry) */ + "msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t" + + /* store all registers */ + "stmfd sp!, { r0-r12 } \n\t" + + /* saved cpsr (from aborted mode) */ + "mrs r0, SPSR \n\t" + + /* address of abort (pc) */ + "mov r3, lr \n\t" + + /* enter previous mode and get lr(r14), sp(r13) */ + /* TODO: interrupts might be enabled? */ + /* TODO: thumb mode enabled? */ + "msr CPSR_c, r0 \n\t" + "mov r1, sp \n\t" + "mov r2, lr \n\t" + + /* return to abort mode */ + "msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t" + + /* store remaining registers (r1-r3 == r13-r15) */ + "stmfd sp!, { r1-r3 } \n\t" + "mov r1, sp \n\t" + + /* execute C Handler (cpsr, registers) */ + "ldr r5, =at91_abt_handler \n\t" + "mov lr, pc \n\t" + "bx r5 \n\t" + "b . \n\t" + ); +}