diff mbox

[v4,20/28] ARM: owl: Implement CPU enable-method for S500

Message ID 20170606005426.26446-21-afaerber@suse.de (mailing list archive)
State New, archived
Headers show

Commit Message

Andreas Färber June 6, 2017, 12:54 a.m. UTC
Allow to bring up CPU1.

Based on LeMaker linux-actions tree.

Signed-off-by: Andreas Färber <afaerber@suse.de>
---
 v3 -> v4: Unchanged
 
 v3: new
 
 arch/arm/mach-actions/Makefile  |   3 +
 arch/arm/mach-actions/headsmp.S |  68 ++++++++++++++++
 arch/arm/mach-actions/platsmp.c | 166 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 237 insertions(+)
 create mode 100644 arch/arm/mach-actions/headsmp.S
 create mode 100644 arch/arm/mach-actions/platsmp.c

Comments

Andreas Färber June 19, 2017, 2:11 a.m. UTC | #1
Am 06.06.2017 um 02:54 schrieb Andreas Färber:
> Allow to bring up CPU1.
> 
> Based on LeMaker linux-actions tree.
> 
> Signed-off-by: Andreas Färber <afaerber@suse.de>
> ---
>  v3 -> v4: Unchanged
>  
>  v3: new
>  
>  arch/arm/mach-actions/Makefile  |   3 +
>  arch/arm/mach-actions/headsmp.S |  68 ++++++++++++++++
>  arch/arm/mach-actions/platsmp.c | 166 ++++++++++++++++++++++++++++++++++++++++
>  3 files changed, 237 insertions(+)
>  create mode 100644 arch/arm/mach-actions/headsmp.S
>  create mode 100644 arch/arm/mach-actions/platsmp.c

Applied to linux-actions.git v4.13/arm branch:

https://git.kernel.org/pub/scm/linux/kernel/git/afaerber/linux-actions.git/log/?h=v4.13/arm

Regards,
Andreas
Arnd Bergmann June 21, 2017, 8:16 a.m. UTC | #2
On Tue, Jun 6, 2017 at 2:54 AM, Andreas Färber <afaerber@suse.de> wrote:
> Allow to bring up CPU1.
>
> Based on LeMaker linux-actions tree.
>
> Signed-off-by: Andreas Färber <afaerber@suse.de>
> ---
>  v3 -> v4: Unchanged
>
>  v3: new
>
>  arch/arm/mach-actions/Makefile  |   3 +
>  arch/arm/mach-actions/headsmp.S |  68 ++++++++++++++++
>  arch/arm/mach-actions/platsmp.c | 166 ++++++++++++++++++++++++++++++++++++++++

I now see build errors in linux-next:

/git/arm-soc/arch/arm/mach-actions/platsmp.c: In function 'write_pen_release':
/git/arm-soc/arch/arm/mach-actions/platsmp.c:39:2: error:
'pen_release' undeclared (first use in this function); did you mean
'seq_release'?
  pen_release = val;
  ^~~~~~~~~~~
  seq_release
/git/arm-soc/arch/arm/mach-actions/platsmp.c:39:2: note: each
undeclared identifier is reported only once for each function it
appears in
/git/arm-soc/arch/arm/mach-actions/platsmp.c: In function
's500_wakeup_secondary':
/git/arm-soc/arch/arm/mach-actions/platsmp.c:79:2: error: implicit
declaration of function 'dsb_sev'
[-Werror=implicit-function-declaration]
  dsb_sev();
  ^~~~~~~
/git/arm-soc/arch/arm/mach-actions/platsmp.c: In function
's500_smp_boot_secondary':
/git/arm-soc/arch/arm/mach-actions/platsmp.c:108:7: error:
'pen_release' undeclared (first use in this function); did you mean
'seq_release'?


> +static DEFINE_SPINLOCK(boot_lock);
> +
> +static void write_pen_release(int val)
> +{
> +       pen_release = val;
> +       smp_wmb();
> +       __cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
> +       outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
> +}
> +
> +static void s500_smp_secondary_init(unsigned int cpu)
> +{
> +       /*
> +        * let the primary processor know we're out of the
> +        * pen, then head off into the C entry point
> +        */
> +       write_pen_release(-1);
> +
> +       spin_lock(&boot_lock);
> +       spin_unlock(&boot_lock);
> +}
> +
> +void owl_secondary_startup(void);
> +
> +static int s500_wakeup_secondary(unsigned int cpu)
> +{
> +       if (cpu > 3)
> +               return -EINVAL;
> +
> +       switch (cpu) {
> +       case 2:
> +       case 3:
> +               /* CPU2/3 are power-gated */
> +               return -EINVAL;
> +       }
> +
> +       /* wait for CPUx to run to WFE instruction */
> +       udelay(200);
> +
> +       writel(virt_to_phys(owl_secondary_startup),
> +              timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
> +       writel(OWL_CPUx_FLAG_BOOT,
> +              timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
> +
> +       dsb_sev();
> +       mb();
> +
> +       return 0;
> +}
> +
> +static int s500_smp_boot_secondary(unsigned int cpu, struct task_struct *idle)
> +{
> +       unsigned long timeout;
> +       int ret;
> +
> +       ret = s500_wakeup_secondary(cpu);
> +       if (ret)
> +               return ret;
> +
> +       udelay(10);
> +
> +       spin_lock(&boot_lock);
> +
> +       /*
> +        * The secondary processor is waiting to be released from
> +        * the holding pen - release it, then wait for it to flag
> +        * that it has been released by resetting pen_release.
> +        */
> +       write_pen_release(cpu_logical_map(cpu));
> +       smp_send_reschedule(cpu);
> +
> +       timeout = jiffies + (1 * HZ);
> +       while (time_before(jiffies, timeout)) {
> +               if (pen_release == -1)
> +                       break;
> +       }
> +
> +       writel(0, timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
> +       writel(0, timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
> +
> +       spin_unlock(&boot_lock);
> +
> +       return pen_release != -1 ? -ENOSYS : 0;
> +}

This looks more complicated than necessary. Why do you need the holding
pen when you have a register to start up the CPU?

        Arnd
Arnd Bergmann June 29, 2017, 3:07 p.m. UTC | #3
>> +static int s500_smp_boot_secondary(unsigned int cpu, struct task_struct *idle)
>> +{
>> +       unsigned long timeout;
>> +       int ret;
>> +
>> +       ret = s500_wakeup_secondary(cpu);
>> +       if (ret)
>> +               return ret;
>> +
>> +       udelay(10);
>> +
>> +       spin_lock(&boot_lock);
>> +
>> +       /*
>> +        * The secondary processor is waiting to be released from
>> +        * the holding pen - release it, then wait for it to flag
>> +        * that it has been released by resetting pen_release.
>> +        */
>> +       write_pen_release(cpu_logical_map(cpu));
>> +       smp_send_reschedule(cpu);
>> +
>> +       timeout = jiffies + (1 * HZ);
>> +       while (time_before(jiffies, timeout)) {
>> +               if (pen_release == -1)
>> +                       break;
>> +       }
>> +
>> +       writel(0, timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
>> +       writel(0, timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
>> +
>> +       spin_unlock(&boot_lock);
>> +
>> +       return pen_release != -1 ? -ENOSYS : 0;
>> +}
>
> This looks more complicated than necessary. Why do you need the holding
> pen when you have a register to start up the CPU?
>

It seems you missed my question here. Can you please follow up, and
if possible send a patch to remove the pen_release logic that appears
to be unnecessary here?

        Arnd
Andreas Färber June 29, 2017, 3:22 p.m. UTC | #4
Am 29.06.2017 um 17:07 schrieb Arnd Bergmann:
>>> +static int s500_smp_boot_secondary(unsigned int cpu, struct task_struct *idle)
>>> +{
>>> +       unsigned long timeout;
>>> +       int ret;
>>> +
>>> +       ret = s500_wakeup_secondary(cpu);
>>> +       if (ret)
>>> +               return ret;
>>> +
>>> +       udelay(10);
>>> +
>>> +       spin_lock(&boot_lock);
>>> +
>>> +       /*
>>> +        * The secondary processor is waiting to be released from
>>> +        * the holding pen - release it, then wait for it to flag
>>> +        * that it has been released by resetting pen_release.
>>> +        */
>>> +       write_pen_release(cpu_logical_map(cpu));
>>> +       smp_send_reschedule(cpu);
>>> +
>>> +       timeout = jiffies + (1 * HZ);
>>> +       while (time_before(jiffies, timeout)) {
>>> +               if (pen_release == -1)
>>> +                       break;
>>> +       }
>>> +
>>> +       writel(0, timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
>>> +       writel(0, timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
>>> +
>>> +       spin_unlock(&boot_lock);
>>> +
>>> +       return pen_release != -1 ? -ENOSYS : 0;
>>> +}
>>
>> This looks more complicated than necessary. Why do you need the holding
>> pen when you have a register to start up the CPU?
>>
> 
> It seems you missed my question here. Can you please follow up, and
> if possible send a patch to remove the pen_release logic that appears
> to be unnecessary here?

I do not have any documentation on these registers, only the downstream
code that I forward-ported here. If you tell me what you mean exactly, I
can do some testing and if it still works submit a patch to simplify it.

Comments from the so far quiet Actions Semi side would help, too.

Regards,
Andreas
Arnd Bergmann June 29, 2017, 3:50 p.m. UTC | #5
On Thu, Jun 29, 2017 at 5:22 PM, Andreas Färber <afaerber@suse.de> wrote:
> Am 29.06.2017 um 17:07 schrieb Arnd Bergmann:
>>
>> It seems you missed my question here. Can you please follow up, and
>> if possible send a patch to remove the pen_release logic that appears
>> to be unnecessary here?
>
> I do not have any documentation on these registers, only the downstream
> code that I forward-ported here. If you tell me what you mean exactly, I
> can do some testing and if it still works submit a patch to simplify it.
>
> Comments from the so far quiet Actions Semi side would help, too.

IIRC, there are two ways to implement SMP bootup: either you have
registers to tell the secondary CPU how to start up out of reset or they
get put into a holding pen during early boot where they spin waiting
for a variable to get written.

However, doing both is not necessary. See for example mach-sunxi
for an example without the holding pen. I think you can just delete
half of your file to do the same.

        Arnd
刘炜 July 1, 2017, 4:42 a.m. UTC | #6
Hi, Andrea

OWL_CPUx_ADDR is the physical address of CPUx wakeup function.
OWL_CPUx_FLAG is a valid flag of OWL_CPUx_ADDR. 

After CPUxs are wakeuped by SEV instruction, they will check their own OWL_CPUx_FLAG register. If the register vlaue is 0x55aa, CPUx will jump to OWL_CPUx_ADDR to boot up, otherwize go to sleep by WFE.

So the pen release staff is not necessary, you can remove these code safely.

BTW: CPU2/3 must exit the power gate state before wakeup, and CPU1 is always power on and has no power gate control.

Best Regards,
David Liu

-----邮件原件-----
发件人: Andreas Färber [mailto:afaerber@suse.de] 
发送时间: 2017年6月29日 23:22
收件人: Arnd Bergmann; Thomas Liau
抄送: Linux ARM; mp-cs; 张东风; 刘炜; 张天益; 96boards@ucrobotics.com; support@lemaker.org; Linux Kernel Mailing List; Russell King
主题: Re: [PATCH v4 20/28] ARM: owl: Implement CPU enable-method for S500

Am 29.06.2017 um 17:07 schrieb Arnd Bergmann:
>>> +static int s500_smp_boot_secondary(unsigned int cpu, struct 

>>> +task_struct *idle) {

>>> +       unsigned long timeout;

>>> +       int ret;

>>> +

>>> +       ret = s500_wakeup_secondary(cpu);

>>> +       if (ret)

>>> +               return ret;

>>> +

>>> +       udelay(10);

>>> +

>>> +       spin_lock(&boot_lock);

>>> +

>>> +       /*

>>> +        * The secondary processor is waiting to be released from

>>> +        * the holding pen - release it, then wait for it to flag

>>> +        * that it has been released by resetting pen_release.

>>> +        */

>>> +       write_pen_release(cpu_logical_map(cpu));

>>> +       smp_send_reschedule(cpu);

>>> +

>>> +       timeout = jiffies + (1 * HZ);

>>> +       while (time_before(jiffies, timeout)) {

>>> +               if (pen_release == -1)

>>> +                       break;

>>> +       }

>>> +

>>> +       writel(0, timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);

>>> +       writel(0, timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);

>>> +

>>> +       spin_unlock(&boot_lock);

>>> +

>>> +       return pen_release != -1 ? -ENOSYS : 0; }

>>

>> This looks more complicated than necessary. Why do you need the 

>> holding pen when you have a register to start up the CPU?

>>

> 

> It seems you missed my question here. Can you please follow up, and if 

> possible send a patch to remove the pen_release logic that appears to 

> be unnecessary here?


I do not have any documentation on these registers, only the downstream code that I forward-ported here. If you tell me what you mean exactly, I can do some testing and if it still works submit a patch to simplify it.

Comments from the so far quiet Actions Semi side would help, too.

Regards,
Andreas

--
SUSE Linux GmbH, Maxfeldstr. 5, 90409 Nürnberg, Germany
GF: Felix Imendörffer, Jane Smithard, Graham Norton HRB 21284 (AG Nürnberg)
Andreas Färber July 1, 2017, 7:56 p.m. UTC | #7
Hi David,

Am 01.07.2017 um 06:42 schrieb 刘炜:
> OWL_CPUx_ADDR is the physical address of CPUx wakeup function.
> OWL_CPUx_FLAG is a valid flag of OWL_CPUx_ADDR. 
> 
> After CPUxs are wakeuped by SEV instruction, they will check their own OWL_CPUx_FLAG register. If the register vlaue is 0x55aa, CPUx will jump to OWL_CPUx_ADDR to boot up, otherwize go to sleep by WFE.
> 
> So the pen release staff is not necessary, you can remove these code safely.

Thank you for the quick confirmation!

I have just tested a patch, and it appears to work.

Is owl_v7_invalidate_l1 necessary? mach-sunxi (that Arnd pointed to as
example) uses secondary_startup directly, without custom assembler code.

https://git.kernel.org/pub/scm/linux/kernel/git/next/linux-next.git/tree/arch/arm/mach-actions/headsmp.S

> BTW: CPU2/3 must exit the power gate state before wakeup, and CPU1 is always power on and has no power gate control.

Yes, the S500 SPS was luckily documented in the manual. The power-gating
for CPU2/3 is already implemented:

https://git.kernel.org/pub/scm/linux/kernel/git/next/linux-next.git/tree/arch/arm/mach-actions/platsmp.c

For S900 however SPS is sadly not documented... If you know who at
Actions Semi could help there, please also take a look at:
https://github.com/96boards/documentation/issues/59

Best regards,

Andreas
刘炜 July 3, 2017, 8:13 a.m. UTC | #8
Hi, Andreas

Actually the owl_v7_invalidate_l1 is not needed, and the headsmp-owl.S file can be removed if pen release staff is not used. The CPUx can jump to secondary_startup directly by setting OWL_CPUx_ADDR register.

Because S900 is aarch64 architecture, it cannot boot CPUx simply by setting SPS register. It depends on the low-level arm-trusted-firmware. Linux kernel can boot up CPUx by PSCI interface, and the arm-trusted-firmware will power on CPUx, initialize EL3 register and jump to EL2 entry in Linux kernel.

Best Regards,
David Liu

-----Original Message-----
From: Andreas Färber [mailto:afaerber@suse.de] 

Sent: Sunday, July 02, 2017 3:57 AM
To: 刘炜
Cc: Arnd Bergmann; Thomas Liau; Linux ARM; mp-cs; 张东风; 张天益; 96boards@ucrobotics.com; support@lemaker.org; Linux Kernel Mailing List; Russell King
Subject: Re: 答复: [PATCH v4 20/28] ARM: owl: Implement CPU enable-method for S500

Hi David,

Am 01.07.2017 um 06:42 schrieb 刘炜:
> OWL_CPUx_ADDR is the physical address of CPUx wakeup function.

> OWL_CPUx_FLAG is a valid flag of OWL_CPUx_ADDR. 

> 

> After CPUxs are wakeuped by SEV instruction, they will check their own OWL_CPUx_FLAG register. If the register vlaue is 0x55aa, CPUx will jump to OWL_CPUx_ADDR to boot up, otherwize go to sleep by WFE.

> 

> So the pen release staff is not necessary, you can remove these code safely.


Thank you for the quick confirmation!

I have just tested a patch, and it appears to work.

Is owl_v7_invalidate_l1 necessary? mach-sunxi (that Arnd pointed to as
example) uses secondary_startup directly, without custom assembler code.

https://git.kernel.org/pub/scm/linux/kernel/git/next/linux-next.git/tree/arch/arm/mach-actions/headsmp.S

> BTW: CPU2/3 must exit the power gate state before wakeup, and CPU1 is always power on and has no power gate control.


Yes, the S500 SPS was luckily documented in the manual. The power-gating
for CPU2/3 is already implemented:

https://git.kernel.org/pub/scm/linux/kernel/git/next/linux-next.git/tree/arch/arm/mach-actions/platsmp.c

For S900 however SPS is sadly not documented... If you know who at
Actions Semi could help there, please also take a look at:
https://github.com/96boards/documentation/issues/59

Best regards,

Andreas

-- 
SUSE Linux GmbH, Maxfeldstr. 5, 90409 Nürnberg, Germany
GF: Felix Imendörffer, Jane Smithard, Graham Norton
HRB 21284 (AG Nürnberg)
diff mbox

Patch

diff --git a/arch/arm/mach-actions/Makefile b/arch/arm/mach-actions/Makefile
index 524c3817bcb3..217e95d04b43 100644
--- a/arch/arm/mach-actions/Makefile
+++ b/arch/arm/mach-actions/Makefile
@@ -1 +1,4 @@ 
 obj-$(CONFIG_ARCH_ACTIONS) += owl.o
+obj-${CONFIG_ARCH_ACTIONS} += platsmp.o headsmp.o
+
+AFLAGS_headsmp.o := -Wa,-march=armv7-a
diff --git a/arch/arm/mach-actions/headsmp.S b/arch/arm/mach-actions/headsmp.S
new file mode 100644
index 000000000000..dc4832fc101a
--- /dev/null
+++ b/arch/arm/mach-actions/headsmp.S
@@ -0,0 +1,68 @@ 
+/*
+ * Copyright 2012 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+ENTRY(owl_v7_invalidate_l1)
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c5, 0	@ invalidate I cache
+	mcr	p15, 2, r0, c0, c0, 0
+	mrc	p15, 1, r0, c0, c0, 0
+
+	ldr	r1, =0x7fff
+	and	r2, r1, r0, lsr #13
+
+	ldr	r1, =0x3ff
+
+	and	r3, r1, r0, lsr #3	@ NumWays - 1
+	add	r2, r2, #1		@ NumSets
+
+	and	r0, r0, #0x7
+	add	r0, r0, #4	@ SetShift
+
+	clz	r1, r3		@ WayShift
+	add	r4, r3, #1	@ NumWays
+1:	sub	r2, r2, #1	@ NumSets--
+	mov	r3, r4		@ Temp = NumWays
+2:	subs	r3, r3, #1	@ Temp--
+	mov	r5, r3, lsl r1
+	mov	r6, r2, lsl r0
+	orr	r5, r5, r6	@ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
+	mcr	p15, 0, r5, c7, c6, 2
+	bgt	2b
+	cmp	r2, #0
+	bgt	1b
+	dsb
+	isb
+	mov	pc, lr
+ENDPROC(owl_v7_invalidate_l1)
+
+ENTRY(owl_secondary_startup)
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, r0, #0xf
+	adr	r4, 1f
+	ldmia	r4, {r5, r6}
+	sub	r4, r4, r5
+	add	r6, r6, r4
+pen:
+	ldr	r7, [r6]
+	cmp	r7, r0
+	bne	pen
+
+	/*
+	 * we've been released from the holding pen: secondary_stack
+	 * should now contain the SVC stack for this core
+	 */
+	bl	owl_v7_invalidate_l1
+	b	secondary_startup
+
+1:	.long	.
+	.long	pen_release
diff --git a/arch/arm/mach-actions/platsmp.c b/arch/arm/mach-actions/platsmp.c
new file mode 100644
index 000000000000..9d3601ebe535
--- /dev/null
+++ b/arch/arm/mach-actions/platsmp.c
@@ -0,0 +1,166 @@ 
+/*
+ * Actions Semi Leopard
+ *
+ * This file is based on arm realview smp platform.
+ *
+ * Copyright 2012 Actions Semi Inc.
+ * Author: Actions Semi, Inc.
+ *
+ * Copyright (c) 2017 Andreas Färber
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/smp.h>
+#include <asm/cacheflush.h>
+#include <asm/smp_plat.h>
+#include <asm/smp_scu.h>
+
+#define OWL_CPU1_ADDR	0x50
+#define OWL_CPU1_FLAG	0x5c
+
+#define OWL_CPUx_FLAG_BOOT	0x55aa
+
+static void __iomem *scu_base_addr;
+static void __iomem *timer_base_addr;
+static int ncores;
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static void write_pen_release(int val)
+{
+	pen_release = val;
+	smp_wmb();
+	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
+	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
+}
+
+static void s500_smp_secondary_init(unsigned int cpu)
+{
+	/*
+	 * let the primary processor know we're out of the
+	 * pen, then head off into the C entry point
+	 */
+	write_pen_release(-1);
+
+	spin_lock(&boot_lock);
+	spin_unlock(&boot_lock);
+}
+
+void owl_secondary_startup(void);
+
+static int s500_wakeup_secondary(unsigned int cpu)
+{
+	if (cpu > 3)
+		return -EINVAL;
+
+	switch (cpu) {
+	case 2:
+	case 3:
+		/* CPU2/3 are power-gated */
+		return -EINVAL;
+	}
+
+	/* wait for CPUx to run to WFE instruction */
+	udelay(200);
+
+	writel(virt_to_phys(owl_secondary_startup),
+	       timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
+	writel(OWL_CPUx_FLAG_BOOT,
+	       timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
+
+	dsb_sev();
+	mb();
+
+	return 0;
+}
+
+static int s500_smp_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned long timeout;
+	int ret;
+
+	ret = s500_wakeup_secondary(cpu);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	spin_lock(&boot_lock);
+
+	/*
+	 * The secondary processor is waiting to be released from
+	 * the holding pen - release it, then wait for it to flag
+	 * that it has been released by resetting pen_release.
+	 */
+	write_pen_release(cpu_logical_map(cpu));
+	smp_send_reschedule(cpu);
+
+	timeout = jiffies + (1 * HZ);
+	while (time_before(jiffies, timeout)) {
+		if (pen_release == -1)
+			break;
+	}
+
+	writel(0, timer_base_addr + OWL_CPU1_ADDR + (cpu - 1) * 4);
+	writel(0, timer_base_addr + OWL_CPU1_FLAG + (cpu - 1) * 4);
+
+	spin_unlock(&boot_lock);
+
+	return pen_release != -1 ? -ENOSYS : 0;
+}
+
+static void __init s500_smp_prepare_cpus(unsigned int max_cpus)
+{
+	struct device_node *node;
+
+	node = of_find_compatible_node(NULL, NULL, "actions,s500-timer");
+	if (!node) {
+		pr_err("%s: missing timer\n", __func__);
+		return;
+	}
+
+	timer_base_addr = of_iomap(node, 0);
+	if (!timer_base_addr) {
+		pr_err("%s: could not map timer registers\n", __func__);
+		return;
+	}
+
+	if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) {
+		node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-scu");
+		if (!node) {
+			pr_err("%s: missing scu\n", __func__);
+			return;
+		}
+
+		scu_base_addr = of_iomap(node, 0);
+		if (!scu_base_addr) {
+			pr_err("%s: could not map scu registers\n", __func__);
+			return;
+		}
+
+		/*
+		 * While the number of cpus is gathered from dt, also get the
+		 * number of cores from the scu to verify this value when
+		 * booting the cores.
+		 */
+		ncores = scu_get_core_count(scu_base_addr);
+		pr_debug("%s: ncores %d\n", __func__, ncores);
+
+		scu_enable(scu_base_addr);
+	}
+}
+
+static const struct smp_operations s500_smp_ops __initconst = {
+	.smp_prepare_cpus = s500_smp_prepare_cpus,
+	.smp_secondary_init = s500_smp_secondary_init,
+	.smp_boot_secondary = s500_smp_boot_secondary,
+};
+CPU_METHOD_OF_DECLARE(s500_smp, "actions,s500-smp", &s500_smp_ops);