/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright 2018 NXP
 */

#include <kernel/unwind.h>
#include <asm.S>
#include <arm.h>
#include <arm32_macros.S>
#include <generated/imx_pm_asm_defines.h>
#include <imx-regs.h>
#include <platform_config.h>
#include <kernel/cache_helpers.h>
#include <kernel/tz_ssvce_def.h>
#include <kernel/tz_proc_def.h>

#define MX7_SRC_GPR1	0x74
#define MX7_SRC_GPR2	0x78
#define MX7_SRC_GPR3	0x7c
#define MX7_SRC_GPR4	0x80
#define MX7_GPC_IMR1	0x30
#define MX7_GPC_IMR2	0x34
#define MX7_GPC_IMR3	0x38
#define MX7_GPC_IMR4	0x3c
#define DDRC_STAT	0x4
#define DDRC_PWRCTL	0x30
#define DDRC_DBG1	0x304
#define DDRC_DBGCAM	0x308
#define DDRC_PSTAT	0x3fc
#define DDRC_PCTRL_0	0x490

	.section .text.psci.cpuidle
	.align 3
/*
 * imx_pen_lock
 *
 * The reference link of Peterson's algorithm:
 * http://en.wikipedia.org/wiki/Peterson's_algorithm
 *
 * val1 = r1 = !turn (inverted from Peterson's algorithm)
 * on cpu 0:
 * r2 = flag[0] (in flag0)
 * r3 = flag[1] (in flag1)
 * on cpu1:
 * r2 = flag[1] (in flag1)
 * r3 = flag[0] (in flag0)
 *
 */
	.macro	imx_pen_lock

	mov	r8, r0
	read_mpidr r5
	and	r5, r5, #3
	add	r6, r8, #PM_INFO_MX7_VAL_OFF
	cmp	r5, #0
	addeq	r7, r8, #PM_INFO_MX7_FLAG0_OFF
	addeq	r8, r8, #PM_INFO_MX7_FLAG1_OFF
	addne	r7, r8, #PM_INFO_MX7_FLAG1_OFF
	addne	r8, r8, #PM_INFO_MX7_FLAG0_OFF

	mov	r9, #1
	str	r9, [r7]
	dsb
	str	r5, [r6]
1:
	dsb
	ldr	r9, [r8]
	cmp	r9, #1
	ldreq	r9, [r6]
	cmpeq	r9, r5
	beq	1b

	.endm

	.macro	imx_pen_unlock

	dsb
	read_mpidr r6
	and	r6, r6, #3
	cmp	r6, #0
	addeq	r7, r0, #PM_INFO_MX7_FLAG0_OFF
	addne	r7, r0, #PM_INFO_MX7_FLAG1_OFF
	mov	r9, #0
	str	r9, [r7]

	.endm

	.macro  disable_l1_dcache

	push	{r0 - r12, lr}
	mov	r0, #DCACHE_OP_CLEAN_INV
	ldr	r1, =dcache_op_all
	blx	r1
	pop	{r0 - r12, lr}

	/* disable d-cache */
	read_sctlr r7
	bic	r7, r7, #SCTLR_C
	write_sctlr r7
	dsb
	isb

	push	{r0 - r12, lr}
	mov	r0, #DCACHE_OP_CLEAN_INV
	ldr	r1, =dcache_op_all
	blx	r1
	pop	{r0 - r12, lr}

	/* TODO: handle non-SMP kernel */
	clrex

	/* Turn off SMP bit. */
	read_actlr r8
	bic	r8, r8, #ACTLR_SMP
	write_actlr r8
	isb
	dsb

	.endm

	.macro	tlb_set_to_ocram

	/* save ttbr */
	read_ttbr1 r7
	str	r7, [r0, #PM_INFO_MX7_TTBR1_OFF]

	read_ttbr0 r7
	str	r7, [r0, #PM_INFO_MX7_TTBR0_OFF]

	/*
	 * To ensure no page table walks occur in DDR, we
	 * have a another page table stored in IRAM that only
	 * contains entries pointing to IRAM, AIPS1 and AIPS2.
	 * We need to set the TTBR1 to the new IRAM TLB.
	 * Do the following steps:
	 * 1. Flush the Branch Target Address Cache (BTAC)
	 * 2. Set TTBR1 to point to IRAM page table.
	 * 3. Disable page table walks in TTBR0 (PD0 = 1)
	 */

	/* Disable Branch Prediction, Z bit in SCTLR. */
	read_sctlr r6
	bic	r6, r6, #SCTLR_Z
	write_sctlr r6

	/* Flush the BTAC. */
	write_bpiallis

	ldr	r6, =iram_tbl_phys_addr
	ldr	r7, [r6]

	dsb
	isb

	/* Store the IRAM table in TTBR1/TTBR0 */
	write_ttbr1 r7
	write_ttbr0 r7

	/* Read TTBCR and set PD0=1 */
	read_ttbcr r6
	orr	r6, r6, #TTBCR_PD0
	write_ttbcr r6

	dsb
	isb

	/* flush the TLB */
	write_tlbiallis
	isb

	.endm

	.macro	tlb_back_to_ddr

	/* Read TTBCR and set PD0=0 */
	read_ttbcr r6
	bic	r6, r6, #TTBCR_PD0
	write_ttbcr r6

	dsb
	isb

	/* flush the TLB */
	write_tlbiallis

	/* Enable Branch Prediction, Z bit in SCTLR. */
	read_sctlr r6
	orr	r6, r6, #SCTLR_Z
	write_sctlr r6

	/* Flush the Branch Target Address Cache (BTAC) */
	write_bpiallis

	/* restore ttbr */
	ldr	r7, [r0, #PM_INFO_MX7_TTBR1_OFF]
	write_ttbr1 r7
	ldr	r7, [r0, #PM_INFO_MX7_TTBR0_OFF]
	write_ttbr0 r7

	.endm

	/* r10 must be DDRC base address */
	.macro ddrc_enter_self_refresh

	ldr	r10, [r0, #PM_INFO_MX7_DDRC_V_OFF]

	/* disable port */
	ldr	r7, =0x0
	str	r7, [r10, #DDRC_PCTRL_0]

	/* let DDR out of self-refresh */
	ldr	r7, =0x0
	str	r7, [r10, #DDRC_PWRCTL]

	/* wait rw port_busy clear */
	ldr	r6, =(0x1 << 16)
	orr	r6, r6, #0x1
2:
	ldr	r7, [r10, #DDRC_PSTAT]
	ands	r7, r7, r6
	bne	2b

	ldr	r7, =0x1
	str	r7, [r10, #DDRC_DBG1]

	ldr	r6, =0x36000000
11:
	ldr	r7, [r10, #DDRC_DBGCAM]
	and	r7, r7, r6
	cmp	r7, r6
	bne	11b

	/* enter self-refresh bit 5 */
	ldr	r7, =(0x1 << 5)
	str	r7, [r10, #DDRC_PWRCTL]

	/* wait until self-refresh mode entered */
3:
	ldr	r7, [r10, #DDRC_STAT]
	and	r7, r7, #0x3
	cmp	r7, #0x3
	bne	3b
4:
	ldr	r7, [r10, #DDRC_STAT]
	ands	r7, r7, #0x20
	beq	4b

	/* disable dram clk */
	ldr	r7, [r10, #DDRC_PWRCTL]
	orr	r7, r7, #(1 << 3)
	str	r7, [r10, #DDRC_PWRCTL]

	/*
	 * TO1.1 adds feature of DDR pads power down,
	 * although TO1.0 has no such function, but it is
	 * NOT harmful to program GPR registers for TO1.0,
	 * it can avoid the logic of version check in idle
	 * thread.
	 */
	ldr	r10, [r0, #PM_INFO_MX7_IOMUXC_GPR_V_OFF]
	ldr	r7, =0xf0000
	str	r7, [r10]

	/* delay 20us, measured by gpio */
	ldr	r7, =20
12:
	subs	r7, r7, #0x1
	bne	12b

	.endm

	/* r10 must be DDRC base address */
	.macro ddrc_exit_self_refresh

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_IOMUXC_GPR_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_IOMUXC_GPR_V_OFF]

	ldr	r7, =0x0
	str	r7, [r10]

	ldr	r7, =20
13:
	subs	r7, r7, #0x1
	bne	13b

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_DDRC_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_DDRC_V_OFF]

	ldr	r7, =0x0
	str	r7, [r10, #DDRC_DBG1]

	ldr	r6, =0x30000000
14:
	ldr	r7, [r10, #DDRC_DBGCAM]
	and	r7, r7, r6
	cmp	r7, r6
	bne	14b

	/* let DDR out of self-refresh */
	ldr	r7, =0x0
	str	r7, [r10, #DDRC_PWRCTL]

	/* wait until self-refresh mode exited */
5:
	ldr	r7, [r10, #DDRC_STAT]
	and	r7, r7, #0x3
	cmp	r7, #0x3
	beq	5b

	/* enable auto self-refresh */
	ldr	r7, [r10, #DDRC_PWRCTL]
	orr	r7, r7, #(1 << 0)
	str	r7, [r10, #DDRC_PWRCTL]

	ldr	r7, =0x1
	str	r7, [r10, #DDRC_PCTRL_0]

	.endm

	.macro	pll_do_wait_lock
6:
	ldr	r7, [r10, r8]
	ands	r7, #0x80000000
	beq	6b

	.endm

	.macro ccm_enter_idle

	ldr	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* ungate pfd1 332m for lower axi */
	ldr	r7, =0x8000
	str	r7, [r10, #0xc8]

	ldr	r10, [r0, #PM_INFO_MX7_CCM_V_OFF]

	/* switch ARM CLK to OSC */
	ldr	r8, =0x8000
	ldr	r7, [r10, r8]
	bic	r7, r7, #0x7000000
	str	r7, [r10, r8]

	/* lower AXI clk from 24MHz to 3MHz */
	ldr	r8, =0x8800
	ldr	r7, [r10, r8]
	orr	r7, r7, #0x7
	str	r7, [r10, r8]

	/* lower AHB clk from 24MHz to 3MHz */
	ldr	r8, =0x9000
	ldr	r7, [r10, r8]
	orr	r7, r7, #0x7
	str	r7, [r10, r8]

	/* gate dram clk */
	ldr	r8, =0x9880
	ldr	r7, [r10, r8]
	bic	r7, r7, #0x10000000
	str	r7, [r10, r8]

	ldr	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* gate pfd1 332m */
	ldr	r7, =0x8000
	str	r7, [r10, #0xc4]

	/* gate system pll pfd div 1 */
	ldr	r7, =0x10
	str	r7, [r10, #0xb4]
	/* power down ARM, 480 and DRAM PLL */
	ldr	r7, =0x1000
	str	r7, [r10, #0x64]
	str	r7, [r10, #0xb4]
	ldr	r7, =0x100000
	str	r7, [r10, #0x74]

	.endm

	.macro ccm_exit_idle

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_ANATOP_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* power up ARM, 480 and DRAM PLL */
	ldr	r7, =0x1000
	str	r7, [r10, #0x68]
	ldr	r8, =0x60
	pll_do_wait_lock

	ldr	r7, =0x1000
	str	r7, [r10, #0xb8]
	ldr	r8, =0xb0
	pll_do_wait_lock

	ldr	r7, =0x100000
	str	r7, [r10, #0x78]
	ldr	r8, =0x70
	pll_do_wait_lock

	/* ungate pfd1 332m for lower axi */
	ldr	r7, =0x8000
	str	r7, [r10, #0xc8]

	/* ungate system pll pfd div 1 */
	ldr	r7, =0x10
	str	r7, [r10, #0xb8]

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_CCM_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_CCM_V_OFF]

	/* switch ARM CLK to PLL */
	ldr	r8, =0x8000
	ldr	r7, [r10, r8]
	orr	r7, r7, #0x1000000
	str	r7, [r10, r8]

	/* restore AXI clk from 3MHz to 24MHz */
	ldr	r8, =0x8800
	ldr	r7, [r10, r8]
	bic	r7, r7, #0x7
	str	r7, [r10, r8]

	/* restore AHB clk from 3MHz to 24MHz */
	ldr	r8, =0x9000
	ldr	r7, [r10, r8]
	bic	r7, r7, #0x7
	str	r7, [r10, r8]

	/* ungate dram clk */
	ldr	r8, =0x9880
	ldr	r7, [r10, r8]
	orr	r7, r7, #0x10000000
	str	r7, [r10, r8]

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_ANATOP_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* gate pfd1 332m for lower axi */
	ldr	r7, =0x8000
	str	r7, [r10, #0xc4]

	.endm

	.macro anatop_enter_idle

	ldr	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* XTAL to RC-OSC switch */
	ldr	r7, [r10]
	orr	r7, r7, #0x1000
	str	r7, [r10]
	/* power down XTAL */
	ldr	r7, [r10]
	orr	r7, r7, #0x1
	str	r7, [r10]

	/* enable weak 1P0A */
	ldr	r7, [r10, #0x200]
	orr	r7, r7, #0x40000
	str	r7, [r10, #0x200]

	/* disable LDO 1P0A */
	ldr	r7, [r10, #0x200]
	bic	r7, r7, #0x1
	str	r7, [r10, #0x200]

	/* disable LDO 1P0D */
	ldr	r7, [r10, #0x210]
	bic	r7, r7, #0x1
	str	r7, [r10, #0x210]

	/* disable LDO 1P2 */
	ldr	r7, [r10, #0x220]
	bic	r7, r7, #0x1
	str	r7, [r10, #0x220]

	/* switch to low power bandgap */
	ldr	r7, [r10, #0x270]
	orr	r7, r7, #0x400
	str	r7, [r10, #0x270]
	/* power down normal bandgap */
	orr	r7, r7, #0x1
	str	r7, [r10, #0x270]

	.endm

	.macro anatop_exit_idle

	cmp	r5, #0x1
	ldreq	r10, [r0, #PM_INFO_MX7_ANATOP_P_OFF]
	ldrne	r10, [r0, #PM_INFO_MX7_ANATOP_V_OFF]

	/* power on normal bandgap */
	ldr	r7, [r10, #0x270]
	bic	r7, r7, #0x1
	str	r7, [r10, #0x270]
	/* switch to normal bandgap */
	bic	r7, r7, #0x400
	str	r7, [r10, #0x270]

	/* enable LDO 1P2 */
	ldr	r7, [r10, #0x220]
	orr	r7, r7, #0x1
	str	r7, [r10, #0x220]
7:
	ldr	r7, [r10, #0x220]
	ands	r7, #0x20000
	beq	7b

	/* enable LDO 1P0D */
	ldr	r7, [r10, #0x210]
	orr	r7, r7, #0x1
	str	r7, [r10, #0x210]
8:
	ldr	r7, [r10, #0x210]
	ands	r7, #0x20000
	beq	8b

	/* enable LDO 1P0A */
	ldr	r7, [r10, #0x200]
	orr	r7, r7, #0x1
	str	r7, [r10, #0x200]
9:
	ldr	r7, [r10, #0x200]
	ands	r7, #0x20000
	beq	9b
	/* disable weak 1P0A */
	ldr	r7, [r10, #0x200]
	bic	r7, r7, #0x40000
	str	r7, [r10, #0x200]

	/* power up XTAL and wait */
	ldr	r7, [r10]
	bic	r7, r7, #0x1
	str	r7, [r10]
10:
	ldr	r7, [r10]
	ands	r7, r7, #0x4
	beq	10b
	/* RC-OSC to XTAL switch */
	ldr	r7, [r10]
	bic	r7, r7, #0x1000
	str	r7, [r10]

	.endm

.extern iram_tlb_phys_addr

FUNC imx7d_low_power_idle, :
UNWIND( .fnstart)
	push	{r0 - r12}

	/* get necessary info from pm_info */
	ldr	r1, [r0, #PM_INFO_MX7_PBASE_OFF]
	ldr	r2, [r0, #PM_INFO_MX7_SIZE_OFF]

	/*
	 * counting the resume address in iram
	 * to set it in SRC register.
	 */
	ldr	r5, =imx7d_low_power_idle
	ldr     r6, =wakeup
	sub	r6, r6, r5
	add     r8, r1, r2
	add	r3, r8, r6

	/* r11 is cpu id */
	read_mpidr r11
	and	r11, r11, #3
	cmp	r11, #0x0
	ldreq	r6, =MX7_SRC_GPR1
	ldreq	r7, =MX7_SRC_GPR2
	ldrne	r6, =MX7_SRC_GPR3
	ldrne	r7, =MX7_SRC_GPR4
	/* store physical resume addr and pm_info address. */
	ldr	r10, [r0, #PM_INFO_MX7_SRC_V_OFF]
	str	r3, [r10, r6]
	str	r1, [r10, r7]

	disable_l1_dcache

	tlb_set_to_ocram

	/* check last to sleep */
	ldr	r6, [r0, #PM_INFO_MX7_NUM_ONLINE_CPUS_OFF]
	ldr	r7, [r0, #PM_INFO_MX7_NUM_LPI_CPUS_OFF]
	cmp	r6, r7
	bne	lpi_enter_done

	ddrc_enter_self_refresh
	ccm_enter_idle
	anatop_enter_idle

	ldr	r10, [r0, #PM_INFO_MX7_GIC_DIST_V_OFF]
	ldr	r7, =0x0
	ldr	r8, =0x1000
	str	r7, [r10, r8]

	ldr	r10, [r0, #PM_INFO_MX7_GPC_V_OFF]
	ldr	r4, [r10, #MX7_GPC_IMR1]
	ldr	r5, [r10, #MX7_GPC_IMR2]
	ldr	r6, [r10, #MX7_GPC_IMR3]
	ldr	r7, [r10, #MX7_GPC_IMR4]

	ldr	r8, =0xffffffff
	str	r8, [r10, #MX7_GPC_IMR1]
	str	r8, [r10, #MX7_GPC_IMR2]
	str	r8, [r10, #MX7_GPC_IMR3]
	str	r8, [r10, #MX7_GPC_IMR4]

	/*
	 * enable the RBC bypass counter here
	 * to hold off the interrupts. RBC counter
	 * = 8 (240us). With this setting, the latency
	 * from wakeup interrupt to ARM power up
	 * is ~250uS.
	 */
	ldr	r8, [r10, #0x14]
	bic	r8, r8, #(0x3f << 24)
	orr	r8, r8, #(0x8 << 24)
	str	r8, [r10, #0x14]

	/* enable the counter. */
	ldr	r8, [r10, #0x14]
	orr	r8, r8, #(0x1 << 30)
	str	r8, [r10, #0x14]

	/* unmask all the GPC interrupts. */
	str	r4, [r10, #MX7_GPC_IMR1]
	str	r5, [r10, #MX7_GPC_IMR2]
	str	r6, [r10, #MX7_GPC_IMR3]
	str	r7, [r10, #MX7_GPC_IMR4]

	/*
	 * now delay for a short while (30usec)
	 * ARM is at 24MHz at this point
	 * so a short loop should be enough.
	 * this delay is required to ensure that
	 * the RBC counter can start counting in
	 * case an interrupt is already pending
	 * or in case an interrupt arrives just
	 * as ARM is about to assert DSM_request.
	 */
	ldr	r4, =5
rbc_loop:
	subs	r4, r4, #0x1
	bne	rbc_loop

lpi_enter_done:
	imx_pen_unlock

        wfi
	isb

	imx_pen_lock

	/* check first to wake */
	ldr	r6, [r0, #PM_INFO_MX7_NUM_ONLINE_CPUS_OFF]
	ldr	r7, [r0, #PM_INFO_MX7_NUM_LPI_CPUS_OFF]
	cmp	r6, r7
	bne	skip_lpi_flow

	ldr	r5, =0x0
	anatop_exit_idle
	ccm_exit_idle
	ddrc_exit_self_refresh

	ldr	r10, [r0, #PM_INFO_MX7_GIC_DIST_V_OFF]
	ldr	r7, =0x3
	ldr	r8, =0x1000
	str	r7, [r10, r8]
skip_lpi_flow:

	tlb_back_to_ddr

	/* TODO: handle non-SMP kernel */
	/* Turn on SMP bit. */
	read_actlr r7
	orr	r7, r7, #ACTLR_SMP
	write_actlr r7

	isb

	/* enable d-cache */
	read_sctlr r7
	orr	r7, r7, #SCTLR_C
	write_sctlr r7
	dsb
	isb

	/* Restore registers */
	pop	{r0 - r12}
	bx	lr

wakeup:
	/* invalidate L1 I-cache first */
	write_iciallu
	write_bpiall
	/* enable the Icache and branch prediction */
	mov	r1, #(SCTLR_I | SCTLR_Z)
	write_sctlr r1
	isb

	/* switch monitor mode */
	cps	#CPSR_MODE_MON

	imx_pen_lock
	/* check first to wake */
	ldr	r6, [r0, #PM_INFO_MX7_NUM_ONLINE_CPUS_OFF]
	ldr	r7, [r0, #PM_INFO_MX7_NUM_LPI_CPUS_OFF]
	cmp	r6, r7
	bne	wakeup_skip_lpi_flow

	ldr	r5, =0x1
	anatop_exit_idle
	ccm_exit_idle
	ddrc_exit_self_refresh
wakeup_skip_lpi_flow:

	/* get physical resume address from pm_info. */
	ldr	lr, [r0, #PM_INFO_MX7_RESUME_ADDR_OFF]

	/* Restore registers */
	bx	lr

UNWIND( .fnend)
END_FUNC imx7d_low_power_idle

/*
 * Note: OPTEE VA = PA, for TEE_RAM.
 * This maybe changed in future.
 */
FUNC v7_cpu_resume, :
UNWIND( .fnstart)
	mov     r0, #0	@ ; write the cache size selection register to be
	write_csselr r0	@ ; sure we address the data cache
	isb		@ ; isb to sync the change to the cachesizeid reg

_inv_dcache_off:
	mov     r0, #0	@ ; set way number to 0
_inv_nextway:
	mov     r1, #0	@ ; set line number (=index) to 0
_inv_nextline:
	orr     r2, r0, r1	@ ; construct way/index value
	write_dcisw r2 @ ; invalidate data or unified cache line by set/way
	add     r1, r1, #1 << LINE_FIELD_OFFSET	@ ; increment the index
	cmp     r1, #1 << LINE_FIELD_OVERFLOW	@ ; overflow out of set field
	bne     _inv_nextline
	add     r0, r0, #1 << WAY_FIELD_OFFSET	@ ; increment the way number
	cmp     r0, #0				@ ; overflow out of way field
	bne     _inv_nextway

	dsb					@ ; synchronise

	/*
	 * no stack, scratch r0-r3
	 * TODO: need to use specific configure, but not plat_xxx.
	 * because plat_xx maybe changed in future, we can not rely on it.
	 * need handle sp carefully.
	 */
	blx plat_cpu_reset_early

	b	sm_pm_cpu_resume
UNWIND( .fnend)
END_FUNC v7_cpu_resume