@@ -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
+
.globl psci_check_target_cpu_id
psci_check_target_cpu_id:
@ Get the real CPU number
@@ -170,6 +223,52 @@ psci_cpu_off:
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, r0
+
+ @ 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_arch_init
psci_arch_init:
mov r6, lr
@@ -180,6 +279,4 @@ psci_arch_init:
bx r6
- .globl psci_text_end
-psci_text_end:
.popsection
@@ -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)
@@ -73,6 +73,11 @@
#define PSCI_CPU_STATUS_ON 1
#define PSCI_CPU_STATUS_ON_PENDING 2
+/* PSCI affinity level state returned by AFFINITY_INFO */
+#define PSCI_AFFINITY_LEVEL_ON 0
+#define PSCI_AFFINITY_LEVEL_OFF 1
+#define PSCI_AFFINITY_LEVEL_ON_PENDING 2
+
#ifndef __ASSEMBLY__
int psci_update_dt(void *fdt);
void psci_board_init(void);
@@ -8,3 +8,4 @@ obj-y += ls1021aqds.o
obj-y += ddr.o
obj-y += eth.o
obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
new file mode 100644
@@ -0,0 +1,36 @@
+/*
+ * 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
+
+.globl psci_text_end
+psci_text_end:
+ nop
+ .popsection
@@ -6,3 +6,4 @@
obj-y += ls1021atwr.o
obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
new file mode 100644
@@ -0,0 +1,28 @@
+/*
+ * 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
+
+.globl psci_text_end
+psci_text_end:
+ nop
+ .popsection
@@ -10,6 +10,7 @@
#define CONFIG_LS102XA
#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
#define CONFIG_SYS_FSL_CLK
@@ -279,6 +280,8 @@ unsigned long get_board_ddr_clk(void);
#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
@@ -10,6 +10,7 @@
#define CONFIG_LS102XA
#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
#define CONFIG_SYS_FSL_CLK