quadrotor/crt0.s

# 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,	1 /* 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

	.equ	FP0XVAL,	0x3FFFC014

	# 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

# Turn on LED
#	ldr	r2, =FP0XVAL
#	ldr	r0, [r2, #0]
#	bic	r0, r0, #0x04000000
#	str	r0, [r2, #0]
#	b	__back

# Turn off LED
#	ldr	r2, =FP0XVAL
#	ldr	r0, [r2, #0]
#	orr	r0, r0, #0x04000000
#	str	r0, [r2, #0]
#	b	__back

undefined_handler:
prefetch_abort_handler:
data_abort_handler:
fiq_handler:
	mov	r0, r14
	bl	panic

__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