From f172769a2dffd0135a28d3fb85c176f15d116683 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 1 Mar 2007 18:51:09 +0000 Subject: Major borking in progress, to reorganize the bootstrap code after enlightenment by lejos' way of doing things. --- estorm/Makefile | 7 +- estorm/crt0.h | 6 ++ estorm/crt0/crt0.h | 6 -- estorm/crt0/crt0_c.c | 166 ------------------------------- estorm/crt0/crt0_s.S | 274 --------------------------------------------------- estorm/crt0_c.c | 166 +++++++++++++++++++++++++++++++ estorm/crt0_s.S | 274 +++++++++++++++++++++++++++++++++++++++++++++++++++ estorm/vectors.s | 27 +++++ 8 files changed, 477 insertions(+), 449 deletions(-) create mode 100644 estorm/crt0.h delete mode 100644 estorm/crt0/crt0.h delete mode 100644 estorm/crt0/crt0_c.c delete mode 100644 estorm/crt0/crt0_s.S create mode 100644 estorm/crt0_c.c create mode 100644 estorm/crt0_s.S create mode 100644 estorm/vectors.s diff --git a/estorm/Makefile b/estorm/Makefile index 5193c38..e66e6dd 100644 --- a/estorm/Makefile +++ b/estorm/Makefile @@ -1,7 +1,8 @@ -TOOL_PREFIX=arm-elf- -TOOL_SUFFIX=-4.1.1 +include environment.mak -S_SOURCES = +TARGET = nxtos + +S_SOURCES = crt0_s.S vectors.s C_SOURCES = main.c aic.c sys_timer.c S_OBJECTS = $(S_SOURCES:.S=.o) diff --git a/estorm/crt0.h b/estorm/crt0.h new file mode 100644 index 0000000..b5ca7fa --- /dev/null +++ b/estorm/crt0.h @@ -0,0 +1,6 @@ +/* + * Helper functions installed by the bootstrap code. + */ + +extern void interrupts_disable(); +extern void interrupts_enable(); diff --git a/estorm/crt0/crt0.h b/estorm/crt0/crt0.h deleted file mode 100644 index b5ca7fa..0000000 --- a/estorm/crt0/crt0.h +++ /dev/null @@ -1,6 +0,0 @@ -/* - * Helper functions installed by the bootstrap code. - */ - -extern void interrupts_disable(); -extern void interrupts_enable(); diff --git a/estorm/crt0/crt0_c.c b/estorm/crt0/crt0_c.c deleted file mode 100644 index eef515c..0000000 --- a/estorm/crt0/crt0_c.c +++ /dev/null @@ -1,166 +0,0 @@ -/* NXT bootup code */ - -#include "at91sam7s256.h" - -extern void nxt_default_irq_handler(void); - -/* Boot the board's main oscillator and step the CPU up to 48MHz. */ -static inline void init_clocks() -{ - /* Enable the internal main oscillator. - * - * The oscillator must be given a quartz-dependent time to start - * up. For the NXT, this must be set to 6 cycles of the slow clock, - * which at this stage controls the board. - */ - *AT91C_CKGR_MOR = AT91C_CKGR_MOSCEN | (6 << 8); - - /* Wait for the oscillator to stabilize. */ - while ((*AT91C_PMC_SR & AT91C_PMC_MOSCS) == 0); - - /* Initialize the PLL clock. - * - * This clock will later provide the basis for the main board - * clock. The quartz on the board runs at 18.432MHz, and we want a - * main clock running at 48MHz. The best we can do is to set the PLL - * to run at 96MHz, and then clock back down when configuring the - * main clock. - * - * To do so, we divide the input signal by 14, then multiply the - * output by 73 (72+1, as the datasheet says that the board adds 1 - * to the value written in the register), which clocks us to - * 96.11MHz. Note that the USB clock requires an input signal of - * 96MHz +/- 0.25%. A frequency of 96.11MHz is a deviation of .11%, - * therefore acceptable. - * - * We also configure the USB clock divider in the same register - * write, as the divider is in the same register. - * - * The PLL clock is estimated to lock within 0.844ms (estimate - * documented in the LEGO source code), which maps to ~28 slow clock - * cycles. - */ - *AT91C_CKGR_PLLR = (14 | (28 << 8) | (72 << 16) | AT91C_CKGR_USBDIV_1); - - /* Wait for the PLL to lock. */ - while ((*AT91C_PMC_SR & AT91C_PMC_LOCK) == 0); - - /* Wait for master clock ready before fiddling with it, just as a - precaution. It should be running fine at this stage. */ - while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); - - /* Set the master clock prescaler to divide by two, which sets the - * master clock to 16KHz (half the slow clock frequency). - * - * Note that we stay on the slow clock, because of the procedure - * explained in the specification: you must first set the prescaler, - * wait for the newly scaled clock to stabilize, then switch to a - * new clock source in a separate operation. - */ - *AT91C_PMC_MCKR = AT91C_PMC_CSS_SLOW_CLK | AT91C_PMC_PRES_CLK_2; - - /* Wait for the master clock to stabilize. */ - while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); - - /* Switch the master clock source over to the PLL clock. */ - *AT91C_PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | AT91C_PMC_PRES_CLK_2; - - /* Wait for the master clock to stabilize at 48MHz. */ - while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); - - /* TODO enable USB clocks */ -} - - -/* - * Initialize the Advanced Interrupt Controller with stub handlers and - * all interrupts disabled. - */ -static inline void init_aic() { - int i; - - /* Do some cleanup on the AIC. All these are to protect against the - * case where we are coming from a warm boot. These values define - * the modes in which the AIC should be booting up. - * - * - Disable all peripheral interrupt lines. - * - Turn off Fast Forcing for all peripheral interrupt lines. - * - Clear all pending interrupts. - * - Tell the AIC that it is no longer handling any interrupt. - */ - AT91C_BASE_AIC->AIC_IDCR = 0xFFFFFFFF; - AT91C_BASE_AIC->AIC_FFDR = 0xFFFFFFFF; - AT91C_BASE_AIC->AIC_ICCR = 0xFFFFFFFF; - AT91C_BASE_AIC->AIC_EOICR = 0xFFFFFFFF; - - /* Since it can be useful for JTAG debugging, enable debug mode in - * the AIC. In this mode, reading the interrupt vector register will - * not unlock the AIC and let it trigger any higher priority - * IRQs. We will have to write back to the vector register to unlock - * the AIC (the low-level IRQ dispatcher routine does this already). - */ - AT91C_BASE_AIC->AIC_DCR = 0x1; - - /* Set up the AIC with the default handlers. The handlers at startup - * are undefined, which will cause undefined behavior if the kernel - * activates an interrupt line before configuring the handler. - */ - AT91C_BASE_AIC->AIC_SVR[0] = (long int) nxt_default_irq_handler; - for (i=1; i<31; i++) { - AT91C_BASE_AIC->AIC_SVR[i] = (long int) nxt_default_irq_handler; - } - AT91C_BASE_AIC->AIC_SPU = (long int) nxt_default_irq_handler; - -} - - -/* Routine called by the low level bootstrapper assembly code to - * perform basic board initialization. - */ -void nxt_low_level_init() { - /* Configure the Flash controller with write speed settings. These - * settings are valid for writing everywhere but the non-volatile bits - * (lock, security, general-purpose NVM). - * - * These values are valid only after the master clock is configured - * to run at 48MHz, ie. after the call to init_clocks. Do NOT write - * to flash before then! - */ - *AT91C_MC_FMR = AT91C_MC_FWS_1FWS | (0x48 << 16); - - /* Disable the watchdog timer for now. The kernel can reenable it - * later if it feels like it. - */ - *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS; - - /* Start the board clocks and step up to 48MHz. */ - init_clocks(); - - /* Initialize the Advanced Interrupt Controller. */ - init_aic(); -} - - -/* - * Helper functions to do data copy and memory initialization. This is - * only used during the initialization, and the code is no longer - * available once the payload boots. - * - * Both the following methods return the address (dst + n_bytes), - * suitable for passing back into the helpers as the new destination - * (for chained copies/inits). - */ - -char *init_memcpy(char *dst, char *src, long int n_bytes) { - while (n_bytes--) - *dst++ = *src++; - - return dst; -} - -char *init_memset(char *dst, char val, long int n_bytes) { - while (n_bytes--) - *dst++ = val; - - return dst; -} diff --git a/estorm/crt0/crt0_s.S b/estorm/crt0/crt0_s.S deleted file mode 100644 index 5617983..0000000 --- a/estorm/crt0/crt0_s.S +++ /dev/null @@ -1,274 +0,0 @@ -.section ".bootstrap" - -/* - * Definition of a few constants used in the interrupt dispatchers. - */ -#define AIC_BASE 0xFFFFF000 /* Base of the AIC register bank. */ -#define AIC_IVR 0x100 /* Offset of the IVR register within the AIC */ -#define AIC_FVR 0x104 /* Offset of the FVR register within the AIC. */ -#define AIC_EOICR 0x130 /* Offset of the EOICR register within the AIC. */ - -#define MC_BASE 0xFFFFFF00 /* Base of the MC register bank. */ -#define MC_REMAP 0x0 /* Offset of the RCR register within the MC. */ - -#define FIQ_MASK 0x6 /* FIQ disable control bit in the CPSR. */ -#define IRQ_MASK 0x7 /* IRQ disable control bit in the CPSR. */ - -#define MODE_FIQ 0x11 /* FIQ mode. */ -#define MODE_IRQ 0x12 /* IRQ mode. */ -#define MODE_SVC 0x13 /* Supervisor mode. */ - -#define RAM_START 0x00200000 /* The first address of RAM. */ -#define RAM_END 0x002FFFFF /* The last address of RAM. */ -#define IRQ_STACK_SIZE 0x60 /* The size of the IRQ mode stack: 3*4*8 bytes. */ - -/* - * The following is the interrupt vector block of the system. - */ -reset: b init_reset /* Reset */ -illegal_hdl: b illegal_hdl /* Illegal instruction */ -swi_hdl: b swi_hdl /* Software interrupt */ -prefetch_hdl: b prefetch_hdl /* Prefetch abort */ -data_hdl: b data_hdl /* Data abort */ -reserved_hdl: b reserved_hdl /* Reserved vector */ -irq_hdl: b irq_dispatch /* IRQ */ -fiq_hdl: /* FIQ */ - - -/* - * This is the FIQ handler. It simply wraps the registered FIQ - * handling routine in the required exception handling code. - */ - - /* Save R0 to an FIQ banked register, replace it with the AIC - * register base address, and switch into supervisor (SVC) mode. - */ - mov r9, r0 - ldr r0, [r8, #AIC_FVR] - msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_SVC) - - /* Now that we have a real stack, save the other registers - * we're responsible for saving (according to the ATPCS), and - * branch to the FIQ handler. - */ - stmfd sp!, {r1-r3, ip, lr} - mov r14, pc - bx r0 - - /* Restore stacked registers, switch back into FIQ mode and - * resume execution. - */ - ldmfd sp!, {r1-r3, ip, lr} - msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_FIQ) - mov r0, r9 - subs pc, lr, #4 - - -/* - * IRQ exception dispatcher. This code runs when the CPU detects an - * interrupt coming from the AIC. It switches to supervisor mode and - * dispatches executions to the registered handler. - */ -irq_dispatch: - - /* Save the return address and return processor mode registers - * on the IRQ stack. This is needed to allow nested interrupts, - * where a higher-priority interrupt halts execution of a lower - * priority interrupt. Also save r0, as we're going to need a - * scratch register. - */ - sub lr, lr, #4 - stmfd sp!, {lr} - mrs lr, SPSR - stmfd sp!, {lr} - stmfd sp!, {r0} - - /* Get the IVR value. */ - ldr lr, =AIC_BASE - ldr r0, [lr, #AIC_IVR] - - /* If we're in Protected mode (usually for JTAG debugging), we - * need to write back to the IVR register to tell the AIC it - * can dispatch other higher priority interrupts again. - * In normal mode, this has no effect, so we can safely do it. - */ - str lr, [lr, #AIC_IVR] - - /* Switch to Supervisor mode and reenable IRQ and FIQ handling. */ - msr CPSR_c, #MODE_SVC - - /* Dispatch the IRQ to the registed handler. */ - stmfd sp!, {r1-r3, ip, lr} - mov lr, pc - bx r0 - - /* Restore registers and switch back to IRQ mode. */ - ldmfd sp!, {r1-r3, ip, lr} - msr CPSR_c, #(IRQ_MASK | MODE_IRQ) - ldmfd sp!, {r0} - - /* Tell the AIC that the interrupt has been handled. */ - str lr, [lr, #AIC_EOICR] - - /* Restore the SPSR register from the IRQ stack. */ - ldmfd sp!, {lr} - msr SPSR_cxsf, lr - - /* Return to the normal execution flow. */ - ldmfd sp!, {pc}^ - - -/* - * Low level service routines that inhibit and enable interrupts in - * the CPU. These are provided as helpers to the OS, as C code cannot - * directly touch the core mode register. - */ - -/* Disable IRQ and FIQ handling in the CPU. */ -interrupts_disable: - /* Get the interrupt counter and increment it. */ - adr r1, interrupts_count - ldr r2, [r1] - adds r2, r2, #1 - str r2, [r1] - - /* If the add didn't cause an overflow, interrupts are already - * disabled. Return immediately. - */ - bxvc lr - - /* Otherwise, disable interrupts. */ - mrs r0, CPSR - orr r0, r0, #(IRQ_MASK | FIQ_MASK) - msr CPSR_c, r0 - bx lr - -/* Enable IRQ and FIQ handling in the CPU. */ -interrupts_enable: - adr r1, interrupts_count - ldr r2, [r1] - subs r2, r2, #1 - str r2, [r1] - - /* If we are still inside a nested interrupt disable, don't - * reenable interrupts. Return immediately. - */ - bxpl lr - - /* Otherwise, reenable interrupts. */ - mrs r0, CPSR - bic r0, r0, #(IRQ_MASK | FIQ_MASK) - msr CPSR_c, r0 - bx lr - -interrupts_count: - /* The byte count starts at 0, which is interrupts disabled with 1 level of nesting. Interrupts enabled is 0xFF. */ - .int 0 - -/* - * Default handler for the interrupt controller. The board is - * initialized with interrupts disabled, and this handler set for all - * interrupts. - */ -nxt_default_irq_handler: - b nxt_default_irq_handler -.global nxt_default_irq_handler - -end: b end - -/* - * This is a marker, used in the payload initialization code to figure - * out where to copy the payload in ram. - */ -payload_start: -.global payload_start - - -/* - * Main initialization. This is where the execution starts when the - * board is powered on. - */ -init_reset: - /* Set up the supervisor mode stack at the top of RAM. */ - ldr sp, =RAM_END - - /* Call the low-level initialization code. Once this has run, - * we're at 48MHz, and the AIC has valid (if useless) exception - * handlers registered. - */ - bl nxt_low_level_init - - /* Switch to FIQ mode and set the r8 register with the AIC - * register base address. - */ - msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_FIQ) - ldr r8, =AIC_BASE - - /* Switch to IRQ mode and set up the IRQ stack at the very top - * of ram. Given our IRQ dispatch code, we need 3 words per - * priority level, so a stack of 3*4*8 bytes. - */ - msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_IRQ) - ldr r0, =RAM_END - mov sp, r0 - sub r0, r0, #IRQ_STACK_SIZE - - /* Go back to Supervisor mode and set up the Supervisor stack - * to start at the top of the IRQ stack. - */ - msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_SVC) - mov sp, r0 - - /* Copy the interrupt vectors and interrupt dispatchers into - * ram. We don't copy the init_reset method, as we're going to - * put the actual application payload there. - */ - ldr r0, =RAM_START - mov r1, #0 - ldr r2, =payload_start - bl init_memcpy - - /* Overwrite the reset vector to point to itself. We don't yet - * support warm reboots, so we should just crash if it's - * attempted. - */ - ldr r1, =RAM_START - str r1, [r1] - - /* Copy the application payload to ram. For this, we use the - * handy symbols that ld provides us, that give us the address in - * flash where the payload is, and the total size of the payload. - * - * We load the payload at the address payload_start, which is - * where ld relocated the addresses to be. - * - * Note that this overwrites the code we're currently in, so - * as soon as we remap the bottom section from flash to ram, the - * crt0 initialization code will be lost. No biggie, we can still - * get to it by hitting flash directly. - */ - ldr r1, =__b_load - ldr r2, =__load_size - bl init_memcpy - - /* Initialize the BSS. Again, we use handy symbols that ld - * defines for us, that specify the size of the BSS section. - */ - mov r1, #0 - ldr r2, =__bss_size - bl init_memset - - /* We're almost ready to boot the kernel. We first set up all - * the registers, getting ready for the remap. Once the remap - * command has executed, we have to banch to load_main before the - * pipeline empties, otherwise we'll fall down into the - * application payload and crash. - */ - ldr lr, =end - ldr ip, =kernel_main - ldr r4, =MC_BASE - mov r5, #1 - - /* Execute the remap, and branch into the kernel. */ - str r5, [r4, #MC_REMAP] - bx ip diff --git a/estorm/crt0_c.c b/estorm/crt0_c.c new file mode 100644 index 0000000..eef515c --- /dev/null +++ b/estorm/crt0_c.c @@ -0,0 +1,166 @@ +/* NXT bootup code */ + +#include "at91sam7s256.h" + +extern void nxt_default_irq_handler(void); + +/* Boot the board's main oscillator and step the CPU up to 48MHz. */ +static inline void init_clocks() +{ + /* Enable the internal main oscillator. + * + * The oscillator must be given a quartz-dependent time to start + * up. For the NXT, this must be set to 6 cycles of the slow clock, + * which at this stage controls the board. + */ + *AT91C_CKGR_MOR = AT91C_CKGR_MOSCEN | (6 << 8); + + /* Wait for the oscillator to stabilize. */ + while ((*AT91C_PMC_SR & AT91C_PMC_MOSCS) == 0); + + /* Initialize the PLL clock. + * + * This clock will later provide the basis for the main board + * clock. The quartz on the board runs at 18.432MHz, and we want a + * main clock running at 48MHz. The best we can do is to set the PLL + * to run at 96MHz, and then clock back down when configuring the + * main clock. + * + * To do so, we divide the input signal by 14, then multiply the + * output by 73 (72+1, as the datasheet says that the board adds 1 + * to the value written in the register), which clocks us to + * 96.11MHz. Note that the USB clock requires an input signal of + * 96MHz +/- 0.25%. A frequency of 96.11MHz is a deviation of .11%, + * therefore acceptable. + * + * We also configure the USB clock divider in the same register + * write, as the divider is in the same register. + * + * The PLL clock is estimated to lock within 0.844ms (estimate + * documented in the LEGO source code), which maps to ~28 slow clock + * cycles. + */ + *AT91C_CKGR_PLLR = (14 | (28 << 8) | (72 << 16) | AT91C_CKGR_USBDIV_1); + + /* Wait for the PLL to lock. */ + while ((*AT91C_PMC_SR & AT91C_PMC_LOCK) == 0); + + /* Wait for master clock ready before fiddling with it, just as a + precaution. It should be running fine at this stage. */ + while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); + + /* Set the master clock prescaler to divide by two, which sets the + * master clock to 16KHz (half the slow clock frequency). + * + * Note that we stay on the slow clock, because of the procedure + * explained in the specification: you must first set the prescaler, + * wait for the newly scaled clock to stabilize, then switch to a + * new clock source in a separate operation. + */ + *AT91C_PMC_MCKR = AT91C_PMC_CSS_SLOW_CLK | AT91C_PMC_PRES_CLK_2; + + /* Wait for the master clock to stabilize. */ + while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); + + /* Switch the master clock source over to the PLL clock. */ + *AT91C_PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | AT91C_PMC_PRES_CLK_2; + + /* Wait for the master clock to stabilize at 48MHz. */ + while ((*AT91C_PMC_SR & AT91C_PMC_MCKRDY) == 0); + + /* TODO enable USB clocks */ +} + + +/* + * Initialize the Advanced Interrupt Controller with stub handlers and + * all interrupts disabled. + */ +static inline void init_aic() { + int i; + + /* Do some cleanup on the AIC. All these are to protect against the + * case where we are coming from a warm boot. These values define + * the modes in which the AIC should be booting up. + * + * - Disable all peripheral interrupt lines. + * - Turn off Fast Forcing for all peripheral interrupt lines. + * - Clear all pending interrupts. + * - Tell the AIC that it is no longer handling any interrupt. + */ + AT91C_BASE_AIC->AIC_IDCR = 0xFFFFFFFF; + AT91C_BASE_AIC->AIC_FFDR = 0xFFFFFFFF; + AT91C_BASE_AIC->AIC_ICCR = 0xFFFFFFFF; + AT91C_BASE_AIC->AIC_EOICR = 0xFFFFFFFF; + + /* Since it can be useful for JTAG debugging, enable debug mode in + * the AIC. In this mode, reading the interrupt vector register will + * not unlock the AIC and let it trigger any higher priority + * IRQs. We will have to write back to the vector register to unlock + * the AIC (the low-level IRQ dispatcher routine does this already). + */ + AT91C_BASE_AIC->AIC_DCR = 0x1; + + /* Set up the AIC with the default handlers. The handlers at startup + * are undefined, which will cause undefined behavior if the kernel + * activates an interrupt line before configuring the handler. + */ + AT91C_BASE_AIC->AIC_SVR[0] = (long int) nxt_default_irq_handler; + for (i=1; i<31; i++) { + AT91C_BASE_AIC->AIC_SVR[i] = (long int) nxt_default_irq_handler; + } + AT91C_BASE_AIC->AIC_SPU = (long int) nxt_default_irq_handler; + +} + + +/* Routine called by the low level bootstrapper assembly code to + * perform basic board initialization. + */ +void nxt_low_level_init() { + /* Configure the Flash controller with write speed settings. These + * settings are valid for writing everywhere but the non-volatile bits + * (lock, security, general-purpose NVM). + * + * These values are valid only after the master clock is configured + * to run at 48MHz, ie. after the call to init_clocks. Do NOT write + * to flash before then! + */ + *AT91C_MC_FMR = AT91C_MC_FWS_1FWS | (0x48 << 16); + + /* Disable the watchdog timer for now. The kernel can reenable it + * later if it feels like it. + */ + *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS; + + /* Start the board clocks and step up to 48MHz. */ + init_clocks(); + + /* Initialize the Advanced Interrupt Controller. */ + init_aic(); +} + + +/* + * Helper functions to do data copy and memory initialization. This is + * only used during the initialization, and the code is no longer + * available once the payload boots. + * + * Both the following methods return the address (dst + n_bytes), + * suitable for passing back into the helpers as the new destination + * (for chained copies/inits). + */ + +char *init_memcpy(char *dst, char *src, long int n_bytes) { + while (n_bytes--) + *dst++ = *src++; + + return dst; +} + +char *init_memset(char *dst, char val, long int n_bytes) { + while (n_bytes--) + *dst++ = val; + + return dst; +} diff --git a/estorm/crt0_s.S b/estorm/crt0_s.S new file mode 100644 index 0000000..5617983 --- /dev/null +++ b/estorm/crt0_s.S @@ -0,0 +1,274 @@ +.section ".bootstrap" + +/* + * Definition of a few constants used in the interrupt dispatchers. + */ +#define AIC_BASE 0xFFFFF000 /* Base of the AIC register bank. */ +#define AIC_IVR 0x100 /* Offset of the IVR register within the AIC */ +#define AIC_FVR 0x104 /* Offset of the FVR register within the AIC. */ +#define AIC_EOICR 0x130 /* Offset of the EOICR register within the AIC. */ + +#define MC_BASE 0xFFFFFF00 /* Base of the MC register bank. */ +#define MC_REMAP 0x0 /* Offset of the RCR register within the MC. */ + +#define FIQ_MASK 0x6 /* FIQ disable control bit in the CPSR. */ +#define IRQ_MASK 0x7 /* IRQ disable control bit in the CPSR. */ + +#define MODE_FIQ 0x11 /* FIQ mode. */ +#define MODE_IRQ 0x12 /* IRQ mode. */ +#define MODE_SVC 0x13 /* Supervisor mode. */ + +#define RAM_START 0x00200000 /* The first address of RAM. */ +#define RAM_END 0x002FFFFF /* The last address of RAM. */ +#define IRQ_STACK_SIZE 0x60 /* The size of the IRQ mode stack: 3*4*8 bytes. */ + +/* + * The following is the interrupt vector block of the system. + */ +reset: b init_reset /* Reset */ +illegal_hdl: b illegal_hdl /* Illegal instruction */ +swi_hdl: b swi_hdl /* Software interrupt */ +prefetch_hdl: b prefetch_hdl /* Prefetch abort */ +data_hdl: b data_hdl /* Data abort */ +reserved_hdl: b reserved_hdl /* Reserved vector */ +irq_hdl: b irq_dispatch /* IRQ */ +fiq_hdl: /* FIQ */ + + +/* + * This is the FIQ handler. It simply wraps the registered FIQ + * handling routine in the required exception handling code. + */ + + /* Save R0 to an FIQ banked register, replace it with the AIC + * register base address, and switch into supervisor (SVC) mode. + */ + mov r9, r0 + ldr r0, [r8, #AIC_FVR] + msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_SVC) + + /* Now that we have a real stack, save the other registers + * we're responsible for saving (according to the ATPCS), and + * branch to the FIQ handler. + */ + stmfd sp!, {r1-r3, ip, lr} + mov r14, pc + bx r0 + + /* Restore stacked registers, switch back into FIQ mode and + * resume execution. + */ + ldmfd sp!, {r1-r3, ip, lr} + msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_FIQ) + mov r0, r9 + subs pc, lr, #4 + + +/* + * IRQ exception dispatcher. This code runs when the CPU detects an + * interrupt coming from the AIC. It switches to supervisor mode and + * dispatches executions to the registered handler. + */ +irq_dispatch: + + /* Save the return address and return processor mode registers + * on the IRQ stack. This is needed to allow nested interrupts, + * where a higher-priority interrupt halts execution of a lower + * priority interrupt. Also save r0, as we're going to need a + * scratch register. + */ + sub lr, lr, #4 + stmfd sp!, {lr} + mrs lr, SPSR + stmfd sp!, {lr} + stmfd sp!, {r0} + + /* Get the IVR value. */ + ldr lr, =AIC_BASE + ldr r0, [lr, #AIC_IVR] + + /* If we're in Protected mode (usually for JTAG debugging), we + * need to write back to the IVR register to tell the AIC it + * can dispatch other higher priority interrupts again. + * In normal mode, this has no effect, so we can safely do it. + */ + str lr, [lr, #AIC_IVR] + + /* Switch to Supervisor mode and reenable IRQ and FIQ handling. */ + msr CPSR_c, #MODE_SVC + + /* Dispatch the IRQ to the registed handler. */ + stmfd sp!, {r1-r3, ip, lr} + mov lr, pc + bx r0 + + /* Restore registers and switch back to IRQ mode. */ + ldmfd sp!, {r1-r3, ip, lr} + msr CPSR_c, #(IRQ_MASK | MODE_IRQ) + ldmfd sp!, {r0} + + /* Tell the AIC that the interrupt has been handled. */ + str lr, [lr, #AIC_EOICR] + + /* Restore the SPSR register from the IRQ stack. */ + ldmfd sp!, {lr} + msr SPSR_cxsf, lr + + /* Return to the normal execution flow. */ + ldmfd sp!, {pc}^ + + +/* + * Low level service routines that inhibit and enable interrupts in + * the CPU. These are provided as helpers to the OS, as C code cannot + * directly touch the core mode register. + */ + +/* Disable IRQ and FIQ handling in the CPU. */ +interrupts_disable: + /* Get the interrupt counter and increment it. */ + adr r1, interrupts_count + ldr r2, [r1] + adds r2, r2, #1 + str r2, [r1] + + /* If the add didn't cause an overflow, interrupts are already + * disabled. Return immediately. + */ + bxvc lr + + /* Otherwise, disable interrupts. */ + mrs r0, CPSR + orr r0, r0, #(IRQ_MASK | FIQ_MASK) + msr CPSR_c, r0 + bx lr + +/* Enable IRQ and FIQ handling in the CPU. */ +interrupts_enable: + adr r1, interrupts_count + ldr r2, [r1] + subs r2, r2, #1 + str r2, [r1] + + /* If we are still inside a nested interrupt disable, don't + * reenable interrupts. Return immediately. + */ + bxpl lr + + /* Otherwise, reenable interrupts. */ + mrs r0, CPSR + bic r0, r0, #(IRQ_MASK | FIQ_MASK) + msr CPSR_c, r0 + bx lr + +interrupts_count: + /* The byte count starts at 0, which is interrupts disabled with 1 level of nesting. Interrupts enabled is 0xFF. */ + .int 0 + +/* + * Default handler for the interrupt controller. The board is + * initialized with interrupts disabled, and this handler set for all + * interrupts. + */ +nxt_default_irq_handler: + b nxt_default_irq_handler +.global nxt_default_irq_handler + +end: b end + +/* + * This is a marker, used in the payload initialization code to figure + * out where to copy the payload in ram. + */ +payload_start: +.global payload_start + + +/* + * Main initialization. This is where the execution starts when the + * board is powered on. + */ +init_reset: + /* Set up the supervisor mode stack at the top of RAM. */ + ldr sp, =RAM_END + + /* Call the low-level initialization code. Once this has run, + * we're at 48MHz, and the AIC has valid (if useless) exception + * handlers registered. + */ + bl nxt_low_level_init + + /* Switch to FIQ mode and set the r8 register with the AIC + * register base address. + */ + msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_FIQ) + ldr r8, =AIC_BASE + + /* Switch to IRQ mode and set up the IRQ stack at the very top + * of ram. Given our IRQ dispatch code, we need 3 words per + * priority level, so a stack of 3*4*8 bytes. + */ + msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_IRQ) + ldr r0, =RAM_END + mov sp, r0 + sub r0, r0, #IRQ_STACK_SIZE + + /* Go back to Supervisor mode and set up the Supervisor stack + * to start at the top of the IRQ stack. + */ + msr CPSR_c, #(IRQ_MASK | FIQ_MASK | MODE_SVC) + mov sp, r0 + + /* Copy the interrupt vectors and interrupt dispatchers into + * ram. We don't copy the init_reset method, as we're going to + * put the actual application payload there. + */ + ldr r0, =RAM_START + mov r1, #0 + ldr r2, =payload_start + bl init_memcpy + + /* Overwrite the reset vector to point to itself. We don't yet + * support warm reboots, so we should just crash if it's + * attempted. + */ + ldr r1, =RAM_START + str r1, [r1] + + /* Copy the application payload to ram. For this, we use the + * handy symbols that ld provides us, that give us the address in + * flash where the payload is, and the total size of the payload. + * + * We load the payload at the address payload_start, which is + * where ld relocated the addresses to be. + * + * Note that this overwrites the code we're currently in, so + * as soon as we remap the bottom section from flash to ram, the + * crt0 initialization code will be lost. No biggie, we can still + * get to it by hitting flash directly. + */ + ldr r1, =__b_load + ldr r2, =__load_size + bl init_memcpy + + /* Initialize the BSS. Again, we use handy symbols that ld + * defines for us, that specify the size of the BSS section. + */ + mov r1, #0 + ldr r2, =__bss_size + bl init_memset + + /* We're almost ready to boot the kernel. We first set up all + * the registers, getting ready for the remap. Once the remap + * command has executed, we have to banch to load_main before the + * pipeline empties, otherwise we'll fall down into the + * application payload and crash. + */ + ldr lr, =end + ldr ip, =kernel_main + ldr r4, =MC_BASE + mov r5, #1 + + /* Execute the remap, and branch into the kernel. */ + str r5, [r4, #MC_REMAP] + bx ip diff --git a/estorm/vectors.s b/estorm/vectors.s new file mode 100644 index 0000000..57e5ea8 --- /dev/null +++ b/estorm/vectors.s @@ -0,0 +1,27 @@ + .code 32 + .align 0 + .section ".vectors" + +@ What follows is the exception vectors for. They are placed at the +@ bottom of memory at system start up, and just call into exception +@ handlers in crt0_s.S. +@ +@ LDR is used instead of branching because LDR jumps can be relocated. + + ldr pc, v0 @ Reset + ldr pc, v1 @ Undefined Instruction + ldr pc, v2 @ Software Interrupt + ldr pc, v3 @ Prefetch Abort + ldr pc, v4 @ Data Abort + ldr pc, v5 @ Reserved + ldr pc, v6 @ IRQ + ldr pc, v7 @ FIQ + +v0: .long init_reset +v1: .long illegal_hdl +v2: .long swi_hdl +v3: .long prefetch_hdl +v4: .long data_hdl +v5: .long reserved_hdl +v6: .long irq_hdl +v7: .long fiq_hdl -- cgit v1.2.3