[U-Boot] [PATCH v2 0/5] sun7i: PSCI enhancements
This adds CPU offlining and PSCI v0.2 support. Changes since v1: - add AFFINITY_INFO support (mandatory for v0.2) - rebase over master - include "sunxi: Align PSCI stack calculation to comment" to provide the complete queue
See patches for further details.
Jan
Jan Kiszka (5): sunxi: Align PSCI stack calculation to comment sun7i: Add support for taking CPUs offline via PSCI sun7i: Add PSCI v0.2 support sun7i: Move psci_arch_init close to text_end sun7i: Implement PSCI v0.2 AFFINITY_INFO
arch/arm/cpu/armv7/psci.S | 35 +++- arch/arm/cpu/armv7/sunxi/psci.S | 350 ++++++++++++++++++++++++++++++++-- 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, 391 insertions(+), 25 deletions(-)
From: Jan Kiszka jan.kiszka@siemens.com
0x400 is true 1K.
Signed-off-by: Jan Kiszka jan.kiszka@siemens.com --- arch/arm/cpu/armv7/sunxi/psci.S | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index b9ea78b..a84807d 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -68,7 +68,7 @@ psci_arch_init:
mrc p15, 0, r4, c0, c0, 5 @ MPIDR and r4, r4, #3 @ cpu number in cluster - mov r5, #400 @ 1kB of stack per CPU + mov r5, #0x400 @ 1kB of stack per CPU mul r4, r4, r5
adr r5, text_end @ end of text
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 | 170 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 170 insertions(+)
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index a84807d..0aa4007 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,77 @@ 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 + + movw r8, #(GICC_BASE & 0xffff) + movt r8, #(GICC_BASE >> 16) + ldr r9, [r8, #GICC_IAR] + movw r10, #0x3ff + movt r10, #0 + cmp r9, r10 + beq out + movw r10, #0x3fe + cmp r9, r10 + beq out + str r9, [r8, #GICC_EOIR] + 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 +238,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 +299,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 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__ */
From: Jan Kiszka jan.kiszka@siemens.com
"adr rX, text_end" only works if the label is close. Adding further code to the other functions will prevent this. So move the containing function close to label. No functional change.
Signed-off-by: Jan Kiszka jan.kiszka@siemens.com --- arch/arm/cpu/armv7/sunxi/psci.S | 80 ++++++++++++++++++++--------------------- 1 file changed, 40 insertions(+), 40 deletions(-)
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index bba1894..a5b2b0c 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -94,46 +94,6 @@ isb .endm
-.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 - - mrc p15, 0, r4, c0, c0, 5 @ MPIDR - and r4, r4, #3 @ cpu number in cluster - mov r5, #0x400 @ 1kB of stack per CPU - mul r4, r4, r5 - - adr r5, text_end @ end of text - add r5, r5, #0x2000 @ Skip two pages - lsr r5, r5, #12 @ Align to start of page - lsl r5, r5, #12 - sub sp, r5, r4 @ here's our stack! - - bx lr - .globl psci_fiq_enter psci_fiq_enter: push {r0-r12} @@ -445,5 +405,45 @@ psci_system_reset: str r1, [r0, #WDOG_CTL] 1: b 1b
+.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 + + mrc p15, 0, r4, c0, c0, 5 @ MPIDR + and r4, r4, #3 @ cpu number in cluster + mov r5, #0x400 @ 1kB of stack per CPU + mul r4, r4, r5 + + adr r5, text_end @ end of text + add r5, r5, #0x2000 @ Skip two pages + lsr r5, r5, #12 @ Align to start of page + lsl r5, r5, #12 + sub sp, r5, r4 @ here's our stack! + + bx lr + text_end: .popsection
From: Jan Kiszka jan.kiszka@siemens.com
It's mandatory according to the spec, and Linux uses it for checking if an offlined CPU is already dead. Without this implemented, we get some warnings on the kernel console at least.
Signed-off-by: Jan Kiszka jan.kiszka@siemens.com --- arch/arm/cpu/armv7/sunxi/psci.S | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+)
diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S index a5b2b0c..e62e295 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.S +++ b/arch/arm/cpu/armv7/sunxi/psci.S @@ -159,6 +159,12 @@ psci_fiq_enter: tst r10, #0x100 beq 1b
+ @ Mark CPU disabled + adr r2, _cpu_off + ldr r0, [r2] + orr r0, r0, r9 + str r0, [r2] + @ Restore security level out: mcr p15, 0, r7, c1, c1, 0
@@ -227,6 +233,12 @@ psci_cpu_on: orr r6, r6, r4 str r6, [r0, #0x1e4]
+ @ Mark CPU enabled + adr r2, _cpu_off + ldr r0, [r2] + bic r0, r0, r4 + str r0, [r2] + mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS mov pc, lr
@@ -234,6 +246,8 @@ _target_pc: .word 0 _context: .word 0 +_cpu_off: + .word 0x2
/* Imported from Linux kernel */ v7_flush_dcache_all: @@ -327,6 +341,25 @@ psci_cpu_off: 1: wfi b 1b
+ @ r1 = target CPU + @ r2 = lowest affinity level +.globl psci_affinity_info +psci_affinity_info: + @ only support full match + cmp r2, #0 + mov r0, #ARM_PSCI_RET_INVAL + bne 1 + + @ prepare CPU ID in r1, only caring about the first cluster + and r1, r1, #3 + + adr r2, _cpu_off + ldr r0, [r2] @ load bitmap of offlined CPUs + lsr r0, r0, r1 @ shift right by CPU ID + and r0, r0, #1 @ filter out relevant bit + +1: mov pc, lr + /* * r0: TWI base address * r1: state to wait for
On Tue, 2014-12-30 at 18:14 +0100, Jan Kiszka wrote:
I had a couple of comments on v1 a few days ago (a bit late, sorry) which I think you may have missed? AFIACT they still stand on this version.
This adds CPU offlining and PSCI v0.2 support. Changes since v1:
- add AFFINITY_INFO support (mandatory for v0.2)
- rebase over master
- include "sunxi: Align PSCI stack calculation to comment" to provide the complete queue
Would be better to base on #next at this point I think, which would include the first patch already.
Ian.
See patches for further details.
Jan
Jan Kiszka (5): sunxi: Align PSCI stack calculation to comment sun7i: Add support for taking CPUs offline via PSCI sun7i: Add PSCI v0.2 support sun7i: Move psci_arch_init close to text_end sun7i: Implement PSCI v0.2 AFFINITY_INFO
arch/arm/cpu/armv7/psci.S | 35 +++- arch/arm/cpu/armv7/sunxi/psci.S | 350 ++++++++++++++++++++++++++++++++-- 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, 391 insertions(+), 25 deletions(-)
On 2014-12-31 12:30, Ian Campbell wrote:
On Tue, 2014-12-30 at 18:14 +0100, Jan Kiszka wrote:
I had a couple of comments on v1 a few days ago (a bit late, sorry) which I think you may have missed? AFIACT they still stand on this version.
This adds CPU offlining and PSCI v0.2 support. Changes since v1:
- add AFFINITY_INFO support (mandatory for v0.2)
- rebase over master
- include "sunxi: Align PSCI stack calculation to comment" to provide the complete queue
Would be better to base on #next at this point I think, which would include the first patch already.
Will rebase over your tree and address the comment.
Jan
participants (2)
-
Ian Campbell -
Jan Kiszka