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 
m10v_boot_secondary(unsigned int l_cpu,struct task_struct * idle)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 
m10v_smp_init(unsigned int max_cpus)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
m10v_cpu_die(unsigned int l_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 
m10v_cpu_kill(unsigned int l_cpu)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 
m10v_pm_valid(suspend_state_t state)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 
m10v_die(unsigned long arg)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 
m10v_pm_enter(suspend_state_t state)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 
m10v_pm_init(void)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);
148