[PATCH 2/2] ARM: SH-Mobile: sh73a0: Add CPU Hotplug

[Rob Herring]

On 12/06/2012 06:08 AM, Bastian Hecht wrote:
> From: Bastian Hecht <hechtb at gmail.com>
> 
> Add the capability to add and remove CPUs on the fly.
> The Cortex-A9 offers the possibility to take single cores out of the
> MP Core. We add this capabilty taking care that caches are kept
> coherent. For the actual shutdown via a WFI instruction, a code snippet
> from the omap2 code tree is copied. Thanks for that! For verifying the
> shutdown we rely on the internal SH73A0 Power Status Register
> PSTR.
> 
> Signed-off-by: Bastian Hecht <hechtb+renesas at gmail.com>
> ---
>  arch/arm/mach-shmobile/headsmp-sh73a0.S      |   46 ++++++++++++++++++++++++++
>  arch/arm/mach-shmobile/include/mach/common.h |    1 +
>  arch/arm/mach-shmobile/smp-sh73a0.c          |   41 +++++++++++++++++++----
>  3 files changed, 82 insertions(+), 6 deletions(-)
> 
> diff --git a/arch/arm/mach-shmobile/headsmp-sh73a0.S b/arch/arm/mach-shmobile/headsmp-sh73a0.S
> index bec4c0d..be463a3 100644
> --- a/arch/arm/mach-shmobile/headsmp-sh73a0.S
> +++ b/arch/arm/mach-shmobile/headsmp-sh73a0.S
> @@ -23,6 +23,52 @@
>  #include <linux/init.h>
>  #include <asm/memory.h>
>  
> +/* Taken from arch/arm/mach-omap2/sleep44xx.S. Thanks! */
> +ENTRY(sh73a0_do_wfi)
> +        stmfd   sp!, {lr}

Why does the lr need to be pushed to the stack?

> +
> +        /*
> +         * Execute an ISB instruction to ensure that all of the
> +         * CP15 register changes have been committed.
> +         */
> +        isb

Generally writes to cp15 registers that need an isb already do so.

> +
> +        /*
> +         * Execute a barrier instruction to ensure that all cache,
> +         * TLB and branch predictor maintenance operations issued
> +         * by any CPU in the cluster have completed.
> +         */
> +        dsb
> +        dmb

A dsb is a superset of a dmb, so you should not need both.

> +
> +        /*
> +         * Execute a WFI instruction and wait until the
> +         * STANDBYWFI output is asserted to indicate that the
> +         * CPU is in idle and low power state. CPU can specualatively
> +         * prefetch the instructions so add NOPs after WFI. Sixteen
> +         * NOPs as per Cortex-A9 pipeline.

Why do you care what is prefetched? You're never coming back here, right?

Rob

> +         */
> +        wfi                                     @ Wait For Interrupt
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +        nop
> +
> +        ldmfd   sp!, {pc}
> +ENDPROC(sh73a0_do_wfi)
> +
>  	__CPUINIT
>  /*
>   * Reset vector for secondary CPUs.
> diff --git a/arch/arm/mach-shmobile/include/mach/common.h b/arch/arm/mach-shmobile/include/mach/common.h
> index f2e2c29..40f767e 100644
> --- a/arch/arm/mach-shmobile/include/mach/common.h
> +++ b/arch/arm/mach-shmobile/include/mach/common.h
> @@ -55,6 +55,7 @@ extern void sh73a0_add_standard_devices(void);
>  extern void sh73a0_clock_init(void);
>  extern void sh73a0_pinmux_init(void);
>  extern void sh73a0_secondary_vector(void);
> +extern void sh73a0_do_wfi(void);
>  extern struct clk sh73a0_extal1_clk;
>  extern struct clk sh73a0_extal2_clk;
>  extern struct clk sh73a0_extcki_clk;
> diff --git a/arch/arm/mach-shmobile/smp-sh73a0.c b/arch/arm/mach-shmobile/smp-sh73a0.c
> index 5e36f5d..9237e13 100644
> --- a/arch/arm/mach-shmobile/smp-sh73a0.c
> +++ b/arch/arm/mach-shmobile/smp-sh73a0.c
> @@ -24,6 +24,7 @@
>  #include <linux/io.h>
>  #include <linux/delay.h>
>  #include <mach/common.h>
> +#include <asm/cacheflush.h>
>  #include <asm/smp_plat.h>
>  #include <mach/sh73a0.h>
>  #include <asm/smp_scu.h>
> @@ -36,6 +37,8 @@
>  #define SBAR		IOMEM(0xe6180020)
>  #define APARMBAREA	IOMEM(0xe6f10020)
>  
> +#define PSTR_SHUTDOWN_MODE	3
> +
>  static void __iomem *scu_base_addr(void)
>  {
>  	return (void __iomem *)0xf0000000;
> @@ -92,16 +95,20 @@ static void __init sh73a0_smp_init_cpus(void)
>  	shmobile_smp_init_cpus(ncores);
>  }
>  
> -static int __maybe_unused sh73a0_cpu_kill(unsigned int cpu)
> +#ifdef CONFIG_HOTPLUG_CPU
> +static int sh73a0_cpu_kill(unsigned int cpu)
>  {
> +
>  	int k;
> +	u32 pstr;
>  
> -	/* this function is running on another CPU than the offline target,
> -	 * here we need wait for shutdown code in platform_cpu_die() to
> -	 * finish before asking SoC-specific code to power off the CPU core.
> +	/*
> +	 * wait until the power status register confirms the shutdown of the
> +	 * offline target
>  	 */
>  	for (k = 0; k < 1000; k++) {
> -		if (shmobile_cpu_is_dead(cpu))
> +		pstr = (__raw_readl(PSTR) >> (4 * cpu)) & 3;
> +		if (pstr == PSTR_SHUTDOWN_MODE)
>  			return 1;
>  
>  		mdelay(1);
> @@ -110,6 +117,28 @@ static int __maybe_unused sh73a0_cpu_kill(unsigned int cpu)
>  	return 0;
>  }
>  
> +static void sh73a0_cpu_die(unsigned int cpu)
> +{
> +	/*
> +	 * The ARM MPcore does not issue a cache coherency request for the L1
> +	 * cache when powering off single CPUs. We must take care of this and
> +	 * further caches.
> +	 */
> +	dsb();
> +	flush_cache_all();
> +
> +	/* Set power off mode. This takes the CPU out of the MP cluster */
> +	scu_power_mode(scu_base_addr(), SCU_PM_POWEROFF);
> +
> +	/* Enter shutdown mode */
> +	sh73a0_do_wfi();
> +
> +	/* We assume success always. We never reach this */
> +	pr_err("Shutting down CPU failed. This should never happen!\n");
> +	for (;;)
> +		;
> +}
> +#endif /* CONFIG_HOTPLUG_CPU */
>  
>  struct smp_operations sh73a0_smp_ops __initdata = {
>  	.smp_init_cpus		= sh73a0_smp_init_cpus,
> @@ -118,7 +147,7 @@ struct smp_operations sh73a0_smp_ops __initdata = {
>  	.smp_boot_secondary	= sh73a0_boot_secondary,
>  #ifdef CONFIG_HOTPLUG_CPU
>  	.cpu_kill		= sh73a0_cpu_kill,
> -	.cpu_die		= shmobile_cpu_die,
> +	.cpu_die		= sh73a0_cpu_die,
>  	.cpu_disable		= shmobile_cpu_disable,
>  #endif
>  };
>