#include "aarch64_irq.h"

.globl aarch64_init
aarch64_init:
	/* set vector table */
	adr x0, aarch64_vectors
	msr vbar_el1, x0
	mov x0, 0
	ret

.globl aarch64_get_el
aarch64_get_el:
	mrs x0, CurrentEL
	lsr x0, x0, 0x02
	ret

.globl aarch64_get32r
aarch64_get32r:
	ldr w0, [x0]
	ret

.globl aarch64_set32r
aarch64_set32r:
	str w1, [x0]
	ret

.globl aarch64_delay
aarch64_delay:
	subs x0, x0, 0x01
	bne aarch64_delay
	ret

.globl aarch64_irq_vector_init
irq_vector_init:
	ret

.globl aarch64_irq_enable
aarch64_irq_enable:
	msr daifclr, 2
	ret

.globl aarch64_irq_disable
aarch64_irq_disable:
	msr daifset, 2
	ret


/* Exceptions Vector Table
 * */
	.macro KERNEL_ENTER
	sub sp, sp, 256
	stp x0, x1, [sp, 16 * 0]
	stp x2, x3, [sp, 16 * 1]
	stp x4, x5, [sp, 16 * 2]
	stp x6, x7, [sp, 16 * 3]
	stp x8, x9, [sp, 16 * 4]
	stp x10, x11, [sp, 16 * 5]
	stp x12, x13, [sp, 16 * 6]
	stp x14, x15, [sp, 16 * 7]
	stp x16, x17, [sp, 16 * 8]
	stp x18, x19, [sp, 16 * 9]
	stp x20, x21, [sp, 16 * 10]
	stp x22, x23, [sp, 16 * 11]
	stp x24, x25, [sp, 16 * 12]
	stp x26, x27, [sp, 16 * 13]
	stp x28, x29, [sp, 16 * 14]
	str x30, [sp, 16 * 15]
	.endm

	.macro KERNEL_LEAVE
	ldp x0, x1, [sp, 16 * 0]
	ldp x2, x3, [sp, 16 * 1]
	ldp x4, x5, [sp, 16 * 2]
	ldp x6, x7, [sp, 16 * 3]
	ldp x8, x9, [sp, 16 * 4]
	ldp x10, x11, [sp, 16 * 5]
	ldp x12, x13, [sp, 16 * 6]
	ldp x14, x15, [sp, 16 * 7]
	ldp x16, x17, [sp, 16 * 8]
	ldp x18, x19, [sp, 16 * 9]
	ldp x20, x21, [sp, 16 * 10]
	ldp x22, x23, [sp, 16 * 11]
	ldp x24, x25, [sp, 16 * 12]
	ldp x26, x27, [sp, 16 * 13]
	ldp x28, x29, [sp, 16 * 14]
	ldr x30, [sp, 16 * 15]
	add sp, sp, 256
	.endm

	.macro VECTOR_ENTRY GOTO_LABEL
	.align 7
	b \GOTO_LABEL
	.endm

	.macro EXCEPTION_FALLBACK EID
	KERNEL_ENTER
	mov x0, \EID
	mrs x1, esr_el1
	mrs x2, elr_el1
	bl k_irq_fallback
	b die
	.endm

die:
	b die

.align 11
.globl aarch64_vectors
aarch64_vectors:
	/* EL1t */
	VECTOR_ENTRY el1t_sync
	VECTOR_ENTRY el1t_irq
	VECTOR_ENTRY el1t_fiq
	VECTOR_ENTRY el1t_error
	/* EL1h */
	VECTOR_ENTRY el1h_sync
	VECTOR_ENTRY el1h_irq
	VECTOR_ENTRY el1h_fiq
	VECTOR_ENTRY el1h_error
	/* EL0 - 64bit */
	VECTOR_ENTRY el0_64_sync
	VECTOR_ENTRY el0_64_irq
	VECTOR_ENTRY el0_64_fiq
	VECTOR_ENTRY el0_64_error
	/* EL0 - 32bit */
	VECTOR_ENTRY el0_32_sync
	VECTOR_ENTRY el0_32_irq
	VECTOR_ENTRY el0_32_fiq
	VECTOR_ENTRY el0_32_error

el1t_sync:
	EXCEPTION_FALLBACK EL1t_SYNC

el1t_irq:
	EXCEPTION_FALLBACK EL1t_IRQ

el1t_fiq:
	EXCEPTION_FALLBACK EL1t_FIQ

el1t_error:
	EXCEPTION_FALLBACK EL1t_ERROR

el1h_sync:
	EXCEPTION_FALLBACK EL1h_SYNC

el1h_irq:
	KERNEL_ENTER
	bl k_irq_handler
	KERNEL_LEAVE
	eret

el1h_fiq:
	EXCEPTION_FALLBACK EL1h_FIQ

el1h_error:
	EXCEPTION_FALLBACK EL1h_ERROR

el0_64_sync:
	EXCEPTION_FALLBACK EL0_64_SYNC

el0_64_irq:
	EXCEPTION_FALLBACK EL0_64_IRQ

el0_64_fiq:
	EXCEPTION_FALLBACK EL0_64_FIQ

el0_64_error:
	EXCEPTION_FALLBACK EL0_64_ERROR

el0_32_sync:
	EXCEPTION_FALLBACK EL0_32_SYNC

el0_32_irq:
	EXCEPTION_FALLBACK EL0_32_IRQ

el0_32_fiq:
	EXCEPTION_FALLBACK EL0_32_FIQ

el0_32_error:
	EXCEPTION_FALLBACK EL0_32_ERROR