From 0fd8287b2be109cd8671a65cac955f943ab60d32 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 26 Jun 2018 07:15:28 +0200
Subject: riscv: Add _CPU_Get_current_per_CPU_control()

Update #3433.
---
 bsps/riscv/riscv/start/start.S                       |  6 +++++-
 cpukit/score/cpu/riscv/include/rtems/asm.h           |  8 ++++++++
 cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h | 15 +++++++++++++++
 3 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/bsps/riscv/riscv/start/start.S b/bsps/riscv/riscv/start/start.S
index 390459a1fb..d5c6be9c3a 100644
--- a/bsps/riscv/riscv/start/start.S
+++ b/bsps/riscv/riscv/start/start.S
@@ -28,7 +28,7 @@
  */
 
 #include <rtems/asm.h>
-#include <rtems/score/cpu.h>
+#include <rtems/score/percpu.h>
 #include <rtems/score/riscv-utility.h>
 #include <bsp/linker-symbols.h>
 #include <bspopts.h>
@@ -55,6 +55,10 @@ SYM(_start):
 
 #ifdef RTEMS_SMP
 	csrr	s0, mhartid
+	la	t0, _Per_CPU_Information
+	slli	t1, s0, PER_CPU_CONTROL_SIZE_LOG2
+	add	t0, t0, t1
+	csrw	mscratch, t0
 	bnez	s0, .Lwait_for_go
 #endif
 
diff --git a/cpukit/score/cpu/riscv/include/rtems/asm.h b/cpukit/score/cpu/riscv/include/rtems/asm.h
index 34b6474515..55fcb801c3 100644
--- a/cpukit/score/cpu/riscv/include/rtems/asm.h
+++ b/cpukit/score/cpu/riscv/include/rtems/asm.h
@@ -117,4 +117,12 @@
 #define EXTERN(sym)    .extern SYM (sym)
 #define TYPE_FUNC(sym) .type SYM (sym), %function
 
+.macro GET_SELF_CPU_CONTROL REG
+#ifdef RTEMS_SMP
+	csrr	\REG, mscratch
+#else
+	la	\REG, _Per_CPU_Information
+#endif
+.endm
+
 #endif
diff --git a/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h b/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
index 2a63e03513..1370e656dd 100644
--- a/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
+++ b/cpukit/score/cpu/riscv/include/rtems/score/cpuimpl.h
@@ -52,6 +52,21 @@
 extern "C" {
 #endif
 
+#ifdef RTEMS_SMP
+
+static inline struct Per_CPU_Control *_RISCV_Get_current_per_CPU_control( void )
+{
+  struct Per_CPU_Control *cpu_self;
+
+  __asm__ volatile ( "csrr %0, mscratch" : "=r" ( cpu_self ) );
+
+  return cpu_self;
+}
+
+#define _CPU_Get_current_per_CPU_control() _RISCV_Get_current_per_CPU_control()
+
+#endif /* RTEMS_SMP */
+
 #ifdef __cplusplus
 }
 #endif
-- 
cgit v1.2.3