ARMv7: PSCI: ls102xa: add more PSCI v1.0 functions implemention
This patch implements PSCI functions for ls102xa SoC following PSCI v1.0,
they are as the list:
psci_version,
psci_features,
psci_cpu_suspend,
psci_affinity_info,
psci_system_reset,
psci_system_off.
Tested on LS1021aQDS, LS1021aTWR.
Signed-off-by: Wang Dongsheng <dongsheng.wang@nxp.com>
Signed-off-by: Hongbo Zhang <hongbo.zhang@nxp.com>
Reviewed-by: York Sun <york.sun@nxp.com>
diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index cba9c1e..8f38680 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -12,19 +12,72 @@
#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_BRR 0x0E4
-#define DCFG_CCSR_SCRATCHRW1 0x200
+#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
.pushsection ._secure.text, "ax"
.arch_extension sec
+ .align 5
+
#define ONE_MS (GENERIC_TIMER_CLK / 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 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
@@ -144,4 +197,50 @@
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
+
.popsection
diff --git a/arch/arm/include/asm/arch-ls102xa/config.h b/arch/arm/include/asm/arch-ls102xa/config.h
index 67a4311..e0b1185 100644
--- a/arch/arm/include/asm/arch-ls102xa/config.h
+++ b/arch/arm/include/asm/arch-ls102xa/config.h
@@ -32,6 +32,7 @@
#define CONFIG_SYS_FSL_SERDES_ADDR (CONFIG_SYS_IMMR + 0x00ea0000)
#define CONFIG_SYS_FSL_GUTS_ADDR (CONFIG_SYS_IMMR + 0x00ee0000)
#define CONFIG_SYS_FSL_LS1_CLK_ADDR (CONFIG_SYS_IMMR + 0x00ee1000)
+#define CONFIG_SYS_FSL_RCPM_ADDR (CONFIG_SYS_IMMR + 0x00ee2000)
#define CONFIG_SYS_NS16550_COM1 (CONFIG_SYS_IMMR + 0x011c0500)
#define CONFIG_SYS_NS16550_COM2 (CONFIG_SYS_IMMR + 0x011d0500)
#define CONFIG_SYS_DCU_ADDR (CONFIG_SYS_IMMR + 0x01ce0000)
diff --git a/board/freescale/ls1021aqds/Makefile b/board/freescale/ls1021aqds/Makefile
index ab02344..f0390c1 100644
--- a/board/freescale/ls1021aqds/Makefile
+++ b/board/freescale/ls1021aqds/Makefile
@@ -8,3 +8,4 @@
obj-y += ddr.o
obj-y += eth.o
obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
diff --git a/board/freescale/ls1021aqds/psci.S b/board/freescale/ls1021aqds/psci.S
new file mode 100644
index 0000000..598168c
--- /dev/null
+++ b/board/freescale/ls1021aqds/psci.S
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2016 NXP Semiconductor.
+ * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/psci.h>
+
+ .pushsection ._secure.text, "ax"
+
+ .arch_extension sec
+
+ .align 5
+
+.globl psci_system_off
+psci_system_off:
+ @ Get QIXIS base address
+ movw r1, #(QIXIS_BASE & 0xffff)
+ movt r1, #(QIXIS_BASE >> 16)
+
+ ldrb r2, [r1, #QIXIS_PWR_CTL]
+ orr r2, r2, #QIXIS_PWR_CTL_POWEROFF
+ strb r2, [r1, #QIXIS_PWR_CTL]
+
+1: wfi
+ b 1b
+
+ .popsection
diff --git a/board/freescale/ls1021atwr/Makefile b/board/freescale/ls1021atwr/Makefile
index 01296c0..5238b15 100644
--- a/board/freescale/ls1021atwr/Makefile
+++ b/board/freescale/ls1021atwr/Makefile
@@ -6,3 +6,4 @@
obj-y += ls1021atwr.o
obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
diff --git a/board/freescale/ls1021atwr/psci.S b/board/freescale/ls1021atwr/psci.S
new file mode 100644
index 0000000..bec7356
--- /dev/null
+++ b/board/freescale/ls1021atwr/psci.S
@@ -0,0 +1,25 @@
+/*
+ * Copyright 2016 NXP Semiconductor.
+ * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/psci.h>
+
+ .pushsection ._secure.text, "ax"
+
+ .arch_extension sec
+
+ .align 5
+
+.globl psci_system_off
+psci_system_off:
+1: wfi
+ b 1b
+
+ .popsection
diff --git a/include/configs/ls1021aqds.h b/include/configs/ls1021aqds.h
index eb444eb..ba3331f 100644
--- a/include/configs/ls1021aqds.h
+++ b/include/configs/ls1021aqds.h
@@ -10,6 +10,7 @@
#define CONFIG_LS102XA
#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
#define CONFIG_ARMV7_PSCI_NR_CPUS CONFIG_MAX_CPUS
#define CONFIG_SYS_FSL_CLK
@@ -280,6 +281,8 @@
#define QIXIS_LBMAP_SHIFT 0
#define QIXIS_LBMAP_DFLTBANK 0x00
#define QIXIS_LBMAP_ALTBANK 0x04
+#define QIXIS_PWR_CTL 0x21
+#define QIXIS_PWR_CTL_POWEROFF 0x80
#define QIXIS_RST_CTL_RESET 0x44
#define QIXIS_RCFG_CTL_RECONFIG_IDLE 0x20
#define QIXIS_RCFG_CTL_RECONFIG_START 0x21
diff --git a/include/configs/ls1021atwr.h b/include/configs/ls1021atwr.h
index c588fdd..5b6e0a5 100644
--- a/include/configs/ls1021atwr.h
+++ b/include/configs/ls1021atwr.h
@@ -10,6 +10,7 @@
#define CONFIG_LS102XA
#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
#define CONFIG_ARMV7_PSCI_NR_CPUS CONFIG_MAX_CPUS
#define CONFIG_SYS_FSL_CLK