/*
 * Copyright (c) 2019 Carlo Caione <ccaione@baylibre.com>
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/toolchain.h>
#include <zephyr/linker/sections.h>
#include <zephyr/arch/cpu.h>
#include <offsets.h>
#include "boot.h"
#include "macro_priv.inc"

_ASM_FILE_PROLOGUE

/*
 * Platform specific pre-C init code
 *
 * Note: - Stack is not yet available
 *       - x23, x24 and x25 must be preserved
 */

WTEXT(z_arm64_el3_plat_prep_c)
SECTION_FUNC(TEXT,z_arm64_el3_plat_prep_c)
	ret

WTEXT(z_arm64_el2_plat_prep_c)
SECTION_FUNC(TEXT,z_arm64_el2_plat_prep_c)
	ret

WTEXT(z_arm64_el1_plat_prep_c)
SECTION_FUNC(TEXT,z_arm64_el1_plat_prep_c)
	ret

/*
 * Set the minimum necessary to safely call C code
 */

GTEXT(__reset_prep_c)
SECTION_SUBSEC_FUNC(TEXT,_reset_section,__reset_prep_c)
	/* return address: x23 */
	mov	x23, lr

	switch_el x0, 3f, 2f, 1f
3:
	/* Reinitialize SCTLR from scratch in EL3 */
	ldr	w0, =(SCTLR_EL3_RES1 | SCTLR_I_BIT | SCTLR_SA_BIT)
	msr	sctlr_el3, x0
	isb

	/* Custom plat prep_c init */
	bl	z_arm64_el3_plat_prep_c

	/* Set SP_EL1 */
	msr     sp_el1, x24

	b	out
2:
	/* Disable alignment fault checking */
	mrs	x0, sctlr_el2
	bic	x0, x0, SCTLR_A_BIT
	msr	sctlr_el2, x0
	isb

	/* Custom plat prep_c init */
	bl	z_arm64_el2_plat_prep_c

	/* Set SP_EL1 */
	msr     sp_el1, x24

	b	out
1:
	/* Disable alignment fault checking */
	mrs	x0, sctlr_el1
	bic	x0, x0, SCTLR_A_BIT
	msr	sctlr_el1, x0
	isb

	/* Custom plat prep_c init */
	bl	z_arm64_el1_plat_prep_c

	/* Set SP_EL1. We cannot use sp_el1 at EL1 */
	msr     SPSel, #1
	mov     sp, x24
out:
	isb

	/* Select SP_EL0 */
	msr	SPSel, #0

	/* Initialize stack */
	mov	sp, x24

	/* fp = NULL */
	mov	fp, xzr

	ret	x23

/*
 * Reset vector
 *
 * Ran when the system comes out of reset. The processor is in thread mode with
 * privileged level. At this point, neither SP_EL0 nor SP_ELx point to a valid
 * area in SRAM.
 */

GTEXT(__reset)
SECTION_SUBSEC_FUNC(TEXT,_reset_section,__reset)

GTEXT(__start)
SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start)

#ifdef CONFIG_WAIT_AT_RESET_VECTOR
resetwait:
	wfe
	b       resetwait
#endif

	/* Mask all exceptions */
	msr	DAIFSet, #0xf

#if CONFIG_MP_MAX_NUM_CPUS > 1

	/*
	 * Deal with multi core booting simultaneously to race for being the primary core.
	 * Use voting lock[1] with reasonable but minimal requirements on the memory system
	 * to make sure only one core wins at last.
	 *
	 * [1] kernel.org/doc/html/next/arch/arm/vlocks.html
	 */
	ldr	x0, =arm64_cpu_boot_params

	/*
	 * Get the "logic" id defined by cpu_node_list statically for voting lock self-identify.
	 * It is worth noting that this is NOT the final logic id (arch_curr_cpu()->id)
	 */
	get_cpu_logic_id	x1, x2, x3, x4	//x1: MPID, x2: logic id

	add	x4, x0, #BOOT_PARAM_VOTING_OFFSET

	/* signal our desire to vote */
	mov	w5, #1
	strb	w5, [x4, x2]
	ldr	x3, [x0, #BOOT_PARAM_MPID_OFFSET]
	cmn	x3, #1
	beq	1f

	/* some core already won, release */
	strb	wzr, [x4, x2]
	b	secondary_core

	/* suggest current core then release */
1:	str	x1, [x0, #BOOT_PARAM_MPID_OFFSET]
	strb	wzr, [x4, x2]
	dmb	ish

	/* then wait until every core else is done voting */
	mov	x5, #0
2:	ldrb	w3, [x4, x5]
	tst	w3, #255
	/* wait */
	bne	2b
	add	x5, x5, #1
	cmp	x5, #CONFIG_MP_MAX_NUM_CPUS
	bne	2b


	/* check if current core won */
	dmb	ish
	ldr	x3, [x0, #BOOT_PARAM_MPID_OFFSET]
	cmp	x3, x1
	beq	primary_core
	/* fallthrough secondary */

	/* loop until our turn comes */
secondary_core:
	dmb	ish
	ldr	x2, [x0, #BOOT_PARAM_MPID_OFFSET]
	cmp	x1, x2
	bne	secondary_core

	/* we can now load our stack pointer value and move on */
	ldr	x24, [x0, #BOOT_PARAM_SP_OFFSET]
	ldr	x25, =z_arm64_secondary_prep_c
	b	boot

primary_core:
#endif
	/* load primary stack and entry point */
	ldr	x24, =(z_interrupt_stacks + __z_interrupt_stack_SIZEOF)
	ldr	x25, =z_prep_c
boot:
	/* Prepare for calling C code */
	bl	__reset_prep_c

	/*
	 * Initialize the interrupt stack with 0xaa so stack utilization
	 * can be measured. This needs to be done before using the stack
	 * so that we don't clobber any data.
	 */
#ifdef CONFIG_INIT_STACKS
	mov_imm	x0, CONFIG_ISR_STACK_SIZE
	sub	x0, sp, x0
	sub     x9, sp, #8
	mov     x10, 0xaaaaaaaaaaaaaaaa
stack_init_loop:
	cmp     x0, x9
	beq     stack_init_done
	str     x10, [x0], #8
	b       stack_init_loop
stack_init_done:
#endif

	/* Platform hook for highest EL */
	bl	z_arm64_el_highest_init

switch_el:
	switch_el x0, 3f, 2f, 1f
3:
	/* EL3 init */
	bl	z_arm64_el3_init

	/* Get next EL */
	adr	x0, switch_el
	bl	z_arm64_el3_get_next_el
	eret

2:
	/* EL2 init */
	bl	z_arm64_el2_init

	/* Move to EL1 with all exceptions masked */
	mov_imm	x0, (SPSR_DAIF_MASK | SPSR_MODE_EL1T)
	msr	spsr_el2, x0

	adr	x0, 1f
	msr	elr_el2, x0
	eret

1:
	/* EL1 init */
	bl	z_arm64_el1_init

	/* We want to use SP_ELx from now on */
	msr	SPSel, #1

	/* Enable SError interrupts */
	msr	DAIFClr, #(DAIFCLR_ABT_BIT)
	isb

	ret	x25  /* either z_prep_c or z_arm64_secondary_prep_c */