From: Jan Kiszka jan.kiszka@siemens.com

Based on the original version by Marc Zyngier. It adds a psci_cpu_off implementation for the A20 SoC. The mechanism works by first preparing the calling CPU to go offline (disable and flush cache, disable SMP), then requesting CPU 0 to pull the plug. The request is sent as FIQ on SGI15.

Signed-off-by: Jan Kiszka jan.kiszka@siemens.com --- arch/arm/cpu/armv7/sunxi/psci.S | 171 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+)

diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index a84807d..739ce77 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -18,6 +18,7 @@ */

#include <config.h> +#include <asm/gic.h> #include <asm/psci.h> #include <asm/arch/cpu.h>

@@ -38,6 +39,8 @@

#define ONE_MS (CONFIG_SYS_CLK_FREQ / 1000) #define TEN_MS (10 * ONE_MS) +#define GICD_BASE 0x1c81000 +#define GICC_BASE 0x1c82000

.macro timer_wait reg, ticks @ Program CNTP_TVAL @@ -61,7 +64,27 @@

.globl psci_arch_init psci_arch_init: + movw r4, #(GICD_BASE & 0xffff) + movt r4, #(GICD_BASE >> 16) + + ldr r5, [r4, #GICD_IGROUPRn] + bic r5, r5, #(1 << 15) @ SGI15 as Group-0 + str r5, [r4, #GICD_IGROUPRn] + + mov r5, #0 @ Set SGI15 priority to 0 + strb r5, [r4, #(GICD_IPRIORITYRn + 15)] + + add r4, r4, #0x1000 @ GICC address + + mov r5, #0xff + str r5, [r4, #GICC_PMR] @ Be cool with non-secure + + ldr r5, [r4, #GICC_CTLR] + orr r5, r5, #(1 << 3) @ Switch FIQEn on + str r5, [r4, #GICC_CTLR] + mrc p15, 0, r5, c1, c1, 0 @ Read SCR + orr r5, r5, #4 @ Enable FIQ in monitor mode bic r5, r5, #1 @ Secure mode mcr p15, 0, r5, c1, c1, 0 @ Write SCR isb @@ -79,6 +102,78 @@ psci_arch_init:

bx lr

+.globl psci_fiq_enter +psci_fiq_enter: + push {r0-r12} + + @ Switch to secure + mrc p15, 0, r7, c1, c1, 0 + bic r8, r7, #1 + mcr p15, 0, r8, c1, c1, 0 + isb + + @ Validate reason based on IAR and acknowledge + movw r8, #(GICC_BASE & 0xffff) + movt r8, #(GICC_BASE >> 16) + ldr r9, [r8, #GICC_IAR] + movw r10, #0x3ff + movt r10, #0 + cmp r9, r10 @ skip spurious interrupt 1023 + beq out + movw r10, #0x3fe @ ...and 1022 + cmp r9, r10 + beq out + str r9, [r8, #GICC_EOIR] @ acknowledge the interrupt + dsb + + @ Compute CPU number + lsr r9, r9, #10 + and r9, r9, #0xf + + movw r8, #(SUN7I_CPUCFG_BASE & 0xffff) + movt r8, #(SUN7I_CPUCFG_BASE >> 16) + + @ Wait for the core to enter WFI + lsl r11, r9, #6 @ x64 + add r11, r11, r8 + +1: ldr r10, [r11, #0x48] + tst r10, #(1 << 2) + bne 2f + timer_wait r10, ONE_MS + b 1b + + @ Reset CPU +2: mov r10, #0 + str r10, [r11, #0x40] + + @ Lock CPU + mov r10, #1 + lsl r9, r10, r9 @ r9 is now CPU mask + ldr r10, [r8, #0x1e4] + bic r10, r10, r9 + str r10, [r8, #0x1e4] + + @ Set power gating + ldr r10, [r8, #0x1b4] + orr r10, r10, #1 + str r10, [r8, #0x1b4] + timer_wait r10, ONE_MS + + @ Activate power clamp + mov r10, #1 +1: str r10, [r8, #0x1b0] + lsl r10, r10, #1 + orr r10, r10, #1 + tst r10, #0x100 + beq 1b + + @ Restore security level +out: mcr p15, 0, r7, c1, c1, 0 + + pop {r0-r12} + subs pc, lr, #4 + @ r1 = target CPU @ r2 = target PC .globl psci_cpu_on @@ -144,6 +239,53 @@ psci_cpu_on: _target_pc: .word 0

+/* Imported from Linux kernel */ +v7_flush_dcache_all: + dmb @ ensure ordering with previous memory accesses + mrc p15, 1, r0, c0, c0, 1 @ read clidr + ands r3, r0, #0x7000000 @ extract loc from clidr + mov r3, r3, lsr #23 @ left align loc bit field + beq finished @ if loc is 0, then no need to clean + mov r10, #0 @ start clean at cache level 0 +flush_levels: + add r2, r10, r10, lsr #1 @ work out 3x current cache level + mov r1, r0, lsr r2 @ extract cache type bits from clidr + and r1, r1, #7 @ mask of the bits for current cache only + cmp r1, #2 @ see what cache we have at this level + blt skip @ skip if no cache, or just i-cache + mrs r9, cpsr @ make cssr&csidr read atomic + mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr + isb @ isb to sych the new cssr&csidr + mrc p15, 1, r1, c0, c0, 0 @ read the new csidr + msr cpsr_c, r9 + and r2, r1, #7 @ extract the length of the cache lines + add r2, r2, #4 @ add 4 (line length offset) + ldr r4, =0x3ff + ands r4, r4, r1, lsr #3 @ find maximum number on the way size + clz r5, r4 @ find bit position of way size increment + ldr r7, =0x7fff + ands r7, r7, r1, lsr #13 @ extract max number of the index size +loop1: + mov r9, r7 @ create working copy of max index +loop2: + orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 + orr r11, r11, r9, lsl r2 @ factor index number into r11 + mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way + subs r9, r9, #1 @ decrement the index + bge loop2 + subs r4, r4, #1 @ decrement the way + bge loop1 +skip: + add r10, r10, #2 @ increment cache number + cmp r3, r10 + bgt flush_levels +finished: + mov r10, #0 @ swith back to cache level 0 + mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr + dsb st + isb + bx lr + _sunxi_cpu_entry: @ Set SMP bit mrc p15, 0, r0, c1, c0, 1 @@ -158,5 +300,34 @@ _sunxi_cpu_entry: ldr r0, [r0] b _do_nonsec_entry

+.globl psci_cpu_off +psci_cpu_off: + mrc p15, 0, r0, c1, c0, 0 @ SCTLR + bic r0, r0, #(1 << 2) @ Clear C bit + mcr p15, 0, r0, c1, c0, 0 @ SCTLR + isb + dsb + + bl v7_flush_dcache_all + + clrex @ Why??? + + mrc p15, 0, r0, c1, c0, 1 @ ACTLR + bic r0, r0, #(1 << 6) @ Clear SMP bit + mcr p15, 0, r0, c1, c0, 1 @ ACTLR + isb + dsb + + @ Ask CPU0 to pull the rug... + movw r0, #(GICD_BASE & 0xffff) + movt r0, #(GICD_BASE >> 16) + movw r1, #15 @ SGI15 + movt r1, #1 @ Target is CPU0 + str r1, [r0, #GICD_SGIR] + dsb + +1: wfi + b 1b + text_end: .popsection

From: Jan Kiszka jan.kiszka@siemens.com

This extends the PSCI support for the A20 to a dual v0.2 and v0.1 interface. Recent OSes will prefer v0.2, olders will still find the original interface, just at v0.2 service IDs.

In addition to the existing services, v0.2 requires us to implement both system off and reset. At least Linux will make use of them in favor of its own implementations and, thus, fail if they do not work.

Signed-off-by: Jan Kiszka jan.kiszka@siemens.com --- arch/arm/cpu/armv7/psci.S | 35 +++++++++- arch/arm/cpu/armv7/sunxi/psci.S | 117 ++++++++++++++++++++++++++++++++++ arch/arm/cpu/armv7/virt-dt.c | 5 +- arch/arm/include/asm/arch-sunxi/cpu.h | 2 + arch/arm/include/asm/psci.h | 24 +++++-- 5 files changed, 173 insertions(+), 10 deletions(-)

diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S index bf11a34..e2a38ca 100644 --- a/arch/arm/cpu/armv7/psci.S +++ b/arch/arm/cpu/armv7/psci.S @@ -49,8 +49,18 @@ ENTRY(psci_cpu_suspend) ENTRY(psci_cpu_off) ENTRY(psci_cpu_on) ENTRY(psci_migrate) +ENTRY(psci_affinity_info) +ENTRY(psci_migrate_info_type) +ENTRY(psci_migrate_info_up_cpu) +ENTRY(psci_system_off) +ENTRY(psci_system_reset) mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) mov pc, lr +ENDPROC(psci_system_reset) +ENDPROC(psci_system_off) +ENDPROC(psci_migrate_info_up_cpu) +ENDPROC(psci_migrate_info_type) +ENDPROC(psci_affinity_info) ENDPROC(psci_migrate) ENDPROC(psci_cpu_on) ENDPROC(psci_cpu_off) @@ -59,16 +69,33 @@ ENDPROC(psci_cpu_suspend) .weak psci_cpu_off .weak psci_cpu_on .weak psci_migrate +.weak psci_affinity_info +.weak psci_migrate_info_type +.weak psci_migrate_info_up_cpu +.weak psci_system_off +.weak psci_system_reset

_psci_table: + .word ARM_PSCI_FN_PSCI_VERSION + .word psci_version .word ARM_PSCI_FN_CPU_SUSPEND .word psci_cpu_suspend .word ARM_PSCI_FN_CPU_OFF .word psci_cpu_off .word ARM_PSCI_FN_CPU_ON .word psci_cpu_on + .word ARM_PSCI_FN_AFFINITY_INFO + .word psci_affinity_info .word ARM_PSCI_FN_MIGRATE .word psci_migrate + .word ARM_PSCI_FN_MIGRATE_INFO_TYPE + .word psci_migrate_info_type + .word ARM_PSCI_FN_MIGRATE_INFO_UP_CPU + .word psci_migrate_info_up_cpu + .word ARM_PSCI_FN_SYSTEM_OFF + .word psci_system_off + .word ARM_PSCI_FN_SYSTEM_RESET + .word psci_system_reset .word 0 .word 0

@@ -86,7 +113,7 @@ _smc_psci: ldr r6, [r4, #4] @ Load target PC cmp r5, #0 @ If reach the end, bail out moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) - beq 2f + beq return cmp r0, r5 @ If not matching, try next entry addne r4, r4, #8 bne 1b @@ -94,9 +121,13 @@ _smc_psci: blx r6 @ Execute PSCI function

@ Switch back to non-secure -2: mcr p15, 0, r7, c1, c1, 0 +return: mcr p15, 0, r7, c1, c1, 0

pop {r4-r7, lr} movs pc, lr @ Return to the kernel

+psci_version: + mov r0, #0x00000002 @ Version 0.2 + b return + .popsection diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index 739ce77..d1fa33d 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -2,6 +2,9 @@ * Copyright (C) 2013 - ARM Ltd * Author: Marc Zyngier marc.zyngier@arm.com * + * Copyright (C) Siemens AG, 2014 + * Author: Jan Kiszka jan.kiszka@siemens.com + * * Based on code by Carl van Schaik carl@ok-labs.com. * * This program is free software; you can redistribute it and/or modify @@ -42,6 +45,35 @@ #define GICD_BASE 0x1c81000 #define GICC_BASE 0x1c82000

+#define TWI_DATA 0x0008 +#define TWI_CNTR 0x000c +#define TWI_STAT 0x0010 +#define TWI_CCR 0x0014 +#define TWI_SRST 0x0018 + +#define TWI_CNTR_STOP (1 << 4) +#define TWI_CNTR_START (1 << 5) +#define TWI_CNTR_BUSEN (1 << 6) + +#define TWI_STAT_START_SENT 0x08 +#define TWI_STAT_ADDR_ACK 0x18 +#define TWI_STAT_DATA_ACK 0x28 + +#define TWI_CCR_100KHZ ((11 << 3) | 2) + +#define AXP209_ADDR 0x34 +#define AXP209_REG_SHUTDOWN 0x32 +#define AXP209_SHUTDOWN_CTRL 0x80 + +#define WDOG_CTL 0x00 +#define WDOG_MODE 0x04 + +#define WDOG_CTL_RSTART (1 << 0) + +#define WDOG_MODE_EN (1 << 0) +#define WDOG_MODE_RSTEN (1 << 1) +#define WDOG_MODE_0_5_S (0x0 << 3) + .macro timer_wait reg, ticks @ Program CNTP_TVAL movw \reg, #(\ticks & 0xffff) @@ -176,10 +208,13 @@ out: mcr p15, 0, r7, c1, c1, 0

@ r1 = target CPU @ r2 = target PC + @ r3 = context (v0.2 only) .globl psci_cpu_on psci_cpu_on: adr r0, _target_pc str r2, [r0] + adr r0, _context + str r3, [r0] dsb

movw r0, #(SUN7I_CPUCFG_BASE & 0xffff) @@ -238,6 +273,8 @@ psci_cpu_on:

_target_pc: .word 0 +_context: + .word 0

/* Imported from Linux kernel */ v7_flush_dcache_all: @@ -298,6 +335,8 @@ _sunxi_cpu_entry:

adr r0, _target_pc ldr r0, [r0] + adr r1, _context + ldr r1, [r1] b _do_nonsec_entry

.globl psci_cpu_off @@ -329,5 +368,83 @@ psci_cpu_off: 1: wfi b 1b

+/* + * r0: TWI base address + * r1: state to wait for + */ +twi_wait: +1: ldr r2, [r0, #TWI_STAT] @ Read state + and r2, r2, #0xff @ Mask out state bits + cmp r2, r1 @ State reached? + bne 1b + + bx lr + +/* + * r0: TWI base address + * r1: data value to be sent + */ +twi_send: + str r1, [r0, #TWI_DATA] @ Write outgoing data value + mov r1, #(TWI_CNTR_BUSEN) @ Trigger transmission + str r1, [r0, #TWI_CNTR] + + bx lr + +.globl psci_system_off +psci_system_off: + movw r0, #(SUNXI_TWI0_BASE & 0xffff) + movt r0, #(SUNXI_TWI0_BASE >> 16) + + @ Perform soft-reset + mov r1, #0 + str r1, [r0, #TWI_SRST] + + @ Configure speed + mov r1, #TWI_CCR_100KHZ + str r1, [r0, #TWI_CCR] + + @ Send STOP (just in case), then transmit START condition + mov r1, #(TWI_CNTR_BUSEN | TWI_CNTR_START | TWI_CNTR_STOP) + str r1, [r0, #TWI_CNTR] + mov r1, #TWI_STAT_START_SENT + bl twi_wait + + @ Send AXP209 address + mov r1, #(AXP209_ADDR << 1) + bl twi_send + mov r1, #TWI_STAT_ADDR_ACK + bl twi_wait + + @ Select AXP209 register + mov r1, #AXP209_REG_SHUTDOWN + bl twi_send + mov r1, #TWI_STAT_DATA_ACK + bl twi_wait + + @ Request shutdown + mov r1, #AXP209_SHUTDOWN_CTRL + bl twi_send + mov r1, #TWI_STAT_DATA_ACK + bl twi_wait + + @ Complete the transmission with a STOP condition + mov r1, #TWI_CNTR_STOP + str r1, [r0, #TWI_CNTR] + +1: b 1b + +.globl psci_system_reset +psci_system_reset: + movw r0, #(SUNXI_WDOG_CTL & 0xffff) + movt r0, #(SUNXI_WDOG_CTL >> 16) + + mov r1, #(WDOG_MODE_RSTEN | WDOG_MODE_EN | WDOG_MODE_0_5_S) + str r1, [r0, #WDOG_MODE] + + mov r1, #WDOG_CTL_RSTART + str r1, [r0, #WDOG_CTL] +1: b 1b + text_end: .popsection diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c index 0b0d6a7..f529d51 100644 --- a/arch/arm/cpu/armv7/virt-dt.c +++ b/arch/arm/cpu/armv7/virt-dt.c @@ -66,7 +66,10 @@ static int fdt_psci(void *fdt) return nodeoff; }

- tmp = fdt_setprop_string(fdt, nodeoff, "compatible", "arm,psci"); + tmp = fdt_setprop_string(fdt, nodeoff, "compatible", "arm,psci-0.2"); + if (tmp) + return tmp; + tmp = fdt_appendprop_string(fdt, nodeoff, "compatible", "arm,psci"); if (tmp) return tmp; tmp = fdt_setprop_string(fdt, nodeoff, "method", "smc"); diff --git a/arch/arm/include/asm/arch-sunxi/cpu.h b/arch/arm/include/asm/arch-sunxi/cpu.h index bcfa00d..faf407b 100644 --- a/arch/arm/include/asm/arch-sunxi/cpu.h +++ b/arch/arm/include/asm/arch-sunxi/cpu.h @@ -138,6 +138,8 @@

#define SUNXI_BROM_BASE 0xffff0000 /* 32 kiB */

+#define SUNXI_WDOG_CTL (SUNXI_TIMER_BASE + 0x90) + #define SUNXI_CPU_CFG (SUNXI_TIMER_BASE + 0x13c)

/* SS bonding ids used for cpu identification */ diff --git a/arch/arm/include/asm/psci.h b/arch/arm/include/asm/psci.h index 704b4b0..234475a 100644 --- a/arch/arm/include/asm/psci.h +++ b/arch/arm/include/asm/psci.h @@ -18,18 +18,28 @@ #ifndef __ARM_PSCI_H__ #define __ARM_PSCI_H__

-/* PSCI interface */ -#define ARM_PSCI_FN_BASE 0x95c1ba5e -#define ARM_PSCI_FN(n) (ARM_PSCI_FN_BASE + (n)) +#define ARM_PSCI_FN32_BASE 0x84000000 +#define ARM_PSCI_FN(n) (ARM_PSCI_FN32_BASE + (n))

-#define ARM_PSCI_FN_CPU_SUSPEND ARM_PSCI_FN(0) -#define ARM_PSCI_FN_CPU_OFF ARM_PSCI_FN(1) -#define ARM_PSCI_FN_CPU_ON ARM_PSCI_FN(2) -#define ARM_PSCI_FN_MIGRATE ARM_PSCI_FN(3) +#define ARM_PSCI_FN_PSCI_VERSION ARM_PSCI_FN(0) +#define ARM_PSCI_FN_CPU_SUSPEND ARM_PSCI_FN(1) +#define ARM_PSCI_FN_CPU_OFF ARM_PSCI_FN(2) +#define ARM_PSCI_FN_CPU_ON ARM_PSCI_FN(3) +#define ARM_PSCI_FN_AFFINITY_INFO ARM_PSCI_FN(4) +#define ARM_PSCI_FN_MIGRATE ARM_PSCI_FN(5) +#define ARM_PSCI_FN_MIGRATE_INFO_TYPE ARM_PSCI_FN(6) +#define ARM_PSCI_FN_MIGRATE_INFO_UP_CPU ARM_PSCI_FN(7) +#define ARM_PSCI_FN_SYSTEM_OFF ARM_PSCI_FN(8) +#define ARM_PSCI_FN_SYSTEM_RESET ARM_PSCI_FN(9)

#define ARM_PSCI_RET_SUCCESS 0 #define ARM_PSCI_RET_NI (-1) #define ARM_PSCI_RET_INVAL (-2) #define ARM_PSCI_RET_DENIED (-3) +#define ARM_PSCI_RET_ALREADY_ON (-4) +#define ARM_PSCI_RET_ON_PENDING (-5) +#define ARM_PSCI_RET_INTERNAL_FAILURE (-6) +#define ARM_PSCI_RET_NOT_PRESENT (-7) +#define ARM_PSCI_RET_DISABLED (-8)

#endif /* __ARM_PSCI_H__ */