[U-Boot] sunxi: CPU disabling (was: Re: ARM: flush_dcache_all for PSCI)

[Jan Kiszka <jan.kiszka@web.de>]

On 2014-11-10 14:10, Marc Zyngier wrote:
> On 10/11/14 12:57, Jan Kiszka wrote:
>> Hi all,
>>
>> I'm trying to get Marc's CPU hotplug-anabling patch [1] for sunxi
>> working on a B-Pi. After the first discussion it became clear that we
>> need something like flush_dcache_all in the PSCI monitor (I don't think
>> we need an icache flush, do we?). Does anyone have a clever suggestion
> 
> No, I-cache can be left alone.
> 
>> how to reuse the existing code for that? Or do we really need to
>> re-implement everything, in the worst case in assembly?
> 
> Why don't you turn the u-boot code into a set of macros, included by
> both the core u-boot code and the PSCI code?

I've now ported over v7_flush_dcache_all from the Linux kernel.

However, that didn't magically solve the remaining issues with your
patch: I'm getting crashes on CPU 0 after handling the shoot-down FIQ.
That is even then the case if I only acknowledge the FIQ on the receiver
side, don't do any fiddling with CPU1's power states. Only if I
disabling sending the FIQ from CPU 1, the system remains stable in a CPU
off/on loop.

Below the patch I'm using. Any ideas if something is wrong with the FIQ
handler or the setup of this mechanism or whatever?

Jan

---

diff --git a/arch/arm/cpu/armv7/sunxi/psci.S b/arch/arm/cpu/armv7/sunxi/psci.S
index b9ea78b..0e4bca4 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}
+	movs	pc, lr
+
 	@ r1 = target CPU
 	@ r2 = target PC
 .globl	psci_cpu_on
@@ -144,6 +238,57 @@ 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@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
+movw  r0, #0x8000
+movt  r0, #0x01c2
+mov   r1, #'!'
+str   r1, [r0]
+	bx	lr
+
 _sunxi_cpu_entry:
 	@ Set SMP bit
 	mrc	p15, 0, r0, c1, c0, 1
@@ -158,5 +303,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

-------------- next part --------------
A non-text attachment was scrubbed...
Name: signature.asc
Type: application/pgp-signature
Size: 198 bytes
Desc: OpenPGP digital signature
URL: <http://lists.denx.de/pipermail/u-boot/attachments/20141117/3428daab/attachment.pgp>