Signed-off-by: Timothy Pearson tpearson@raptorengineeringinc.com --- arch/arm/cpu/armv7/sunxi/psci_sun6i.S | 174 ++++++++++++++++++++++++++++++++-- 1 file changed, 164 insertions(+), 10 deletions(-)

diff --git a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S index d35b63e..b3090ec 100644 --- a/arch/arm/cpu/armv7/sunxi/psci_sun6i.S +++ b/arch/arm/cpu/armv7/sunxi/psci_sun6i.S @@ -21,6 +21,7 @@

#include <asm/arch-armv7/generictimer.h> #include <asm/gic.h> +#include <linux/linkage.h> #include <asm/macro.h> #include <asm/psci.h> #include <asm/arch/cpu.h> @@ -45,6 +46,16 @@ #define GICD_BASE 0x1c81000 #define GICC_BASE 0x1c82000

+@ Attempts to condense sparse ID space on multi-cluster systems to dense ID space +ENTRY(psci_get_cpu_id) + mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ + lsr r1, r0, #6 /* extract cluster offset */ + and r1, r1, #0xff + and r0, r0, #0xff /* extract CPU ID in cluster */ + add r0, r0, r1 /* return adjusted CPU ID */ + bx lr +ENDPROC(psci_get_cpu_id) + .globl psci_fiq_enter psci_fiq_enter: push {r0-r12} @@ -106,7 +117,7 @@ psci_fiq_enter: str r10, [r8, #0x100] timer_wait r10, ONE_MS

-#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) +#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) || defined(CONFIG_MACH_SUN8I_A83T) @ Activate power clamp lsl r12, r9, #2 @ x4 add r12, r12, r8 @@ -134,7 +145,13 @@ out: mcr p15, 0, r7, c1, c1, 0 psci_cpu_on: push {lr}

- mov r0, r1 +#if defined(CONFIG_MACH_SUN8I_A83T) + lsr r3, r1, #6 @ extract cluster offset + and r0, r1, #3 @ extract CPU number + add r0, r0, r3 @ pass adjusted CPU number to setup +#else + mov r0, r1 @ pass CPU number to setup +#endif bl psci_get_cpu_stack_top @ get stack top of target CPU str r2, [r0] @ store target PC at stack top dsb @@ -142,15 +159,74 @@ psci_cpu_on: movw r0, #(SUN6I_CPUCFG_BASE & 0xffff) movt r0, #(SUN6I_CPUCFG_BASE >> 16)

+ ldr r6, =psci_cpu_entry + str r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector) + @ CPU mask +#if defined(CONFIG_MACH_SUN8I_A83T) + lsr r3, r1, #8 @ extract cluster number + and r1, r1, #3 @ extract CPU number + + @ obtain silicon revision level + movw r0, #(SUNXI_SRAMC_BASE & 0xffff) + movt r0, #(SUNXI_SRAMC_BASE >> 16) + ldr r2, [r0, #0x24] + and r2, r2, #0x1 +#else + ldr r3, #0 @ only one cluster and r1, r1, #3 @ only care about first cluster + ldr r2, #0 @ default revision +#endif mov r4, #1 lsl r4, r4, r1

- ldr r6, =psci_cpu_entry - str r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector) - @ Assert reset on target CPU +#if defined(CONFIG_MACH_SUN8I_A83T) + movw r0, #(SUN8I_CPUXCFG_BASE & 0xffff) + movt r0, #(SUN8I_CPUXCFG_BASE >> 16) + + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x80 @ SUNXI_CPU_RST_CTRL + ldr r6, [r0, r5] + bic r6, r6, r4 @ assert reset + str r6, [r0, r5] + + cmp r1, #0 + bne skip_cluster_reset_assert + + lsl r5, r3, #4 @ cluster offset + add r5, r5, #0x04 @ SUNXI_CLUSTER_CTRL1 + ldr r6, [r0, r5] + orr r6, r6, #0x1 @ assert ACINACTM + str r6, [r0, r5] + + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x80 @ SUNXI_CPU_RST_CTRL + ldr r6, [r0, r5] + bic r6, r6, #0x1000 @ assert HRESET + bic r6, r6, #0x100 @ assert L2 cache reset + str r6, [r0, r5] +skip_cluster_reset_assert: + + @ re-calculate CPU control register address + movw r0, #(SUN6I_CPUCFG_BASE & 0xffff) + movt r0, #(SUN6I_CPUCFG_BASE >> 16) + + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x30 @ SUNXI_CLUSTER_PWRON_RESET + ldr r6, [r0, r5] + bic r6, r6, r4 @ assert reset + str r6, [r0, r5] + + movw r0, #(SUN8I_CPUXCFG_BASE & 0xffff) + movt r0, #(SUN8I_CPUXCFG_BASE >> 16) + + @ l1 invalidate + lsl r5, r3, #4 @ calculate cluster offset + ldr r6, [r0, r5] @ CX_CTRL_REG0 + bic r6, r6, r4 + str r6, [r0, r5] +#else mov r6, #0 lsl r5, r1, #6 @ 64 bytes per CPU add r5, r5, #0x40 @ Offset from base @@ -166,32 +242,109 @@ psci_cpu_on: ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG bic r6, r6, r4 str r6, [r0, #0x1e4] +#endif

movw r0, #(SUNXI_PRCM_BASE & 0xffff) movt r0, #(SUNXI_PRCM_BASE >> 16)

-#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) +#if defined(CONFIG_MACH_SUN6I) || defined(CONFIG_MACH_SUN8I_H3) || defined(CONFIG_MACH_SUN8I_A83T) @ Release power clamp - lsl r5, r1, #2 @ 1 register per CPU + lsl r5, r3, #2 @ calculate cluster offset + add r5, r1 @ add CPU offset + lsl r5, r5, #2 @ 1 register per CPU add r5, r5, r0 @ PRCM +#if defined(CONFIG_MACH_SUN8I_A83T) + mov r6, #0xfe + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS + timer_wait r6, TEN_MS + mov r6, #0xf8 + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS + mov r6, #0xe0 + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS + mov r6, #0xc0 + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS + mov r6, #0x80 + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS + mov r6, #0x00 + str r6, [r5, #0x140] @ CPUx_PWR_CLAMP + timer_wait r6, TEN_MS +#else movw r6, #0x1ff movt r6, #0 1: lsrs r6, r6, #1 str r6, [r5, #0x140] @ CPUx_PWR_CLAMP bne 1b #endif +#endif

timer_wait r6, TEN_MS

@ Clear power gating - ldr r6, [r0, #0x100] @ CPU_PWROFF_GATING - bic r6, r6, r4 - str r6, [r0, #0x100] + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x100 @ SUNXI_CLUSTER_PWROFF_GATING + ldr r6, [r0, r5] +#if defined(CONFIG_MACH_SUN8I_A83T) + cmp r2, #0 + beq skip_a83t_rev_b_fixup + cmp r1, #0 + bne skip_a83t_rev_b_fixup + bic r6, r6, #0x10 @ core 0 power gate off + b a83t_rev_b_fixup_done +#endif +skip_a83t_rev_b_fixup: + bic r6, r6, r4 @ power gating off +a83t_rev_b_fixup_done: + str r6, [r0, r5] + + movw r0, #(SUN8I_CPUXCFG_BASE & 0xffff) + movt r0, #(SUN8I_CPUXCFG_BASE >> 16)

@ re-calculate CPU control register address movw r0, #(SUN6I_CPUCFG_BASE & 0xffff) movt r0, #(SUN6I_CPUCFG_BASE >> 16)

+#if defined(CONFIG_MACH_SUN8I_A83T) + timer_wait r6, TEN_MS + timer_wait r6, TEN_MS + + @ Deassert reset on target CPU + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x30 @ SUNXI_CLUSTER_PWRON_RESET + ldr r6, [r0, r5] + orr r6, r6, r4 @ deassert reset + str r6, [r0, r5] + + movw r0, #(SUN8I_CPUXCFG_BASE & 0xffff) + movt r0, #(SUN8I_CPUXCFG_BASE >> 16) + + @ Clear any applied cluster resets + cmp r1, #0 + bne skip_cluster_reset_deassert + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x80 @ SUNXI_CPU_RST_CTRL + ldr r6, [r0, r5] + orr r6, r6, #0x1000 @ deassert HRESET + orr r6, r6, #0x100 @ deassert L2 cache reset + str r6, [r0, r5] + + lsl r5, r3, #4 @ cluster offset + add r5, r5, #0x04 @ SUNXI_CLUSTER_CTRL1 + ldr r6, [r0, r5] + bic r6, r6, #0x1 @ deassert ACINACTM + str r6, [r0, r5] +skip_cluster_reset_deassert: + + lsl r5, r3, #2 @ cluster offset + add r5, r5, #0x80 @ SUNXI_CPU_RST_CTRL + ldr r6, [r0, r5] + orr r6, r6, r4 @ deassert reset + str r6, [r0, r5] +#else @ Deassert reset on target CPU mov r6, #3 lsl r5, r1, #6 @ 64 bytes per CPU @@ -203,6 +356,7 @@ psci_cpu_on: ldr r6, [r0, #0x1e4] @ CPUCFG_DBG_CTL1_REG orr r6, r6, r4 str r6, [r0, #0x1e4] +#endif

mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS pop {pc}