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 0aa4007..bba1894 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) @@ -175,10 +207,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) @@ -237,6 +272,8 @@ psci_cpu_on:
_target_pc: .word 0 +_context: + .word 0
/* Imported from Linux kernel */ v7_flush_dcache_all: @@ -297,6 +334,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 @@ -328,5 +367,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 2c92b5c..e887e96 100644 --- a/arch/arm/include/asm/arch-sunxi/cpu.h +++ b/arch/arm/include/asm/arch-sunxi/cpu.h @@ -137,6 +137,8 @@
#define SUNXI_BROM_BASE 0xffff0000 /* 32 kiB */
+#define SUNXI_WDOG_CTL (SUNXI_TIMER_BASE + 0x90) + #define SUNXI_CPU_CFG (SUNXI_TIMER_BASE + 0x13c)
#ifndef __ASSEMBLY__ 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__ */