Browse Source

move exception handler into C-files

master
Olaf Rempel 12 years ago
parent
commit
773cc44a09
3 changed files with 163 additions and 147 deletions
  1. +10
    -128
      at91_init0.s
  2. +102
    -9
      at91_init1.c
  3. +51
    -10
      src/at91_exceptions.c

+ 10
- 128
at91_init0.s View File

@@ -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

+ 102
- 9
at91_init1.c View File

@@ -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"
);
}

+ 51
- 10
src/at91_exceptions.c View File

@@ -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"
);
}

Loading…
Cancel
Save