1 files changed, 13 insertions, 73 deletions

diff --git a/arch/arm/mach-shmobile/smp-emev2.c b/arch/arm/mach-shmobile/smp-emev2.c
index 953eb1f9388d..8225c16b371b 100644
--- a/arch/arm/mach-shmobile/smp-emev2.c
+++ b/arch/arm/mach-shmobile/smp-emev2.c
@@ -28,63 +28,9 @@
 #include <mach/emev2.h>
 #include <asm/smp_plat.h>
 #include <asm/smp_scu.h>
-#include <asm/cacheflush.h>
 
 #define EMEV2_SCU_BASE 0x1e000000
 
-static DEFINE_SPINLOCK(scu_lock);
-static void __iomem *scu_base;
-
-static void modify_scu_cpu_psr(unsigned long set, unsigned long clr)
-{
-        unsigned long tmp;
-
-        /* we assume this code is running on a different cpu
-         * than the one that is changing coherency setting */
-        spin_lock(&scu_lock);
-        tmp = readl(scu_base + 8);
-        tmp &= ~clr;
-        tmp |= set;
-        writel(tmp, scu_base + 8);
-        spin_unlock(&scu_lock);
-
-}
-
-static unsigned int __init emev2_get_core_count(void)
-{
-        if (!scu_base) {
-                scu_base = ioremap(EMEV2_SCU_BASE, PAGE_SIZE);
-                emev2_clock_init(); /* need ioremapped SMU */
-        }
-
-        WARN_ON_ONCE(!scu_base);
-
-        return scu_base ? scu_get_core_count(scu_base) : 1;
-}
-
-static int emev2_platform_cpu_kill(unsigned int cpu)
-{
-        return 0; /* not supported yet */
-}
-
-static int __maybe_unused emev2_cpu_kill(unsigned int cpu)
-{
-        int k;
-
-        /* 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.
-         */
-        for (k = 0; k < 1000; k++) {
-                if (shmobile_cpu_is_dead(cpu))
-                        return emev2_platform_cpu_kill(cpu);
-                mdelay(1);
-        }
-
-        return 0;
-}
-
-
 static void __cpuinit emev2_secondary_init(unsigned int cpu)
 {
         gic_secondary_init(0);
@@ -92,31 +38,30 @@ static void __cpuinit emev2_secondary_init(unsigned int cpu)
 
 static int __cpuinit emev2_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
-        cpu = cpu_logical_map(cpu);
-
-        /* enable cache coherency */
-        modify_scu_cpu_psr(0, 3 << (cpu * 8));
-
-        /* Tell ROM loader about our vector (in headsmp.S) */
-        emev2_set_boot_vector(__pa(shmobile_secondary_vector));
-
-        arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+        arch_send_wakeup_ipi_mask(cpumask_of(cpu_logical_map(cpu)));
         return 0;
 }
 
 static void __init emev2_smp_prepare_cpus(unsigned int max_cpus)
 {
-        int cpu = cpu_logical_map(0);
+        scu_enable(shmobile_scu_base);
 
-        scu_enable(scu_base);
+        /* Tell ROM loader about our vector (in headsmp-scu.S) */
+        emev2_set_boot_vector(__pa(shmobile_secondary_vector_scu));
 
-        /* enable cache coherency on CPU0 */
-        modify_scu_cpu_psr(0, 3 << (cpu * 8));
+        /* enable cache coherency on booting CPU */
+        scu_power_mode(shmobile_scu_base, SCU_PM_NORMAL);
 }
 
 static void __init emev2_smp_init_cpus(void)
 {
-        unsigned int ncores = emev2_get_core_count();
+        unsigned int ncores;
+
+        /* setup EMEV2 specific SCU base */
+        shmobile_scu_base = ioremap(EMEV2_SCU_BASE, PAGE_SIZE);
+        emev2_clock_init(); /* need ioremapped SMU */
+
+        ncores = shmobile_scu_base ? scu_get_core_count(shmobile_scu_base) : 1;
 
         shmobile_smp_init_cpus(ncores);
 }
@@ -126,9 +71,4 @@ struct smp_operations emev2_smp_ops __initdata = {
         .smp_prepare_cpus       = emev2_smp_prepare_cpus,
         .smp_secondary_init     = emev2_secondary_init,
         .smp_boot_secondary     = emev2_boot_secondary,
-#ifdef CONFIG_HOTPLUG_CPU
-        .cpu_kill               = emev2_cpu_kill,
-        .cpu_die                = shmobile_cpu_die,
-        .cpu_disable            = shmobile_cpu_disable,
-#endif
 };