demo/smp.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

#include "demo.h"

#define SMP_CTRL_BASE 0x30140000
#define SMP_CTRL      (*(volatile unsigned *)SMP_CTRL_BASE)

int cpus_ready(void)
{
	return SMP_CTRL == 0;
}

void run_cpu(unsigned num)
{
	run_cpus(1 << num);
}

void run_cpus(unsigned mask)
{
	unsigned ctrl_word = 0;
	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
		if (mask & (1 << cpu)) {
			print("run cpu%u", cpu);
			ctrl_word |= 0b001 << (cpu * 8);
		}

	SMP_CTRL = ctrl_word;
}

void halt_cpu(unsigned num)
{
	halt_cpu(1 << num);
}

void halt_cpus(unsigned mask)
{
	unsigned ctrl_word = 0;
	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
		if (mask & (1 << cpu)) {
			print("halt cpu%u%s", cpu, cpu == this_cpu->num ? " (myself)" : "");
			ctrl_word |= 0b010 << (cpu * 8);
		}

	SMP_CTRL = ctrl_word;
}