Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

230 lines
4.2 KiB

# Runtime support
# Configurable parameters
# Set PLL ratios - M=4, P=2, 58.9824 MHz clock
.equ PLL_M, 4
.equ PLL_P, 2
.equ FLASHCLOCKS, 3 /* 40-60MHz clock */
.equ APB_DIVIDER, 4 /* 1, 2 or 4 */
.equ UND_STACK_SIZE, 0x0004
.equ SVC_STACK_SIZE, 0x0010
.equ ABT_STACK_SIZE, 0x0004
.equ FIQ_STACK_SIZE, 0x0004
.equ IRQ_STACK_SIZE, 0x0080
.equ USR_STACK_SIZE, 0x0400
.global _stack_size
.equ _stack_size, 0
.equ _stack_size, _stack_size + UND_STACK_SIZE
.equ _stack_size, _stack_size + SVC_STACK_SIZE
.equ _stack_size, _stack_size + ABT_STACK_SIZE
.equ _stack_size, _stack_size + FIQ_STACK_SIZE
.equ _stack_size, _stack_size + IRQ_STACK_SIZE
.equ _stack_size, _stack_size + USR_STACK_SIZE
# Processor definitions
.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 I_Bit, 0x80
.equ F_Bit, 0x40
# Register definitions
.equ MAM_BASE, 0xE01FC000
.equ MAMCR, 0
.equ MAMTIM, 4
.equ PLL_BASE, 0xE01FC080
.equ PLLCON, 0x00
.equ PLLCFG, 0x04
.equ PLLSTAT, 0x08
.equ PLLFEED, 0x0c
.equ PLLCON_PLLE, (1<<0)
.equ PLLCON_PLLC, (1<<1)
.equ PLLCFG_MSEL, (0x1F<<0)
.equ PLLCFG_PSEL, (0x03<<5)
.equ PLLSTAT_PLOCK, (1<<10)
.equ APBDIV_BASE, 0xE01FC100
.equ APBDIV, 0
# True is -1 so we subtract values together.
.equ PLL_LOG_P, (0-(PLL_P>1)-(PLL_P>2)-(PLL_P>4))
.equ PLLCFG_VAL, (PLL_M-1) | (PLL_LOG_P << 5)
# 1 => 1, 2 => 2, 4 => 0
.equ APB_VAL, (APB_DIVIDER & 3)
.text
.global _startup
.func _startup
_startup:
vectors:
b reset_handler
b undefined_handler
b swi_handler
b prefetch_abort_handler
b data_abort_handler
nop /* reserved */
ldr pc, [pc, #-0xff0] /* branch through VICVectAddr register */
b fiq_handler
reset_handler:
# MAM and PLL setup
ldr r1, =MAM_BASE
ldr r2, =PLL_BASE
mov r3, #0xAA
mov r4, #0x55
# Configure and Enable PLL
mov r0, #PLLCFG_VAL
str r0, [r2, #PLLCFG]
mov r0, #PLLCON_PLLE
str r0, [r2, #PLLCON]
str r3, [r2, #PLLFEED]
str r4, [r2, #PLLFEED]
# Disable MAM until we turn on the PLL
mov r0, #0 /* MAM disabled */
str r0, [r1, #MAMCR]
# Wait until PLL Locked
lockwait:
ldr r0, [r2, #PLLSTAT]
ands r0, r0, #PLLSTAT_PLOCK
beq lockwait
# Switch to PLL Clock
mov r0, #(PLLCON_PLLE | PLLCON_PLLC)
str r0, [r2, #PLLCON]
str r3, [r2, #PLLFEED]
str r4, [r2, #PLLFEED]
# Enable MAM and set up flash timing
mov r0, #FLASHCLOCKS
str r0, [r1, #MAMTIM]
mov r0, #2 /* MAM fully enabled */
str r0, [r1, #MAMCR]
# Set APB divider
ldr r1, =APBDIV_BASE
mov r0, #APB_DIVIDER
str r0, [r1, #APBDIV]
# Relocate .data section (Copy from ROM to RAM)
ldr r1, =_etext
ldr r2, =_data
ldr r3, =_edata
lrel:
cmp r2, r3
ldrlo r0, [r1], #4
strlo r0, [r2], #4
blo lrel
# Clear .bss section (Zero init)
mov r0, #0
ldr r1, =_bss_start
ldr r2, =_bss_end
lzi:
cmp r1, r2
strlo r0, [r1], #4
blo lzi
# Set up stacks
ldr r0, =_stack_end
msr CPSR_c, #Mode_UND|I_Bit|F_Bit
mov sp, r0
sub r0, r0, #UND_STACK_SIZE
msr CPSR_c, #Mode_ABT|I_Bit|F_Bit
mov sp, r0
sub r0, r0, #ABT_STACK_SIZE
msr CPSR_c, #Mode_FIQ|I_Bit|F_Bit
mov sp, r0
sub r0, r0, #FIQ_STACK_SIZE
msr CPSR_c, #Mode_IRQ|I_Bit|F_Bit
mov sp, r0
sub r0, r0, #IRQ_STACK_SIZE
msr CPSR_c, #Mode_SVC|I_Bit|F_Bit
mov sp, r0
sub r0, r0, #SVC_STACK_SIZE
msr CPSR_c, #Mode_USR
mov sp, r0
# Stack Limit (only when compiled with "-mapcs-stack-check")
sub sl, sp, #USR_STACK_SIZE
# Prepare to go!
mov r0, #0
mov r1, #0
adr lr, __back
b main
# Undefined handlers can just spin for now
undefined_handler:
prefetch_abort_handler:
data_abort_handler:
fiq_handler:
__back:
b __back
.equ SWI_MAX, 1
swi_handler:
stmfd sp!, {ip, lr}
ldr ip, [lr, #-4]
bic ip, #0xff000000
cmp ip, #SWI_MAX
ldmhifd sp!, {ip, pc}^
add ip, pc, ip, lsl #2
ldr pc, [ip]
swi_branch_table:
.word disable_interrupts
.word enable_interrupts
disable_interrupts:
mrs ip, SPSR
orr ip, ip, #I_Bit
msr SPSR_c,ip
ldmfd sp!, {ip, pc}^
enable_interrupts:
mrs ip, SPSR
bic ip, ip, #I_Bit
msr SPSR_c,ip
ldmfd sp!, {ip, pc}^
.endfunc
.end