#include "mem_map.h"

.arm
.cpu mpcore
.fpu softvfp

.global initCpu
.global deinitCpu

.type initCpu STT_FUNC
.type setupExceptionVectors STT_FUNC
.type deinitCpu STT_FUNC

.extern setupMmu
.extern flushDCache

.section ".init"



initCpu:
	mov r10, lr

	@ Control register:
	@ [29] Force AP functionality             : disabled
	@ [28] TEX remap                          : disabled
	@ [27] NMFI bit                           : normal FIQ behavior
	@ [25] CPSR E bit on taking an exception  : 0
	@ [23] Extended page table configuration  : subpage AP bits enabled
	@ [22] Unaligned data access              : disabled
	@ [15] Disable loading TBIT               : disabled
	@ [13] Vector select                      : 0x00000000
	@ [12] Level one instruction cache        : disabled
	@ [11] Program flow prediction            : disabled
	@ [7]  Endianess                          : little
	@ [2]  Level one data cache               : disabled
	@ [1]  Strict data address alignment fault: disabled
	@ [0]  MMU                                : disabled
	ldr r0, =0x54078
	mcr p15, 0, r0, c1, c0, 0   @ Write control register
	mov r0, #0
	mcr p15, 0, r0, c1, c0, 1   @ Write Auxiliary Control Register

	mcr p15, 0, r0, c7, c5, 4   @ Flush Prefetch Buffer
	mcr p15, 0, r0, c7, c7, 0   @ Invalidate Both Caches. Also flushes the branch target cache
	mcr p15, 0, r0, c7, c10, 4  @ Data Synchronization Barrier
	clrex

	ldr sp, =A11_STACK_END

	bl setupExceptionVectors    @ Setup the vectors in AXIWRAM bootrom vectors jump to
	blx setupMmu
	bx r10
.pool


#define MAKE_BRANCH(src, dst) (0xEA000000 | (((((dst) - (src)) >> 2) - 2) & 0xFFFFFF))

setupExceptionVectors:
	adr r0, __vectorStubs
	ldr r1, =A11_VECTORS_START
	ldmia r0!, {r2-r9}
	stmia r1!, {r2-r9}
	ldmia r0, {r2-r5}
	stmia r1, {r2-r5}
	bx lr
.pool
__vectorStubs:
	.word MAKE_BRANCH(A11_VECTORS_START + 0x00, A11_VECTORS_START + 0x00) // IRQ
	.word 0
	.word MAKE_BRANCH(A11_VECTORS_START + 0x08, A11_VECTORS_START + 0x08) // FIQ
	.word 0
	.word MAKE_BRANCH(A11_VECTORS_START + 0x10, A11_VECTORS_START + 0x10) // SVC
	.word 0
	.word MAKE_BRANCH(A11_VECTORS_START + 0x18, A11_VECTORS_START + 0x18) // Undefined instruction
	.word 0
	.word MAKE_BRANCH(A11_VECTORS_START + 0x20, A11_VECTORS_START + 0x20) // Prefetch abort
	.word 0
	.word MAKE_BRANCH(A11_VECTORS_START + 0x28, A11_VECTORS_START + 0x28) // Data abort
	.word 0


deinitCpu:
	mov r4, lr

	@ Stub vectors to endless loops
	ldr r0, =A11_VECTORS_START
	mov r1, #6
	ldr r2, =MAKE_BRANCH(0, 0)  @ Endless loop
	deinitCpu_vector_lp:
		str r2, [r0], #8
		subs r1, r1, #1
		bne deinitCpu_vector_lp

	bl flushDCache
	mrc p15, 0, r0, c1, c0, 0   @ Read control register
	ldr r1, =0x803805           @ MMU, D-Cache, Program flow prediction, I-Cache,
                                @ high exception vectors, subpage AP bits disabled
	bic r0, r0, r1
	mcr p15, 0, r0, c1, c0, 0   @ Write control register
	mrc p15, 0, r0, c1, c0, 1   @ Read Auxiliary Control Register
	bic r0, r0, #0x7F           @ Return stack, Dynamic branch prediction, Static branch prediction,
                                @ Instruction folding, L1 and L2 caches are exclusive,
                                @ SMP mode: the CPU is taking part in coherency and L1 parity checking
	mcr p15, 0, r0, c1, c0, 1   @ Write Auxiliary Control Register

	mov r0, #0
	mcr p15, 0, r0, c7, c5, 4   @ Flush Prefetch Buffer
	mcr p15, 0, r0, c7, c7, 0   @ Invalidate Both Caches. Also flushes the branch target cache
	mcr p15, 0, r0, c7, c10, 4  @ Data Synchronization Barrier
	clrex
	bx r4
.pool