Merge git://git.denx.de/u-boot-fsl-qoriq

[u-boot] / arch / arm / cpu / armv7 / ls102xa / psci.S

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index cba9c1e1aafbdcf6c225a5cdf84d8b90b6d7da59..3d41d37a6438b1aad05e12b8aa3ef2f8987027f3 100644 (file)
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -12,19 +12,75 @@
 #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
+#define PSCI_FN_SYSTEM_SUSPEND_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   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
@@ -144,4 +200,58 @@ 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, 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