/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright 2017 NXP
 *
 * Peng Fan <peng.fan@nxp.com>
 */

#include <arm32_macros_cortex_a9.S>
#include <arm32_macros.S>
#include <arm.h>
#include <asm.S>
#include <generated/asm-defines.h>
#include <keep.h>
#include <kernel/asan.h>
#include <kernel/unwind.h>
#include <platform_config.h>

.section .text

/*
 * int sm_pm_cpu_suspend(uint32_t arg, int (*fn)(uint32_t))
 * @arg will be passed to fn as argument
 * return value: 0 - cpu resumed from suspended state.
 *		 -1 - cpu not suspended.
 */
FUNC sm_pm_cpu_suspend, :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	push	{r4 - r12, lr}
	mov	r5, sp
	sub	sp, sp, #SM_PM_CTX_SIZE
	push	{r0, r1}

	mov	r1, r5
	add	r0, sp, #8
	blx	sm_pm_cpu_suspend_save
	adr	lr, aborted
	/* Jump to arch specific suspend */
	pop	{r0, pc}
aborted:
	/* cpu not suspended */
	add	sp, sp, #SM_PM_CTX_SIZE
	/* Return -1 to the caller */
	mov	r0, #(-1)
suspend_return:
	pop	{r4 - r12, pc}
UNWIND(	.fnend)
END_FUNC sm_pm_cpu_suspend

FUNC sm_pm_cpu_do_suspend, :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	push	{r4 - r11}
	read_midr r4
	ubfx	r5, r4, #4, #12
	ldr     r4, =CORTEX_A7_PART_NUM
	cmp	r5, r4
	beq	a7_suspend
	ldr     r4, =CORTEX_A9_PART_NUM
	cmp	r5, r4
	beq	a9_suspend
	/* cpu not supported */
	b	.
	/* A9 needs PCR/DIAG */
a9_suspend:
	read_pcr r4
	read_diag r5
	stmia	r0!, {r4 - r5}
a7_suspend:
	read_fcseidr r4
	read_tpidruro r5
	stmia	r0!, {r4 - r5}
	read_dacr r4
#ifdef CFG_WITH_LPAE
#error "Not supported"
#else
	read_ttbr0 r5
	read_ttbr1 r6
	read_ttbcr r7
#endif
	read_sctlr r8
	read_actlr r9
	read_cpacr r10
	read_mvbar r11
	stmia	r0!, {r4 - r11}
	read_prrr r4
	read_nmrr r5
	read_vbar r6
	read_nsacr r7
	stmia	r0, {r4 - r7}
	pop	{r4 - r11}
	bx	lr
UNWIND(	.fnend)
END_FUNC sm_pm_cpu_do_suspend

FUNC sm_pm_cpu_resume, :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	cpsid	aif

	/* Call into the runtime address of __get_core_pos */
	adr	r0, _core_pos
	ldr	r1, [r0]
	add	r0, r0, r1
	blx	r0

	/*
	 * At this point, MMU is not enabled now.
	 * 1. Get the runtime physical address of _suspend_sp
	 * 2. Get the offset from _suspend_sp to &thread_core_local
	 * 3. Get the runtime physical address of thread_core_local
	 * Since moving towards non-linear mapping,
	 * `ldr r0, =thread_core_local` is not used here.
	 */
	adr	r4, _suspend_sp
	ldr	r5, [r4]
	add	r4, r4, r5

	mov_imm r1, THREAD_CORE_LOCAL_SIZE
	mla	r0, r0, r1, r4

	ldr	r0, [r0, #THREAD_CORE_LOCAL_SM_PM_CTX_PHYS]
	/* Need to use r0!, because sm_pm_cpu_do_resume needs it */
	ldmia	r0!, {sp, pc}
UNWIND(	.fnend)
END_FUNC sm_pm_cpu_resume

/*
 * The following will be located in text section whose attribute is
 * marked as readonly, but we only need to read here
 * _suspend_sp stores the offset between thread_core_local to _suspend_sp.
 * _core_pos stores the offset between __get_core_pos to _core_pos.
 */
.align 2
.extern thread_core_local
_suspend_sp:
	.long	thread_core_local - .
.extern __get_core_pos
_core_pos:
	.long	__get_core_pos - .

/*
 * void sm_pm_cpu_do_resume(paddr suspend_regs) __noreturn;
 * Restore the registers stored when sm_pm_cpu_do_suspend
 * r0 points to the physical base address of the suspend_regs
 * field of struct sm_pm_ctx.
 */
FUNC sm_pm_cpu_do_resume, :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	read_midr r4
	ubfx	r5, r4, #4, #12
	ldr     r4, =CORTEX_A7_PART_NUM
	cmp	r5, r4
	beq	a7_resume

	/*
	 * A9 needs PCR/DIAG
	 */
	ldmia   r0!, {r4 - r5}
	write_pcr r4
	write_diag r5

a7_resume:
	/* v7 resume */
	mov	ip, #0
	/* Invalidate icache to PoU */
	write_iciallu
	/* set reserved context */
	write_contextidr ip
	ldmia	r0!, {r4 - r5}
	write_fcseidr r4
	write_tpidruro r5
	ldmia	r0!, {r4 - r11}
	/* Invalidate entire TLB */
	write_tlbiall
	write_dacr r4
#ifdef CFG_WITH_LPAE
#error "Not supported -"
#else
	write_ttbr0 r5
	write_ttbr1 r6
	write_ttbcr r7
#endif

	ldmia	r0, {r4 - r7}
	write_prrr r4
	write_nmrr r5
	write_vbar r6
	write_nsacr r7

	write_actlr r9
	write_cpacr r10
	write_mvbar r11
	write_bpiall
	isb
	dsb
	/* MMU will be enabled here */
	write_sctlr r8
	isb
	mov	r0, #0
	b	suspend_return
UNWIND(	.fnend)
END_FUNC sm_pm_cpu_do_resume