rtems_bsd_mutex: Optimize

4 files changed, 123 insertions, 50 deletions

diff --git a/Makefile b/Makefile
index d3c663c5..aa2abfdf 100644
--- a/Makefile
+++ b/Makefile
@@ -77,6 +77,7 @@ LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-kern_synch.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-log.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-malloc.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-mutex.c
+LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-muteximpl.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-newproc.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-nexus.c
 LIB_C_FILES += rtemsbsd/rtems/rtems-bsd-panic.c
diff --git a/freebsd-to-rtems.py b/freebsd-to-rtems.py
index 947bebac..9e61523c 100755
--- a/freebsd-to-rtems.py
+++ b/freebsd-to-rtems.py
@@ -675,6 +675,7 @@ rtems.addRTEMSSourceFiles(
 		'rtems/rtems-bsd-log.c',
 		'rtems/rtems-bsd-malloc.c',
 		'rtems/rtems-bsd-mutex.c',
+		'rtems/rtems-bsd-muteximpl.c',
 		'rtems/rtems-bsd-newproc.c',
 		'rtems/rtems-bsd-nexus.c',
 		'rtems/rtems-bsd-panic.c',
diff --git a/rtemsbsd/include/machine/rtems-bsd-muteximpl.h b/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
index 69b42eb6..76f6f6dc 100644
--- a/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
+++ b/rtemsbsd/include/machine/rtems-bsd-muteximpl.h
@@ -47,10 +47,7 @@
 #include <rtems/bsd/sys/types.h>
 #include <rtems/bsd/sys/lock.h>
 
-#include <rtems/score/isr.h>
-#include <rtems/score/rbtreeimpl.h>
-#include <rtems/score/schedulerimpl.h>
-#include <rtems/score/threaddispatch.h>
+#include <rtems/score/isrlevel.h>
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/threadqimpl.h>
 
@@ -69,6 +66,9 @@ rtems_bsd_mutex_init(struct lock_object *lock, rtems_bsd_mutex *m,
 	lock_init(lock, class, name, type, flags);
 }
 
+void rtems_bsd_mutex_lock_more(struct lock_object *lock, rtems_bsd_mutex *m,
+    Thread_Control *owner, Thread_Control *executing, ISR_Level level);
+
 static inline void
 rtems_bsd_mutex_lock(struct lock_object *lock, rtems_bsd_mutex *m)
 {
@@ -81,29 +81,13 @@ rtems_bsd_mutex_lock(struct lock_object *lock, rtems_bsd_mutex *m)
 	owner = m->owner;
 	executing = _Thread_Executing;
 
-	if (owner == NULL) {
+	if (__predict_true(owner == NULL)) {
 		m->owner = executing;
 		++executing->resource_count;
 
 		_ISR_Enable(level);
-	} else if (owner == executing) {
-		BSD_ASSERT(lock->lo_flags & LO_RECURSABLE);
-		++m->nest_level;
-
-		_ISR_Enable(level);
 	} else {
-		_RBTree_Insert(&m->rivals, &executing->RBNode,
-		    _Thread_queue_Compare_priority, false);
-		++executing->resource_count;
-
-		_Thread_Disable_dispatch();
-		_ISR_Enable(level);
-
-		_Scheduler_Change_priority_if_higher(_Scheduler_Get(owner),
-		    owner, executing->current_priority, false);
-		_Thread_Set_state(executing, STATES_WAITING_FOR_MUTEX);
-
-		_Thread_Enable_dispatch();
+		rtems_bsd_mutex_lock_more(lock, m, owner, executing, level);
 	}
 }
 
@@ -137,6 +121,9 @@ rtems_bsd_mutex_trylock(struct lock_object *lock, rtems_bsd_mutex *m)
 	return (success);
 }
 
+void rtems_bsd_mutex_unlock_more(rtems_bsd_mutex *m, Thread_Control *owner,
+    int keep_priority, RBTree_Node *first, ISR_Level level);
+
 static inline void
 rtems_bsd_mutex_unlock(rtems_bsd_mutex *m)
 {
@@ -146,43 +133,29 @@ rtems_bsd_mutex_unlock(rtems_bsd_mutex *m)
 	_ISR_Disable(level);
 
 	nest_level = m->nest_level;
-	if (nest_level != 0) {
-		m->nest_level = nest_level - 1;
-
-		_ISR_Enable(level);
-	} else {
-		RBTree_Node *first;
+	if (__predict_true(nest_level == 0)) {
+		RBTree_Node *first = _RBTree_First(&m->rivals, RBT_LEFT);
 		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;
 
-		first = _RBTree_Get(&m->rivals, RBT_LEFT);
-
-		if (first == NULL) {
-			m->owner = NULL;
+		m->owner = NULL;
 
+		if (__predict_true(first == NULL && keep_priority
+		    && owner == _Thread_Executing)) {
 			_ISR_Enable(level);
 		} else {
-			Thread_Control *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();
+			rtems_bsd_mutex_unlock_more(m, owner, keep_priority,
+			    first, level);
 		}
 
-		if (!_Thread_Owns_resources(owner)
-		    && owner->real_priority != owner->current_priority) {
-			_Thread_Disable_dispatch();
-			_Thread_Change_priority(owner, owner->real_priority, true);
-			_Thread_Enable_dispatch();
-		}
+	} else {
+		m->nest_level = nest_level - 1;
+
+		_ISR_Enable(level);
 	}
 }
 
diff --git a/rtemsbsd/rtems/rtems-bsd-muteximpl.c b/rtemsbsd/rtems/rtems-bsd-muteximpl.c
new file mode 100644
index 00000000..5dc3346f
--- /dev/null
+++ b/rtemsbsd/rtems/rtems-bsd-muteximpl.c
@@ -0,0 +1,98 @@
+/**
+ * @file
+ *
+ * @ingroup rtems_bsd_rtems
+ *
+ * @brief TODO.
+ */
+
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <machine/rtems-bsd-kernel-space.h>
+#include <machine/rtems-bsd-muteximpl.h>
+
+#include <rtems/score/schedulerimpl.h>
+#include <rtems/score/threaddispatch.h>
+
+void
+rtems_bsd_mutex_lock_more(struct lock_object *lock, rtems_bsd_mutex *m,
+    Thread_Control *owner, Thread_Control *executing, ISR_Level level)
+{
+	if (owner == executing) {
+		BSD_ASSERT(lock->lo_flags & LO_RECURSABLE);
+		++m->nest_level;
+
+		_ISR_Enable(level);
+	} else {
+		_RBTree_Insert(&m->rivals, &executing->RBNode,
+		    _Thread_queue_Compare_priority, false);
+		++executing->resource_count;
+
+		_Thread_Disable_dispatch();
+		_ISR_Enable(level);
+
+		_Scheduler_Change_priority_if_higher(_Scheduler_Get(owner),
+		    owner, executing->current_priority, false);
+		_Thread_Set_state(executing, STATES_WAITING_FOR_MUTEX);
+
+		_Thread_Enable_dispatch();
+	}
+}
+
+void
+rtems_bsd_mutex_unlock_more(rtems_bsd_mutex *m, Thread_Control *owner,
+    int keep_priority, RBTree_Node *first, ISR_Level level)
+{
+	BSD_ASSERT(owner == _Thread_Executing);
+
+	if (first != NULL) {
+		Thread_Control *new_owner;
+
+		_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();
+	}
+
+	if (!keep_priority) {
+		_Thread_Disable_dispatch();
+		_Thread_Change_priority(owner, owner->real_priority, true);
+		_Thread_Enable_dispatch();
+	}
+}