/*
 * 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/>.
 *
 */

@ vim:ft=armv5

.include "swi_numbers.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

	/* 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}

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

	LDR	R8, =0x3f200000
	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
	STR	R1, [R8, #0x04]		@ GPFSEL1

	@ Flash green LED
	MRC	P15, 0, R0, C0, C0, 5
	AND	R0, R0, #0x0f
	CMP	R0, #0
	BNE	skip_flash

	MOV	R1, #1<<18
	STR	R1, [R8, #0x1c]		@ GPSET0
	BL	pause
	MOV	R1, #1<<18
	STR	R1, [R8, #0x28]		@ GPCLR0
	BL	pause

skip_flash:

	@ 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

repeat:
	MOV	R0, #0
	SWI	0
	TST	R0, #1<<17
	MOVNE	R0, #1
	SWINE	0
	B	repeat


swi_handler:
	STMDB	R13!, {R8, LR}
	LDR	R8, =0x3f200000
	CMP	R0, #0
	BEQ	get_gpio
	CMP	R0, #1
	BLEQ	flash
	LDMIA	R13!, {R8, PC}^

get_gpio:
	LDR	R0, [R8, #0x34]		@ GPLEV0
	LDMIA	R13!, {R8, PC}^


flash:
	STMDB	R13!, {R1, R2, R4, LR}
	MOV	R4, #5

1:
	MOV	R1, #1<<16
	STR	R1, [R8, #0x28]		@ GPCLR0
	MOV	R1, #1<<18
	STR	R1, [R8, #0x1c]		@ GPSET0

	BL	pause

	MOV	R1, #1<<18
	STR	R1, [R8, #0x28]		@ GPCLR0
	MOV	R1, #1<<16
	STR	R1, [R8, #0x1c]		@ GPSET0

	BL	pause

	MOV	R1, #1<<18
	ORR	R1, #1<<16
	STR	R1, [R8, #0x28]		@ GPCLR0

	SUBS	R4, R4, #1
	BNE	1b

	LDMIA	R13!, {R1, R2, R4, PC}

pause:
	MOV	R2, #0x3f000
1:
	SUBS	R2, R2, #1
	BNE	1b
	MOV	PC, LR