From b22853b2f4bdd1ed31d601ebd331083d67d40f11 Mon Sep 17 00:00:00 2001
From: Palmer Dabbelt <palmer@dabbelt.com>
Date: Tue, 13 Jun 2017 15:14:38 -0700
Subject: Add a SMP example

This just prints "hello world" on two cores.  It contains an example of
how to initialize a multi-core system using IPIs, and a simple spin
lock.
---
 .gitignore            |  1 +
 bsp/env/start.S       | 51 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 software/smp/Makefile |  6 ++++++
 software/smp/atomic.h | 29 +++++++++++++++++++++++++++++
 software/smp/smp.c    | 40 ++++++++++++++++++++++++++++++++++++++++
 5 files changed, 126 insertions(+), 1 deletion(-)
 create mode 100644 software/smp/Makefile
 create mode 100644 software/smp/atomic.h
 create mode 100644 software/smp/smp.c

diff --git a/.gitignore b/.gitignore
index 29d3e10..534c2dc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -13,3 +13,4 @@ software/hello/hello
 software/led_fade/led_fade
 software/local_interrupts/local_interrupts
 software/performance_counters/performance_counters
+software/smp/smp
diff --git a/bsp/env/start.S b/bsp/env/start.S
index e86105b..4e9f665 100644
--- a/bsp/env/start.S
+++ b/bsp/env/start.S
@@ -1,18 +1,27 @@
 // See LICENSE for license details.
+#include <sifive/smp.h>
 
-// See LICENSE for license details.
+/* This is defined in sifive/platform.h, but that can't be included from
+ * assembly. */
+#define CLINT_CTRL_ADDR 0x02000000
 
 	.section .init
 	.globl _start
 	.type _start,@function
 
 _start:
+	.cfi_startproc
+	.cfi_undefined ra
 .option push
 .option norelax
 	la gp, __global_pointer$
 .option pop
 	la sp, _sp
 
+#if defined(ENABLE_SMP)
+	smp_pause(t0, t1)
+#endif
+
 	/* Load data section */
 	la a0, _data_lma
 	la a1, _data
@@ -52,11 +61,51 @@ _start:
 1:
 #endif
 
+#if defined(ENABLE_SMP)
+	smp_resume(t0, t1)
+
+	csrr a0, mhartid
+	bnez a0, 2f
+#endif
+
+	auipc ra, 0
+	addi sp, sp, -16
+#if __riscv_xlen == 32
+	sw ra, 8(sp)
+#else
+	sd ra, 8(sp)
+#endif
+
 	/* argc = argv = 0 */
 	li a0, 0
 	li a1, 0
 	call main
 	tail exit
+1:
+	j 1b
+
+#if defined(ENABLE_SMP)
+2:
+	la t0, trap_entry
+	csrw mtvec, t0
+
+	csrr a0, mhartid
+	la t1, _sp
+	slli t0, a0, 10
+	sub sp, t1, t0
+
+	auipc ra, 0
+	addi sp, sp, -16
+#if __riscv_xlen == 32
+	sw ra, 8(sp)
+#else
+	sd ra, 8(sp)
+#endif
+
+	call secondary_main
+	tail exit
 
 1:
 	j 1b
+#endif
+	.cfi_endproc
diff --git a/software/smp/Makefile b/software/smp/Makefile
new file mode 100644
index 0000000..4131ffc
--- /dev/null
+++ b/software/smp/Makefile
@@ -0,0 +1,6 @@
+TARGET = smp
+C_SRCS += smp.c
+CFLAGS += -O2 -fno-builtin-printf -DENABLE_SMP
+
+BSP_BASE = ../../bsp
+include $(BSP_BASE)/env/common.mk
diff --git a/software/smp/atomic.h b/software/smp/atomic.h
new file mode 100644
index 0000000..074c6a2
--- /dev/null
+++ b/software/smp/atomic.h
@@ -0,0 +1,29 @@
+#ifndef SIFIVE_ATOMIC_H
+#define SIFIVE_ATOMIC_H
+
+#define ATOMIC_INIT(x)		\
+	{			\
+		.counter = (x),	\
+	}
+
+typedef struct {
+  int counter;
+} atomic_t;
+
+static inline int atomic_xchg(atomic_t *v, int n)
+{
+  register int c;
+
+  __asm__ __volatile__ (
+    "amoswap.w.aqrl %0, %2, %1"
+    : "=r" (c), "+A" (v->counter)
+    : "r" (n));
+  return c;
+}
+
+static inline void mb(void)
+{
+  __asm__ __volatile__ ("fence");
+}
+
+#endif
diff --git a/software/smp/smp.c b/software/smp/smp.c
new file mode 100644
index 0000000..36c9db9
--- /dev/null
+++ b/software/smp/smp.c
@@ -0,0 +1,40 @@
+#include <stdio.h>
+#include <unistd.h>
+#include "atomic.h"
+void write_hex(int, unsigned long);
+
+atomic_t tty_lock = ATOMIC_INIT(0);
+
+void get_lock(atomic_t *lock)
+{
+  while (atomic_xchg(lock, 1) == 1);
+  mb();
+}
+
+void put_lock(atomic_t *lock)
+{
+  mb();
+  atomic_xchg(lock, 0);
+}
+
+int secondary_main(int hartid)
+{
+  volatile int counter;
+
+  while (1) {
+    get_lock(&tty_lock);
+    write(1, "hello world from hart ", 22);
+    char s[] = {'0', '\n', '\0'};
+    s[0] += hartid;
+    write(1, s, 2);
+    put_lock(&tty_lock);
+
+    for (counter = 0; counter < 10000 + 100*hartid; ++counter)
+        mb();
+  }
+}
+
+int main()
+{
+  return secondary_main(0);
+}
-- 
cgit v1.2.1-18-gbd029