/*
 * main.s
 *
 * Copyright © 2018 Thomas White <taw@bitwiz.org.uk>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

.include "swi_numbers.h"
.include "ports.h"

.section .init

_start:
	LDR	PC, [PC, #24]	@ Reset
	LDR	PC, [PC, #24]	@ Undefined
	LDR	PC, [PC, #24]	@ SWI
	LDR	PC, [PC, #24]	@ Prefetch abort
	LDR	PC, [PC, #24]	@ Data abort
	LDR	PC, [PC, #24]	@ Reserved
	LDR	PC, [PC, #24]	@ IRQ
	LDR	PC, [PC, #24]	@ FIQ
	.word	reset		@ Reset
	.word	hang		@ Undefined
	.word	swi_handler	@ SWI
	.word	hang		@ Prefetch abort
	.word	hang		@ Data abort
	.word	hang		@ Reserved
	.word	hang		@ IRQ
	.word	hang		@ FIQ

hang:
	B	hang

reset:
	/* Get out of HYP mode */
	MRS	R0, CPSR
	BIC	R0, R0, #0x1f
	ORR	R0, R0, #0x13
	MSR	SPSR_cxsf, R0
	ADD	R0, PC, #4
	MSR	ELR_hyp,r0
	ERET

	/* Set up supervisor mode stack */
	LDR	R13, =stack_svc_cpu0

	/* Relocate vector table */
	MOV	R10, #0x8000
	MOV	R11, #0
	LDMIA	R10!, {R0-R7}
	STMIA	R11!, {R0-R7}
	LDMIA	R10!, {R0-R7}
	STMIA	R11!, {R0-R7}

	BL	framebuffer_init

	/* Configure GPIOs */
	LDR	R8, =(PERIPHERAL_BASE+OFFS_GPIO)
	LDR	R1, [R8, #0x04]		@ GPFSEL1
	ORR	R1, R1, #1<<18		@ GPIO pin 16 is output
	BIC	R1, R1, #3<<21		@ GPIO pin 17 is input
	ORR	R1, R1, #1<<24		@ GPIO pin 18 is output
	ORR	R1, R1, #1<<29		@ GPIO pin 29 is output
	STR	R1, [R8, #0x04]		@ GPFSEL1

	/* Set another CPU doing something */
	LDR	R0, =init_cpu3
	MOV	R1, #0x40000000
	STR	R0,[R1, #0xbc]
	SEV

	/* Switch to USR mode and set up stack */
	MRS	R0, CPSR
	BIC	R0, R0, #0x1f
	ORR	R0, R0, #0x10
	MSR	CPSR_c, R0
	LDR	R13, =stack_usr_cpu0

	/* Start user mode task */
	B	flash_button


init_cpu3:
	/* Out of HYP mode */
	MRS	R0, CPSR
	BIC	R0, R0, #0x1f
	ORR	R0, R0, #0x13
	MSR	SPSR_cxsf, R0
	ADD	R0, PC, #4
	MSR	ELR_hyp,r0
	ERET

	/* Set up supervisor mode stack */
	LDR	R13, =stack_svc_cpu3

	/* Switch to USR mode and set up stack */
	MRS	R0, CPSR
	BIC	R0, R0, #0x1f
	ORR	R0, R0, #0x10
	MSR	CPSR_c, R0
	LDR	R13, =stack_usr_cpu3

	B	flash_status_led