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
 21static void __iomem *m10v_smp_base;
 22
 23static 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
 46static 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
 69static 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
 76static 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
 89static 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};
 97CPU_METHOD_OF_DECLARE(m10v_smp, "socionext,milbeaut-m10v-smp", &m10v_smp_ops);
 98
 99static int m10v_pm_valid(suspend_state_t state)
100{
101	return (state == PM_SUSPEND_STANDBY) || (state == PM_SUSPEND_MEM);
102}
103
104typedef void (*phys_reset_t)(unsigned long);
105static phys_reset_t phys_reset;
106
107static 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
118static 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
133static const struct platform_suspend_ops m10v_pm_ops = {
134	.valid		= m10v_pm_valid,
135	.enter		= m10v_pm_enter,
136};
137
138struct clk *m10v_clclk_register(struct device *cpu_dev);
139
140static 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}
147late_initcall(m10v_pm_init);