rtems_bsd_mutex: SMP support via ISR locks

3 files changed, 82 insertions, 37 deletions

diff --git a/rtemsbsd/include/machine/rtems-bsd-mutex.h b/rtemsbsd/include/machine/rtems-bsd-mutex.h
index 58b16f2c..d1879ce9 100644
--- a/rtemsbsd/include/machine/rtems-bsd-mutex.h
+++ b/rtemsbsd/include/machine/rtems-bsd-mutex.h
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2015 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -40,6 +40,7 @@
 #ifndef _RTEMS_BSD_MACHINE_RTEMS_BSD_MUTEX_H_
 #define _RTEMS_BSD_MACHINE_RTEMS_BSD_MUTEX_H_
 
+#include <rtems/score/isrlock.h>
 #include <rtems/score/rbtree.h>
 #include <rtems/score/thread.h>
 
@@ -48,6 +49,7 @@ extern "C" {
 #endif /* __cplusplus */
 
 typedef struct {
+	ISR_LOCK_MEMBER(lock)
 	Thread_Control *owner;
 	int nest_level;
 	RBTree_Control rivals;
diff --git a/rtemsbsd/include/machine/rtems-bsd-muteximpl.h b/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
index 76f6f6dc..dfa332b4 100644
--- a/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
+++ b/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
@@ -8,7 +8,7 @@
  */
 
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2015 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -47,9 +47,7 @@
 #include <rtems/bsd/sys/types.h>
 #include <rtems/bsd/sys/lock.h>
 
-#include <rtems/score/isrlevel.h>
 #include <rtems/score/threadimpl.h>
-#include <rtems/score/threadqimpl.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -59,6 +57,7 @@ static inline void
 rtems_bsd_mutex_init(struct lock_object *lock, rtems_bsd_mutex *m,
     struct lock_class *class, const char *name, const char *type, int flags)
 {
+	_ISR_lock_Initialize(&m->lock, name);
 	m->owner = NULL;
 	m->nest_level = 0;
 	_RBTree_Initialize_empty(&m->rivals);
@@ -67,27 +66,31 @@ rtems_bsd_mutex_init(struct lock_object *lock, rtems_bsd_mutex *m,
 }
 
 void rtems_bsd_mutex_lock_more(struct lock_object *lock, rtems_bsd_mutex *m,
-    Thread_Control *owner, Thread_Control *executing, ISR_Level level);
+    Per_CPU_Control *cpu_self, Thread_Control *owner,
+    Thread_Control *executing, ISR_lock_Context *lock_context);
 
 static inline void
 rtems_bsd_mutex_lock(struct lock_object *lock, rtems_bsd_mutex *m)
 {
-	ISR_Level level;
+	ISR_lock_Context lock_context;
+	Per_CPU_Control *cpu_self;
 	Thread_Control *executing;
 	Thread_Control *owner;
 
-	_ISR_Disable(level);
+	_ISR_lock_ISR_disable_and_acquire(&m->lock, &lock_context);
 
 	owner = m->owner;
-	executing = _Thread_Executing;
+	cpu_self = _Per_CPU_Get();
+	executing = cpu_self->executing;
 
 	if (__predict_true(owner == NULL)) {
 		m->owner = executing;
 		++executing->resource_count;
 
-		_ISR_Enable(level);
+		_ISR_lock_Release_and_ISR_enable(&m->lock, &lock_context);
 	} else {
-		rtems_bsd_mutex_lock_more(lock, m, owner, executing, level);
+		rtems_bsd_mutex_lock_more(lock, m, cpu_self, owner, executing,
+		    &lock_context);
 	}
 }
 
@@ -95,11 +98,11 @@ static inline int
 rtems_bsd_mutex_trylock(struct lock_object *lock, rtems_bsd_mutex *m)
 {
 	int success;
-	ISR_Level level;
+	ISR_lock_Context lock_context;
 	Thread_Control *executing;
 	Thread_Control *owner;
 
-	_ISR_Disable(level);
+	_ISR_lock_ISR_disable_and_acquire(&m->lock, &lock_context);
 
 	owner = m->owner;
 	executing = _Thread_Executing;
@@ -116,21 +119,21 @@ rtems_bsd_mutex_trylock(struct lock_object *lock, rtems_bsd_mutex *m)
 		success = 0;
 	}
 
-	_ISR_Enable(level);
+	_ISR_lock_Release_and_ISR_enable(&m->lock, &lock_context);
 
 	return (success);
 }
 
 void rtems_bsd_mutex_unlock_more(rtems_bsd_mutex *m, Thread_Control *owner,
-    int keep_priority, RBTree_Node *first, ISR_Level level);
+    int keep_priority, RBTree_Node *first, ISR_lock_Context *lock_context);
 
 static inline void
 rtems_bsd_mutex_unlock(rtems_bsd_mutex *m)
 {
-	ISR_Level level;
+	ISR_lock_Context lock_context;
 	int nest_level;
 
-	_ISR_Disable(level);
+	_ISR_lock_ISR_disable_and_acquire(&m->lock, &lock_context);
 
 	nest_level = m->nest_level;
 	if (__predict_true(nest_level == 0)) {
@@ -138,24 +141,25 @@ rtems_bsd_mutex_unlock(rtems_bsd_mutex *m)
 		Thread_Control *owner = m->owner;
 		int keep_priority;
 
+		BSD_ASSERT(owner == _Thread_Executing);
+
 		--owner->resource_count;
 		keep_priority = _Thread_Owns_resources(owner)
 		    || owner->real_priority == owner->current_priority;
 
 		m->owner = NULL;
 
-		if (__predict_true(first == NULL && keep_priority
-		    && owner == _Thread_Executing)) {
-			_ISR_Enable(level);
+		if (__predict_true(first == NULL && keep_priority)) {
+			_ISR_lock_Release_and_ISR_enable(&m->lock, &lock_context);
 		} else {
 			rtems_bsd_mutex_unlock_more(m, owner, keep_priority,
-			    first, level);
+			    first, &lock_context);
 		}
 
 	} else {
 		m->nest_level = nest_level - 1;
 
-		_ISR_Enable(level);
+		_ISR_lock_Release_and_ISR_enable(&m->lock, &lock_context);
 	}
 }
 
@@ -183,6 +187,7 @@ rtems_bsd_mutex_destroy(struct lock_object *lock, rtems_bsd_mutex *m)
 		rtems_bsd_mutex_unlock(m);
 	}
 
+	_ISR_lock_Destroy(&m->lock);
 	lock_destroy(lock);
 }
 
diff --git a/rtemsbsd/rtems/rtems-bsd-muteximpl.c b/rtemsbsd/rtems/rtems-bsd-muteximpl.c
index 2873e32c..275a9543 100644
--- a/rtemsbsd/rtems/rtems-bsd-muteximpl.c
+++ b/rtemsbsd/rtems/rtems-bsd-muteximpl.c
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2015 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -42,54 +42,92 @@
 
 #include <rtems/score/schedulerimpl.h>
 #include <rtems/score/threaddispatch.h>
+#include <rtems/score/threadqimpl.h>
+
+#define INTEND_TO_BLOCK \
+    (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_INTEND_TO_BLOCK)
+
+#define BLOCKED \
+    (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_BLOCKED)
+
+#define INTERRUPT_SATISFIED \
+    (THREAD_WAIT_CLASS_OBJECT | THREAD_WAIT_STATE_INTERRUPT_SATISFIED)
 
 void
 rtems_bsd_mutex_lock_more(struct lock_object *lock, rtems_bsd_mutex *m,
-    Thread_Control *owner, Thread_Control *executing, ISR_Level level)
+    Per_CPU_Control *cpu_self, Thread_Control *owner,
+    Thread_Control *executing, ISR_lock_Context *lock_context)
 {
 	if (owner == executing) {
 		BSD_ASSERT(lock->lo_flags & LO_RECURSABLE);
 		++m->nest_level;
 
-		_ISR_Enable(level);
+		_ISR_lock_Release_and_ISR_enable(&m->lock, lock_context);
 	} else {
+		bool success;
+
 		_RBTree_Insert(&m->rivals, &executing->RBNode,
 		    _Thread_queue_Compare_priority, false);
 		++executing->resource_count;
 
-		_Thread_Disable_dispatch();
-		_ISR_Enable(level);
+		_Thread_Dispatch_disable_critical(cpu_self);
+		_Giant_Acquire(cpu_self);
 
+		/* Priority inheritance */
 		_Scheduler_Change_priority_if_higher(_Scheduler_Get(owner),
 		    owner, executing->current_priority, false);
+
+		_Thread_Wait_flags_set(executing, INTEND_TO_BLOCK);
+
+		_ISR_lock_Release_and_ISR_enable(&m->lock, lock_context);
+
 		_Thread_Set_state(executing, STATES_WAITING_FOR_MUTEX);
 
-		_Thread_Enable_dispatch();
+		success = _Thread_Wait_flags_try_change(executing,
+		    INTEND_TO_BLOCK, BLOCKED);
+		if (!success) {
+			_Thread_Unblock(executing);
+		}
+
+		_Giant_Release(cpu_self);
+		_Thread_Dispatch_enable(cpu_self);
 	}
 }
 
 void
 rtems_bsd_mutex_unlock_more(rtems_bsd_mutex *m, Thread_Control *owner,
-    int keep_priority, RBTree_Node *first, ISR_Level level)
+    int keep_priority, RBTree_Node *first, ISR_lock_Context *lock_context)
 {
-	BSD_ASSERT(owner == _Thread_Executing);
-
 	if (first != NULL) {
 		Thread_Control *new_owner;
+		bool success;
 
 		_RBTree_Extract(&m->rivals, first);
 
 		new_owner = THREAD_RBTREE_NODE_TO_THREAD(first);
 		m->owner = new_owner;
 
-		_Thread_Disable_dispatch();
-		_ISR_Enable(level);
-
-		_Thread_Clear_state(new_owner, STATES_WAITING_FOR_MUTEX);
-
-		_Thread_Enable_dispatch();
+		success = _Thread_Wait_flags_try_change_critical(new_owner,
+		    INTEND_TO_BLOCK, INTERRUPT_SATISFIED);
+		if (success) {
+			_ISR_lock_Release_and_ISR_enable(&m->lock,
+			    lock_context);
+		} else {
+			Per_CPU_Control *cpu_self;
+
+			cpu_self = _Per_CPU_Get();
+			_Thread_Dispatch_disable_critical(cpu_self);
+			_ISR_lock_Release_and_ISR_enable(&m->lock,
+			    lock_context);
+			_Giant_Acquire(cpu_self);
+
+			_Thread_Unblock(new_owner);
+
+			_Giant_Release(cpu_self);
+			_Thread_Dispatch_enable(cpu_self);
+		}
 	} else {
-		_ISR_Enable(level);
+		_ISR_lock_Release_and_ISR_enable(&m->lock, lock_context);
 	}
 
 	if (!keep_priority) {