[U-Boot] sunxi: CPU disabling

[Marc Zyngier <marc.zyngier@arm.com>]

On 26/11/14 08:44, Jan Kiszka wrote:
> On 2014-11-17 07:47, Jan Kiszka wrote:
>> 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
> 
> We have to use "subs pc, lr, #4" to return from FIQ. I guess you can
> explain better than I why, I only stumbled over this in a stackoverflow
> posting - and it works!

Of course!!! I wrote it just like the result of an SMC call, but that's
an interrupt, and the stupid "fix the 3-stage pipeline yourself" rule
applies. Very nice catch!

For those who are now thinking "WTF??", this is explained in the ARM ARM
(ARMv7 version), section B1.8.3, table B1-7.

Thanks again,

	M.
-- 
Jazz is not dead. It just smells funny...