1 files changed, 48 insertions, 0 deletions
diff --git a/demo/smp.c b/demo/smp.c
new file mode 100644
index 0000000..4357d35
--- /dev/null
+++ b/demo/smp.c
@@ -0,0 +1,48 @@
+#include "demo.h"
+
+#define SMP_CTRL_BASE 0x30140000
+#define SMP_CTRL      (*(volatile unsigned *)SMP_CTRL_BASE)
+
+void run_cpu(unsigned num)
+{
+	run_cpus(1 << num);
+}
+
+void run_cpus(unsigned mask)
+{
+	unsigned ctrl_word = 0, ctrl_read = SMP_CTRL;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu) {
+		if (!(mask & (1 << cpu)))
+			continue;
+
+		if (ctrl_read & (0b001 << (cpu * 8))) {
+			print("run cpu%u", cpu);
+			ctrl_word |= 0b001 << (cpu * 8);
+		} else
+			print("cpu%u already running", cpu);
+	}
+
+	SMP_CTRL = ctrl_word;
+}
+
+void halt_cpu(unsigned num)
+{
+	halt_cpus(1 << num);
+}
+
+void halt_cpus(unsigned mask)
+{
+	unsigned ctrl_word = 0, ctrl_read = SMP_CTRL;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu) {
+		if (!(mask & (1 << cpu)))
+			continue;
+
+		if (!(ctrl_read & (0b001 << (cpu * 8)))) {
+			print("halt cpu%u%s", cpu, cpu == this_cpu->num ? " (myself)" : "");
+			ctrl_word |= 0b010 << (cpu * 8);
+		} else
+			print("cpu%u already halted", cpu);
+	}
+
+	SMP_CTRL = ctrl_word;
+}