arch/arm/mach-milbeaut/platsmp.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright:   (C) 2018 Socionext Inc.
   4  * Copyright:   (C) 2015 Linaro Ltd.
   5  */
   6
   7 #include <linux/cpu_pm.h>
   8 #include <linux/irqchip/arm-gic.h>
   9 #include <linux/of_address.h>
  10 #include <linux/suspend.h>
  11
  12 #include <asm/cacheflush.h>
  13 #include <asm/cp15.h>
  14 #include <asm/idmap.h>
  15 #include <asm/smp_plat.h>
  16 #include <asm/suspend.h>
  17
  18 #define M10V_MAX_CPU    4
  19 #define KERNEL_UNBOOT_FLAG      0x12345678
  20
  21 static void __iomem *m10v_smp_base;
  22
  23 static int m10v_boot_secondary(unsigned int l_cpu, struct task_struct *idle)
  24 {
  25         unsigned int mpidr, cpu, cluster;
  26
  27         if (!m10v_smp_base)
  28                 return -ENXIO;
  29
  30         mpidr = cpu_logical_map(l_cpu);
  31         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
  32         cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
  33
  34         if (cpu >= M10V_MAX_CPU)
  35                 return -EINVAL;
  36
  37         pr_info("%s: cpu %u l_cpu %u cluster %u\n",
  38                         __func__, cpu, l_cpu, cluster);
  39
  40         writel(__pa_symbol(secondary_startup), m10v_smp_base + cpu * 4);
  41         arch_send_wakeup_ipi_mask(cpumask_of(l_cpu));
  42
  43         return 0;
  44 }
  45
  46 static void m10v_smp_init(unsigned int max_cpus)
  47 {
  48         unsigned int mpidr, cpu, cluster;
  49         struct device_node *np;
  50
  51         np = of_find_compatible_node(NULL, NULL, "socionext,milbeaut-smp-sram");
  52         if (!np)
  53                 return;
  54
  55         m10v_smp_base = of_iomap(np, 0);
  56         if (!m10v_smp_base)
  57                 return;
  58
  59         mpidr = read_cpuid_mpidr();
  60         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
  61         cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
  62         pr_info("MCPM boot on cpu_%u cluster_%u\n", cpu, cluster);
  63
  64         for (cpu = 0; cpu < M10V_MAX_CPU; cpu++)
  65                 writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
  66 }
  67
  68 #ifdef CONFIG_HOTPLUG_CPU
  69 static void m10v_cpu_die(unsigned int l_cpu)
  70 {
  71         gic_cpu_if_down(0);
  72         v7_exit_coherency_flush(louis);
  73         wfi();
  74 }
  75
  76 static int m10v_cpu_kill(unsigned int l_cpu)
  77 {
  78         unsigned int mpidr, cpu;
  79
  80         mpidr = cpu_logical_map(l_cpu);
  81         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
  82
  83         writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
  84
  85         return 1;
  86 }
  87 #endif
  88
  89 static struct smp_operations m10v_smp_ops __initdata = {
  90         .smp_prepare_cpus       = m10v_smp_init,
  91         .smp_boot_secondary     = m10v_boot_secondary,
  92 #ifdef CONFIG_HOTPLUG_CPU
  93         .cpu_die                = m10v_cpu_die,
  94         .cpu_kill               = m10v_cpu_kill,
  95 #endif
  96 };
  97 CPU_METHOD_OF_DECLARE(m10v_smp, "socionext,milbeaut-m10v-smp", &m10v_smp_ops);
  98
  99 static int m10v_pm_valid(suspend_state_t state)
 100 {
 101         return (state == PM_SUSPEND_STANDBY) || (state == PM_SUSPEND_MEM);
 102 }
 103
 104 typedef void (*phys_reset_t)(unsigned long);
 105 static phys_reset_t phys_reset;
 106
 107 static int m10v_die(unsigned long arg)
 108 {
 109         setup_mm_for_reboot();
 110         asm("wfi");
 111         /* Boot just like a secondary */
 112         phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset);
 113         phys_reset(virt_to_phys(cpu_resume));
 114
 115         return 0;
 116 }
 117
 118 static int m10v_pm_enter(suspend_state_t state)
 119 {
 120         switch (state) {
 121         case PM_SUSPEND_STANDBY:
 122                 asm("wfi");
 123                 break;
 124         case PM_SUSPEND_MEM:
 125                 cpu_pm_enter();
 126                 cpu_suspend(0, m10v_die);
 127                 cpu_pm_exit();
 128                 break;
 129         }
 130         return 0;
 131 }
 132
 133 static const struct platform_suspend_ops m10v_pm_ops = {
 134         .valid          = m10v_pm_valid,
 135         .enter          = m10v_pm_enter,
 136 };
 137
 138 struct clk *m10v_clclk_register(struct device *cpu_dev);
 139
 140 static int __init m10v_pm_init(void)
 141 {
 142         if (of_machine_is_compatible("socionext,milbeaut-evb"))
 143                 suspend_set_ops(&m10v_pm_ops);
 144
 145         return 0;
 146 }
 147 late_initcall(m10v_pm_init);