xref: /kvm-unit-tests/lib/riscv/smp.c (revision 71f7db53297d39b7d9b779d6ecbd49f0d8da1dfc)
122f287f4SAndrew Jones // SPDX-License-Identifier: GPL-2.0-only
29c92b28eSAndrew Jones /*
39c92b28eSAndrew Jones  * Boot secondary CPUs
49c92b28eSAndrew Jones  *
59c92b28eSAndrew Jones  * Copyright (C) 2023, Ventana Micro Systems Inc., Andrew Jones <ajones@ventanamicro.com>
69c92b28eSAndrew Jones  */
79c92b28eSAndrew Jones #include <libcflat.h>
80c39d9eaSAndrew Jones #include <alloc_page.h>
922f287f4SAndrew Jones #include <cpumask.h>
109c92b28eSAndrew Jones #include <asm/csr.h>
1192f91f64SAndrew Jones #include <asm/io.h>
120c39d9eaSAndrew Jones #include <asm/mmu.h>
139c92b28eSAndrew Jones #include <asm/page.h>
149c92b28eSAndrew Jones #include <asm/processor.h>
159c92b28eSAndrew Jones #include <asm/sbi.h>
169c92b28eSAndrew Jones #include <asm/smp.h>
1722f287f4SAndrew Jones 
1822f287f4SAndrew Jones cpumask_t cpu_present_mask;
1922f287f4SAndrew Jones cpumask_t cpu_online_mask;
2022f287f4SAndrew Jones cpumask_t cpu_idle_mask;
219c92b28eSAndrew Jones 
secondary_cinit(struct secondary_data * data)229c92b28eSAndrew Jones secondary_func_t secondary_cinit(struct secondary_data *data)
239c92b28eSAndrew Jones {
249c92b28eSAndrew Jones 	struct thread_info *info;
259c92b28eSAndrew Jones 
260c39d9eaSAndrew Jones 	__mmu_enable(data->satp);
279c92b28eSAndrew Jones 	thread_info_init();
2894ca1aafSAndrew Jones 	local_hart_init();
299c92b28eSAndrew Jones 	info = current_thread_info();
309c92b28eSAndrew Jones 	set_cpu_online(info->cpu, true);
319c92b28eSAndrew Jones 	smp_send_event();
329c92b28eSAndrew Jones 
339c92b28eSAndrew Jones 	return data->func;
349c92b28eSAndrew Jones }
359c92b28eSAndrew Jones 
__smp_boot_secondary(int cpu,secondary_func_t func)369c92b28eSAndrew Jones static void __smp_boot_secondary(int cpu, secondary_func_t func)
379c92b28eSAndrew Jones {
38*50fab1a5SAndrew Jones 	void *sp_mem = __va(cpus[cpu].sp);
39*50fab1a5SAndrew Jones 	struct secondary_data *data;
409c92b28eSAndrew Jones 	struct sbiret ret;
419c92b28eSAndrew Jones 
42*50fab1a5SAndrew Jones 	if (!sp_mem) {
43*50fab1a5SAndrew Jones 		phys_addr_t sp_phys;
449c92b28eSAndrew Jones 
45*50fab1a5SAndrew Jones 		sp_mem = alloc_pages(1) + SZ_8K - 16;
46*50fab1a5SAndrew Jones 		sp_phys = virt_to_phys(sp_mem);
47*50fab1a5SAndrew Jones 		cpus[cpu].sp = __pa(sp_phys);
4892f91f64SAndrew Jones 
49*50fab1a5SAndrew Jones 		assert(__va(cpus[cpu].sp) == sp_mem);
50*50fab1a5SAndrew Jones 	}
51*50fab1a5SAndrew Jones 
52*50fab1a5SAndrew Jones 	sp_mem -= sizeof(struct secondary_data);
53*50fab1a5SAndrew Jones 	data = (struct secondary_data *)sp_mem;
54*50fab1a5SAndrew Jones 	data->satp = csr_read(CSR_SATP);
55*50fab1a5SAndrew Jones 	data->stvec = csr_read(CSR_STVEC);
56*50fab1a5SAndrew Jones 	data->func = func;
57*50fab1a5SAndrew Jones 
58*50fab1a5SAndrew Jones 	ret = sbi_hart_start(cpus[cpu].hartid, __pa(secondary_entry), cpus[cpu].sp);
599c92b28eSAndrew Jones 	assert(ret.error == SBI_SUCCESS);
609c92b28eSAndrew Jones }
619c92b28eSAndrew Jones 
smp_boot_secondary(int cpu,void (* func)(void))629c92b28eSAndrew Jones void smp_boot_secondary(int cpu, void (*func)(void))
639c92b28eSAndrew Jones {
64*50fab1a5SAndrew Jones 	struct sbiret ret;
659c92b28eSAndrew Jones 
66*50fab1a5SAndrew Jones 	do {
67*50fab1a5SAndrew Jones 		ret = sbi_hart_get_status(cpus[cpu].hartid);
68*50fab1a5SAndrew Jones 		assert(!ret.error);
69*50fab1a5SAndrew Jones 	} while (ret.value == SBI_EXT_HSM_STOP_PENDING);
70*50fab1a5SAndrew Jones 
71*50fab1a5SAndrew Jones 	assert_msg(ret.value == SBI_EXT_HSM_STOPPED, "CPU%d is not stopped", cpu);
729c92b28eSAndrew Jones 	__smp_boot_secondary(cpu, func);
739c92b28eSAndrew Jones 
749c92b28eSAndrew Jones 	while (!cpu_online(cpu))
759c92b28eSAndrew Jones 		smp_wait_for_event();
769c92b28eSAndrew Jones }
779c92b28eSAndrew Jones 
smp_boot_secondary_nofail(int cpu,void (* func)(void))789c92b28eSAndrew Jones void smp_boot_secondary_nofail(int cpu, void (*func)(void))
799c92b28eSAndrew Jones {
80*50fab1a5SAndrew Jones 	struct sbiret ret;
819c92b28eSAndrew Jones 
82*50fab1a5SAndrew Jones 	do {
83*50fab1a5SAndrew Jones 		ret = sbi_hart_get_status(cpus[cpu].hartid);
84*50fab1a5SAndrew Jones 		assert(!ret.error);
85*50fab1a5SAndrew Jones 	} while (ret.value == SBI_EXT_HSM_STOP_PENDING);
86*50fab1a5SAndrew Jones 
87*50fab1a5SAndrew Jones 	if (ret.value == SBI_EXT_HSM_STOPPED)
889c92b28eSAndrew Jones 		__smp_boot_secondary(cpu, func);
89*50fab1a5SAndrew Jones 	else
90*50fab1a5SAndrew Jones 		assert_msg(ret.value == SBI_EXT_HSM_START_PENDING || ret.value == SBI_EXT_HSM_STARTED,
91*50fab1a5SAndrew Jones 			   "CPU%d is in an unexpected state %ld", cpu, ret.value);
929c92b28eSAndrew Jones 
939c92b28eSAndrew Jones 	while (!cpu_online(cpu))
949c92b28eSAndrew Jones 		smp_wait_for_event();
959c92b28eSAndrew Jones }
96