1 /* SPDX-License-Identifier: GPL-2.0-only */
2
3 #include <arch/barrier.h>
4 #include <arch/encoding.h>
5 #include <arch/smp/smp.h>
6 #include <arch/smp/atomic.h>
7 #include <console/console.h>
8 #include <mcall.h>
9
smp_pause(int working_hartid)10 void smp_pause(int working_hartid)
11 {
12 #define SYNCA (OTHER_HLS(working_hartid)->entry.sync_a)
13 #define SYNCB (OTHER_HLS(working_hartid)->entry.sync_b)
14
15 int hartid = read_csr(mhartid);
16
17 if (hartid != working_hartid) {
18 /* waiting for work hart */
19 do {
20 barrier();
21 } while (atomic_read(&SYNCA) != 0x01234567);
22
23 clear_csr(mstatus, MSTATUS_MIE);
24 write_csr(mie, MIP_MSIP);
25
26 /* count how many cores enter the halt */
27 atomic_add(&SYNCB, 1);
28
29 do {
30 barrier();
31 __asm__ volatile ("wfi");
32 } while ((read_csr(mip) & MIP_MSIP) == 0);
33 set_msip(hartid, 0);
34 HLS()->entry.fn(HLS()->entry.arg);
35 } else {
36 /* Initialize the counter and
37 * mark the work hart into smp_pause */
38 atomic_set(&SYNCB, 0);
39 atomic_set(&SYNCA, 0x01234567);
40
41 /* waiting for other Hart to enter the halt */
42 do {
43 barrier();
44 } while (atomic_read(&SYNCB) + 1 < CONFIG_MAX_CPUS);
45
46 /* initialize for the next call */
47 atomic_set(&SYNCA, 0);
48 atomic_set(&SYNCB, 0);
49 }
50 #undef SYNCA
51 #undef SYNCB
52 }
53
smp_resume(void (* fn)(void *),void * arg)54 void smp_resume(void (*fn)(void *), void *arg)
55 {
56 int hartid = read_csr(mhartid);
57
58 if (fn == NULL)
59 die("must pass a non-null function pointer\n");
60
61 for (int i = 0; i < CONFIG_MAX_CPUS; i++) {
62 OTHER_HLS(i)->entry.fn = fn;
63 OTHER_HLS(i)->entry.arg = arg;
64 }
65
66 for (int i = 0; i < CONFIG_MAX_CPUS; i++)
67 if (i != hartid)
68 set_msip(i, 1);
69
70 if (HLS()->entry.fn == NULL)
71 die("entry fn not set\n");
72
73 HLS()->entry.fn(HLS()->entry.arg);
74 }
75