/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * Copyright 2015 Freescale Semiconductor, Inc.
 * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
 */

#include <config.h>
#include <linux/linkage.h>

#include <asm/armv7.h>
#include <asm/arch-armv7/generictimer.h>
#include <asm/psci.h>

#define RCPM_TWAITSR		0x04C

#define SCFG_CORE0_SFT_RST      0x130
#define SCFG_CORESRENCR         0x204

#define DCFG_CCSR_RSTCR			0x0B0
#define DCFG_CCSR_RSTCR_RESET_REQ	0x2
#define DCFG_CCSR_BRR			0x0E4
#define DCFG_CCSR_SCRATCHRW1		0x200

#define PSCI_FN_PSCI_VERSION_FEATURE_MASK	0x0
#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK	0x0
#define PSCI_FN_CPU_OFF_FEATURE_MASK		0x0
#define PSCI_FN_CPU_ON_FEATURE_MASK		0x0
#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK	0x0
#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK		0x0
#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK	0x0
#define PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK	0x0

	.pushsection ._secure.text, "ax"

	.arch_extension sec

	.align	5

#define	ONE_MS		(COUNTER_FREQUENCY / 1000)
#define	RESET_WAIT	(30 * ONE_MS)

.globl	psci_version
psci_version:
	movw	r0, #0
	movt	r0, #1

	bx	lr

_ls102x_psci_supported_table:
	.word	ARM_PSCI_0_2_FN_PSCI_VERSION
	.word	PSCI_FN_PSCI_VERSION_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_CPU_SUSPEND
	.word	PSCI_FN_CPU_SUSPEND_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_CPU_OFF
	.word	PSCI_FN_CPU_OFF_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_CPU_ON
	.word	PSCI_FN_CPU_ON_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_AFFINITY_INFO
	.word	PSCI_FN_AFFINITY_INFO_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_SYSTEM_OFF
	.word	PSCI_FN_SYSTEM_OFF_FEATURE_MASK
	.word	ARM_PSCI_0_2_FN_SYSTEM_RESET
	.word	PSCI_FN_SYSTEM_RESET_FEATURE_MASK
	.word	ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
	.word	PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK
	.word	0
	.word	ARM_PSCI_RET_NI

.globl	psci_features
psci_features:
	adr	r2, _ls102x_psci_supported_table
1:	ldr	r3, [r2]
	cmp	r3, #0
	beq	out_psci_features
	cmp	r1, r3
	addne	r2, r2, #8
	bne	1b

out_psci_features:
	ldr	r0, [r2, #4]
	bx	lr

@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
@ ARM_PSCI_RET_INVAL,suppose caller saves r4 before calling
LENTRY(psci_check_target_cpu_id)
	@ Get the real CPU number
	and	r4, r1, #0xff
	mov	r0, #ARM_PSCI_RET_INVAL

	@ Bit[31:24], bits must be zero.
	tst	r1, #0xff000000
	bxne	lr

	@ Affinity level 2 - Cluster: only one cluster in LS1021xa.
	tst	r1, #0xff0000
	bxne	lr

	@ Affinity level 1 - Processors: should be in 0xf00 format.
	lsr	r1, r1, #8
	teq	r1, #0xf
	bxne	lr

	@ Affinity level 0 - CPU: only 0, 1 are valid in LS1021xa.
	cmp	r4, #2
	bxge	lr

	mov	r0, #ARM_PSCI_RET_SUCCESS
	bx	lr
ENDPROC(psci_check_target_cpu_id)

	@ r1 = target CPU
	@ r2 = target PC
.globl	psci_cpu_on
psci_cpu_on:
	push	{r4, r5, r6, lr}

	@ Clear and Get the correct CPU number
	@ r1 = 0xf01
	bl	psci_check_target_cpu_id
	cmp	r0, #ARM_PSCI_RET_INVAL
	beq	out_psci_cpu_on

	mov	r0, r4
	mov	r1, r2
	mov	r2, r3
	bl	psci_save
	mov	r1, r4

	@ Get DCFG base address
	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)

	@ Detect target CPU state
	ldr	r2, [r4, #DCFG_CCSR_BRR]
	rev	r2, r2
	lsr	r2, r2, r1
	ands	r2, r2, #1
	beq	holdoff_release

	@ Reset target CPU
	@ Get SCFG base address
	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)

	@ Enable CORE Soft Reset
	movw	r5, #0
	movt	r5, #(1 << 15)
	rev	r5, r5
	str	r5, [r0, #SCFG_CORESRENCR]

	@ Get CPUx offset register
	mov	r6, #0x4
	mul	r6, r6, r1
	add	r2, r0, r6

	@ Do reset on target CPU
	movw	r5, #0
	movt	r5, #(1 << 15)
	rev	r5, r5
	str	r5, [r2, #SCFG_CORE0_SFT_RST]

	@ Wait target CPU up
	timer_wait	r2, RESET_WAIT

	@ Disable CORE soft reset
	mov	r5, #0
	str	r5, [r0, #SCFG_CORESRENCR]

holdoff_release:
	@ Release on target CPU
	ldr	r2, [r4, #DCFG_CCSR_BRR]
	mov	r6, #1
	lsl	r6, r6, r1	@ 32 bytes per CPU

	rev	r6, r6
	orr	r2, r2, r6
	str	r2, [r4, #DCFG_CCSR_BRR]

	@ Set secondary boot entry
	ldr	r6, =psci_cpu_entry
	rev	r6, r6
	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]

	isb
	dsb

	@ Return
	mov	r0, #ARM_PSCI_RET_SUCCESS

out_psci_cpu_on:
	pop	{r4, r5, r6, lr}
	bx	lr

.globl	psci_cpu_off
psci_cpu_off:
	bl	psci_cpu_off_common

1:	wfi
	b	1b

.globl	psci_affinity_info
psci_affinity_info:
	push	{lr}

	mov	r0, #ARM_PSCI_RET_INVAL

	@ Verify Affinity level
	cmp	r2, #0
	bne	out_affinity_info

	bl	psci_check_target_cpu_id
	cmp	r0, #ARM_PSCI_RET_INVAL
	beq	out_affinity_info
	mov	r1, r4

	@ Get RCPM base address
	movw	r4, #(CONFIG_SYS_FSL_RCPM_ADDR & 0xffff)
	movt	r4, #(CONFIG_SYS_FSL_RCPM_ADDR >> 16)

	mov	r0, #PSCI_AFFINITY_LEVEL_ON

	@ Detect target CPU state
	ldr	r2, [r4, #RCPM_TWAITSR]
	rev	r2, r2
	lsr	r2, r2, r1
	ands	r2, r2, #1
	beq	out_affinity_info

	mov	r0, #PSCI_AFFINITY_LEVEL_OFF

out_affinity_info:
	pop	{pc}

.globl	psci_system_reset
psci_system_reset:
	@ Get DCFG base address
	movw	r1, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
	movt	r1, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)

	mov	r2, #DCFG_CCSR_RSTCR_RESET_REQ
	rev	r2, r2
	str	r2, [r1, #DCFG_CCSR_RSTCR]

1:	wfi
	b	1b

.globl	psci_system_suspend
psci_system_suspend:
	push	{lr}

	bl	ls1_system_suspend

	pop	{pc}

	.popsection