Signed-off-by: Jan Kiszka <jan.kis...@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.zyng...@arm.com>
*
+ * Copyright (C) Siemens AG, 2014
+ * Author: Jan Kiszka <jan.kis...@siemens.com>
+ *
* Based on code by Carl van Schaik <c...@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__ */