rtems_bsd_mutex: SMP support via ISR locks

1 files changed, 25 insertions, 20 deletions

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);
 }