[RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[giridhar.maruthy]

From: Giridhar Maruthy <giridhar.maruthy@linaro.org>

In intial boot-up, u-boot commit "3d28a181aab5e... arndale5250: Boot in Hyp
mode and enable architected timers" puts the CPUs in HYP mode.

Hence, the CPUs need to be put in HYP mode when they are hot plugged out and
plugged in back.

This patch is almost same as above u-boot patch, it additionally takes care
if the kernel is compiled with thumb-2 option.

Signed-off-by: Giridhar Maruthy <giridhar.m@samsung.com>
Signed-off-by: Inderpal Singh <inderpal.singh@linaro.org>
---
 arch/arm/mach-exynos/headsmp.S |  114 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 114 insertions(+)

diff --git a/arch/arm/mach-exynos/headsmp.S b/arch/arm/mach-exynos/headsmp.S
index 5364d4b..a837941 100644
--- a/arch/arm/mach-exynos/headsmp.S
+++ b/arch/arm/mach-exynos/headsmp.S
@@ -12,6 +12,7 @@
  */
 #include <linux/linkage.h>
 #include <linux/init.h>
+#include <asm/assembler.h>
 
 	__CPUINIT
 
@@ -20,6 +21,98 @@
  * a "holding pen" into which all secondary cores are held until we're
  * ready for them to initialise.
  */
+
+.arch_extension sec
+.arch_extension virt
+.text
+
+.align 5
+/* We use the same vector table for Hyp and Monitor mode, since
+ * we will only use each once and they don't overlap.
+ */
+mon_vectors:
+	W(b)	.	/* reset */
+	W(b)	.	/* undef */
+	W(b)	2f	/* smc */
+	W(b)	.	/* pabt */
+	W(b)	.	/* dabt */
+	W(b)	1f	/* hyp */
+	W(b)	.	/* irq */
+	W(b)	.	/* fiq */
+
+/* Return directly back to the caller without leaving Hyp mode: */
+1:	mrs	lr, elr_hyp
+	mov	pc, lr
+
+/* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */
+2:
+	mrc	p15, 0, r1, c1, c1, 0		@ SCR
+	/*
+	 * Set SCR.NS=1(needed for setting HVBAR and also returning to NS state)
+	 *        .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode)
+	 *        .FW,AW=1 (CPSR.A,F modifiable in NS state)
+	 *        .nET=0 (early termination OK)
+	 *        .SCD=0 (SMC in NS mode OK, so we can call secure firmware)
+	 *        .HCE=1 (HVC does Hyp call)
+	 */
+	bic	r1, r1, #0x07f
+	ldr	r2, =0x131
+	orr	r1, r1, r2
+	mcr	p15, 0, r2, c1, c1, 0		@ SCR
+	isb
+	ldr	r2, =mon_vectors
+
+
+	adr	r4, 1f
+	ldmia	r4, {r5}
+	sub	r4, r4, r5
+	add	r2, r2, r4
+
+	mcr	p15, 4, r2, c12, c0, 0		@ set HVBAR
+
+ THUMB(	mrc	p15, 4, r2, c1, c0, 0 )		@ ctrl register
+ THUMB(	orr	r2, r2, #1 << 30 )		@ HSCTLR.TE (Thumb exceptions)
+ THUMB(	mcr	p15, 4, r2, c1, c0, 0 )
+ THUMB(	isb)
+
+	/* ...and return to calling code in NS state */
+	movs	pc, lr
+
+
+	.globl monitor_init
+monitor_init:
+	ldr	ip, =mon_vectors
+
+	adr	r4, 1f
+	ldmia	r4, {r5}
+	sub	r4, r4, r5
+	add	ip, ip, r4
+	mcr	p15, 0, ip, c12, c0, 1
+
+ THUMB(	mrc     p15, 0, r1, c1, c0, 0 )		@ ctrl register
+ THUMB( orr     r1, r1, #1 << 30 )		@ SCTLR.TE (Thumb exceptions)
+ THUMB( mcr     p15, 0, r1, c1, c0, 0 )
+ THUMB( isb )
+
+	mov	pc, lr
+
+	/* Try to go into NS-SVC: void enter_ns(void); */
+	.globl enter_ns
+enter_ns:
+	smc	#0
+	mov	pc, lr
+
+	/* void enter_hyp(void); */
+	.globl enter_hyp
+enter_hyp:
+	/* Now we're in NS-SVC, make a Hyp call to get into Hyp mode */
+	mov	r0, lr
+	mov	r1, sp
+	hvc	#0
+	/* We will end up here in NS-Hyp. */
+	mov	sp, r1
+	mov	pc, r0
+
 ENTRY(exynos4_secondary_startup)
 	mrc	p15, 0, r0, c0, c0, 5
 	and	r0, r0, #15
@@ -31,6 +124,27 @@ pen:	ldr	r7, [r6]
 	cmp	r7, r0
 	bne	pen
 
+	ldr	r1, =__boot_cpu_mode
+	add	r1, r1, r4
+	ldr	r2, [r1]
+	mrs     r0, cpsr
+	ands	r0, r0, #MODE_MASK
+	subs	r1, r0, r2
+	beq	3f
+	subs	r2, r2, #HYP_MODE
+	bne	3f
+
+	/* Setting NSACR to allow coprocessor access from non-secure mode */
+	mrc	p15, 0, r0, c1, c1, 2
+	movw	r1, #0x3fff
+	orr	r0, r0, r1
+	mcr	p15, 0, r0, c1, c1, 2
+5:
+	bl	monitor_init
+	bl	enter_ns
+	bl	enter_hyp
+
+3:
 	/*
 	 * we've been released from the holding pen: secondary_stack
 	 * should now contain the SVC stack for this core
-- 
1.7.9.5

Re: [RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[Giridhar Maruthy]

This patch is a modification from the Christoffer Dall's u-boot
patch. This is required to put the secondary processors in hyp
mode during cpu hotplug when u-boot is no longer alive.

Marc Zyngier suggested this logic to go into firmware or, u-boot
putting a trampoline code into a page /memreserve/d by DT. But
this seemed to have a problem. Once the cpu is hotplugged in
runtime, the control is in ROM code and waits for event.
Kernel provides a return address in kernel to which the processor
jumps once it gets an event. If the control branches to the
trampoline code here, this trampoline code has no kernel return
address.

Someone with better logic or better placement of this logic
elsewhere is welcome.

Regards,
Giridhar

On 7 May 2013 11:21,  <giridhar.maruthy@linaro.org> wrote:
> From: Giridhar Maruthy <giridhar.maruthy@linaro.org>
>
> In intial boot-up, u-boot commit "3d28a181aab5e... arndale5250: Boot in Hyp
> mode and enable architected timers" puts the CPUs in HYP mode.
>
> Hence, the CPUs need to be put in HYP mode when they are hot plugged out and
> plugged in back.
>
> This patch is almost same as above u-boot patch, it additionally takes care
> if the kernel is compiled with thumb-2 option.
>
> Signed-off-by: Giridhar Maruthy <giridhar.m@samsung.com>
> Signed-off-by: Inderpal Singh <inderpal.singh@linaro.org>
> ---
>  arch/arm/mach-exynos/headsmp.S |  114 ++++++++++++++++++++++++++++++++++++++++
>  1 file changed, 114 insertions(+)
>
> diff --git a/arch/arm/mach-exynos/headsmp.S b/arch/arm/mach-exynos/headsmp.S
> index 5364d4b..a837941 100644
> --- a/arch/arm/mach-exynos/headsmp.S
> +++ b/arch/arm/mach-exynos/headsmp.S
> @@ -12,6 +12,7 @@
>   */
>  #include <linux/linkage.h>
>  #include <linux/init.h>
> +#include <asm/assembler.h>
>
>         __CPUINIT
>
> @@ -20,6 +21,98 @@
>   * a "holding pen" into which all secondary cores are held until we're
>   * ready for them to initialise.
>   */
> +
> +.arch_extension sec
> +.arch_extension virt
> +.text
> +
> +.align 5
> +/* We use the same vector table for Hyp and Monitor mode, since
> + * we will only use each once and they don't overlap.
> + */
> +mon_vectors:
> +       W(b)    .       /* reset */
> +       W(b)    .       /* undef */
> +       W(b)    2f      /* smc */
> +       W(b)    .       /* pabt */
> +       W(b)    .       /* dabt */
> +       W(b)    1f      /* hyp */
> +       W(b)    .       /* irq */
> +       W(b)    .       /* fiq */
> +
> +/* Return directly back to the caller without leaving Hyp mode: */
> +1:     mrs     lr, elr_hyp
> +       mov     pc, lr
> +
> +/* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */
> +2:
> +       mrc     p15, 0, r1, c1, c1, 0           @ SCR
> +       /*
> +        * Set SCR.NS=1(needed for setting HVBAR and also returning to NS state)
> +        *        .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode)
> +        *        .FW,AW=1 (CPSR.A,F modifiable in NS state)
> +        *        .nET=0 (early termination OK)
> +        *        .SCD=0 (SMC in NS mode OK, so we can call secure firmware)
> +        *        .HCE=1 (HVC does Hyp call)
> +        */
> +       bic     r1, r1, #0x07f
> +       ldr     r2, =0x131
> +       orr     r1, r1, r2
> +       mcr     p15, 0, r2, c1, c1, 0           @ SCR
> +       isb
> +       ldr     r2, =mon_vectors
> +
> +
> +       adr     r4, 1f
> +       ldmia   r4, {r5}
> +       sub     r4, r4, r5
> +       add     r2, r2, r4
> +
> +       mcr     p15, 4, r2, c12, c0, 0          @ set HVBAR
> +
> + THUMB(        mrc     p15, 4, r2, c1, c0, 0 )         @ ctrl register
> + THUMB(        orr     r2, r2, #1 << 30 )              @ HSCTLR.TE (Thumb exceptions)
> + THUMB(        mcr     p15, 4, r2, c1, c0, 0 )
> + THUMB(        isb)
> +
> +       /* ...and return to calling code in NS state */
> +       movs    pc, lr
> +
> +
> +       .globl monitor_init
> +monitor_init:
> +       ldr     ip, =mon_vectors
> +
> +       adr     r4, 1f
> +       ldmia   r4, {r5}
> +       sub     r4, r4, r5
> +       add     ip, ip, r4
> +       mcr     p15, 0, ip, c12, c0, 1
> +
> + THUMB(        mrc     p15, 0, r1, c1, c0, 0 )         @ ctrl register
> + THUMB( orr     r1, r1, #1 << 30 )             @ SCTLR.TE (Thumb exceptions)
> + THUMB( mcr     p15, 0, r1, c1, c0, 0 )
> + THUMB( isb )
> +
> +       mov     pc, lr
> +
> +       /* Try to go into NS-SVC: void enter_ns(void); */
> +       .globl enter_ns
> +enter_ns:
> +       smc     #0
> +       mov     pc, lr
> +
> +       /* void enter_hyp(void); */
> +       .globl enter_hyp
> +enter_hyp:
> +       /* Now we're in NS-SVC, make a Hyp call to get into Hyp mode */
> +       mov     r0, lr
> +       mov     r1, sp
> +       hvc     #0
> +       /* We will end up here in NS-Hyp. */
> +       mov     sp, r1
> +       mov     pc, r0
> +
>  ENTRY(exynos4_secondary_startup)
>         mrc     p15, 0, r0, c0, c0, 5
>         and     r0, r0, #15
> @@ -31,6 +124,27 @@ pen:        ldr     r7, [r6]
>         cmp     r7, r0
>         bne     pen
>
> +       ldr     r1, =__boot_cpu_mode
> +       add     r1, r1, r4
> +       ldr     r2, [r1]
> +       mrs     r0, cpsr
> +       ands    r0, r0, #MODE_MASK
> +       subs    r1, r0, r2
> +       beq     3f
> +       subs    r2, r2, #HYP_MODE
> +       bne     3f
> +
> +       /* Setting NSACR to allow coprocessor access from non-secure mode */
> +       mrc     p15, 0, r0, c1, c1, 2
> +       movw    r1, #0x3fff
> +       orr     r0, r0, r1
> +       mcr     p15, 0, r0, c1, c1, 2
> +5:
> +       bl      monitor_init
> +       bl      enter_ns
> +       bl      enter_hyp
> +
> +3:
>         /*
>          * we've been released from the holding pen: secondary_stack
>          * should now contain the SVC stack for this core
> --
> 1.7.9.5
>

Re: [RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[Marc Zyngier]

On 07/05/13 06:54, Giridhar Maruthy wrote:
> This patch is a modification from the Christoffer Dall's u-boot
> patch. This is required to put the secondary processors in hyp
> mode during cpu hotplug when u-boot is no longer alive.
> 
> Marc Zyngier suggested this logic to go into firmware or, u-boot
> putting a trampoline code into a page /memreserve/d by DT. But
> this seemed to have a problem. Once the cpu is hotplugged in
> runtime, the control is in ROM code and waits for event.
> Kernel provides a return address in kernel to which the processor
> jumps once it gets an event. If the control branches to the
> trampoline code here, this trampoline code has no kernel return
> address.
> 
> Someone with better logic or better placement of this logic
> elsewhere is welcome.

What prevents you from writing the kernel address in the memreserved
page? Some obvious location, like the last word of the page? You only
have to do it once (from the boot CPU, for example).

Or did I miss something else?

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

Re: [RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[Giridhar Maruthy]

On 7 May 2013 14:25, Marc Zyngier <marc.zyngier@arm.com> wrote:
> On 07/05/13 06:54, Giridhar Maruthy wrote:
>> This patch is a modification from the Christoffer Dall's u-boot
>> patch. This is required to put the secondary processors in hyp
>> mode during cpu hotplug when u-boot is no longer alive.
>>
>> Marc Zyngier suggested this logic to go into firmware or, u-boot
>> putting a trampoline code into a page /memreserve/d by DT. But
>> this seemed to have a problem. Once the cpu is hotplugged in
>> runtime, the control is in ROM code and waits for event.
>> Kernel provides a return address in kernel to which the processor
>> jumps once it gets an event. If the control branches to the
>> trampoline code here, this trampoline code has no kernel return
>> address.
>>
>> Someone with better logic or better placement of this logic
>> elsewhere is welcome.
>
> What prevents you from writing the kernel address in the memreserved
> page? Some obvious location, like the last word of the page? You only
> have to do it once (from the boot CPU, for example).
>
> Or did I miss something else?

Thanks Marc, I think I understand now.

I guess I also need to put the primary cpu boot mode into a temporary location.

-Giridhar

Re: [RFC PATCH V2] ARM: EXYNOS: Fix hotplug when CPUs boot in HYP mode

[Marc Zyngier]

On 08/05/13 13:23, Giridhar Maruthy wrote:
> On 7 May 2013 14:25, Marc Zyngier <marc.zyngier@arm.com> wrote:
>> On 07/05/13 06:54, Giridhar Maruthy wrote:
>>> This patch is a modification from the Christoffer Dall's u-boot
>>> patch. This is required to put the secondary processors in hyp
>>> mode during cpu hotplug when u-boot is no longer alive.
>>>
>>> Marc Zyngier suggested this logic to go into firmware or, u-boot
>>> putting a trampoline code into a page /memreserve/d by DT. But
>>> this seemed to have a problem. Once the cpu is hotplugged in
>>> runtime, the control is in ROM code and waits for event.
>>> Kernel provides a return address in kernel to which the processor
>>> jumps once it gets an event. If the control branches to the
>>> trampoline code here, this trampoline code has no kernel return
>>> address.
>>>
>>> Someone with better logic or better placement of this logic
>>> elsewhere is welcome.
>>
>> What prevents you from writing the kernel address in the memreserved
>> page? Some obvious location, like the last word of the page? You only
>> have to do it once (from the boot CPU, for example).
>>
>> Or did I miss something else?
> 
> Thanks Marc, I think I understand now.
> 
> I guess I also need to put the primary cpu boot mode into a temporary location.

Indeed. You'll need that for your secondaries to enter in the same mode
as the primary.

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