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 #include -#include -#include -#include -#include +#include #include #include @@ -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) { + if (__predict_true(nest_level == 0)) { + RBTree_Node *first = _RBTree_First(&m->rivals, RBT_LEFT); + Thread_Control *owner = m->owner; + int keep_priority; + + --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); + } else { + rtems_bsd_mutex_unlock_more(m, owner, keep_priority, + first, level); + } + + } else { m->nest_level = nest_level - 1; _ISR_Enable(level); - } else { - RBTree_Node *first; - Thread_Control *owner = m->owner; - - BSD_ASSERT(owner == _Thread_Executing); - --owner->resource_count; - - first = _RBTree_Get(&m->rivals, RBT_LEFT); - - if (first == NULL) { - m->owner = NULL; - - _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(); - } - - 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(); - } } } 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 + * + * + * 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 +#include + +#include +#include + +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(); + } +}