diff --git a/include/nuttx/semaphore.h b/include/nuttx/semaphore.h index a0c0386024..d11e869638 100644 --- a/include/nuttx/semaphore.h +++ b/include/nuttx/semaphore.h @@ -680,6 +680,47 @@ int nxsem_clockwait_uninterruptible(FAR sem_t *sem, clockid_t clockid, int nxsem_tickwait_uninterruptible(FAR sem_t *sem, uint32_t delay); +/**************************************************************************** + * Name: nxsem_getprioceiling + * + * Description: + * This function attempts to get the priority ceiling of a semaphore. + * + * Input Parameters: + * sem - A pointer to the semaphore whose attributes are to be + * modified + * prioceiling - Location to return the semaphore's priority ceiling + * + * Return Value: + * This is an internal OS interface and should not be used by applications. + * It follows the NuttX internal error return policy: Zero (OK) is + * returned on success. A negated errno value is returned on failure + * + ****************************************************************************/ + +int nxsem_getprioceiling(FAR const sem_t *sem, FAR int *prioceiling); + +/**************************************************************************** + * Name: nxsem_setprioceiling + * + * Description: + * Set the priority ceiling of a semaphore. + * + * Input Parameters: + * mutex - The mutex in which to set the mutex priority ceiling. + * prioceiling - The mutex priority ceiling value to set. + * old_ceiling - Location to return the mutex ceiling priority set before. + * + * Return Value: + * This is an internal OS interface and should not be used by applications. + * It follows the NuttX internal error return policy: Zero (OK) is + * returned on success. A negated errno value is returned on failure + * + ****************************************************************************/ + +int nxsem_setprioceiling(FAR sem_t *sem, int prioceiling, + FAR int *old_ceiling); + #undef EXTERN #ifdef __cplusplus } diff --git a/include/semaphore.h b/include/semaphore.h index eb4b1fcc37..a1a1db772c 100644 --- a/include/semaphore.h +++ b/include/semaphore.h @@ -116,6 +116,10 @@ struct sem_s struct semholder_s holder; /* Slot for old and new holder */ # endif #endif +#ifdef CONFIG_PRIORITY_PROTECT + uint8_t ceiling; /* The priority ceiling owned by mutex */ + uint8_t saved; /* The saved priority of thread before boost */ +#endif }; typedef struct sem_s sem_t; diff --git a/libs/libc/semaphore/CMakeLists.txt b/libs/libc/semaphore/CMakeLists.txt index 76220ac926..687c924fb1 100644 --- a/libs/libc/semaphore/CMakeLists.txt +++ b/libs/libc/semaphore/CMakeLists.txt @@ -34,4 +34,8 @@ if(CONFIG_FS_NAMED_SEMAPHORES) list(APPEND SRCS sem_open.c sem_close.c sem_unlink.c) endif() +if(CONFIG_PRIORITY_PROTECT) + list(APPEND CSRCS sem_getprioceiling.c sem_setprioceiling.c) +endif() + target_sources(c PRIVATE ${SRCS}) diff --git a/libs/libc/semaphore/Make.defs b/libs/libc/semaphore/Make.defs index b95d9f3c29..afb7492b67 100644 --- a/libs/libc/semaphore/Make.defs +++ b/libs/libc/semaphore/Make.defs @@ -28,6 +28,10 @@ ifeq ($(CONFIG_FS_NAMED_SEMAPHORES),y) CSRCS += sem_open.c sem_close.c sem_unlink.c endif +ifeq ($(CONFIG_PRIORITY_PROTECT),y) +CSRCS += sem_getprioceiling.c sem_setprioceiling.c +endif + # Add the semaphore directory to the build DEPPATH += --dep-path semaphore diff --git a/libs/libc/semaphore/sem_getprioceiling.c b/libs/libc/semaphore/sem_getprioceiling.c new file mode 100644 index 0000000000..6480061615 --- /dev/null +++ b/libs/libc/semaphore/sem_getprioceiling.c @@ -0,0 +1,65 @@ +/**************************************************************************** + * libs/libc/semaphore/sem_getprioceiling.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxsem_getprioceiling + * + * Description: + * This function attempts to get the priority ceiling of a semaphore. + * + * Input Parameters: + * sem - A pointer to the semaphore whose attributes are to be + * modified + * prioceiling - Location to return the semaphore's priority ceiling + * + * Return Value: + * This is an internal OS interface and should not be used by applications. + * It follows the NuttX internal error return policy: Zero (OK) is + * returned on success. A negated errno value is returned on failure + * + ****************************************************************************/ + +int nxsem_getprioceiling(FAR const sem_t *sem, FAR int *prioceiling) +{ + DEBUGASSERT(sem != NULL); + + if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT) + { + *prioceiling = sem->ceiling; + return OK; + } + + return -EINVAL; +} diff --git a/libs/libc/semaphore/sem_setprioceiling.c b/libs/libc/semaphore/sem_setprioceiling.c new file mode 100644 index 0000000000..ed3ebdcde5 --- /dev/null +++ b/libs/libc/semaphore/sem_setprioceiling.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * libs/libc/semaphore/sem_setprioceiling.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxsem_setprioceiling + * + * Description: + * Set the priority ceiling of a semaphore. + * + * Input Parameters: + * mutex - The mutex in which to set the mutex priority ceiling. + * prioceiling - The mutex priority ceiling value to set. + * old_ceiling - Location to return the mutex ceiling priority set before. + * + * Return Value: + * This is an internal OS interface and should not be used by applications. + * It follows the NuttX internal error return policy: Zero (OK) is + * returned on success. A negated errno value is returned on failure + * + ****************************************************************************/ + +int nxsem_setprioceiling(FAR sem_t *sem, int prioceiling, + FAR int *old_ceiling) +{ + DEBUGASSERT(sem != NULL); + + if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT && + prioceiling >= sched_get_priority_min(SCHED_FIFO) && + prioceiling <= sched_get_priority_max(SCHED_FIFO)) + { + if (old_ceiling != NULL) + { + *old_ceiling = sem->ceiling; + } + + sem->ceiling = prioceiling; + return OK; + } + + return -EINVAL; +} diff --git a/libs/libc/semaphore/sem_setprotocol.c b/libs/libc/semaphore/sem_setprotocol.c index 7070dd4103..1b48d69623 100644 --- a/libs/libc/semaphore/sem_setprotocol.c +++ b/libs/libc/semaphore/sem_setprotocol.c @@ -83,8 +83,15 @@ int nxsem_set_protocol(FAR sem_t *sem, int protocol) break; case SEM_PRIO_INHERIT: - case SEM_PRIO_PROTECT: return -ENOTSUP; + case SEM_PRIO_PROTECT: +#ifdef CONFIG_PRIORITY_PROTECT + break; +#else + /* Not yet supported */ + + return -ENOTSUP; +#endif default: return -EINVAL; diff --git a/sched/Kconfig b/sched/Kconfig index 43fa0e0587..63f29596df 100644 --- a/sched/Kconfig +++ b/sched/Kconfig @@ -1436,6 +1436,13 @@ config SEM_PREALLOCHOLDERS endif # PRIORITY_INHERITANCE +config PRIORITY_PROTECT + bool "Enable priority protect" + default n + ---help--- + When a thread locks a mutex it inherits the priority ceiling of the + mutex, which is defined by the application as a mutex attribute. + menu "RTOS hooks" config BOARD_EARLY_INITIALIZE diff --git a/sched/semaphore/sem_post.c b/sched/semaphore/sem_post.c index 8539d5f8ba..94c09ef59a 100644 --- a/sched/semaphore/sem_post.c +++ b/sched/semaphore/sem_post.c @@ -72,8 +72,8 @@ int nxsem_post(FAR sem_t *sem) FAR struct tcb_s *stcb = NULL; irqstate_t flags; int16_t sem_count; -#ifdef CONFIG_PRIORITY_INHERITANCE - uint8_t prioinherit; +#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT) + uint8_t proto; #endif DEBUGASSERT(sem != NULL); @@ -116,7 +116,7 @@ int nxsem_post(FAR sem_t *sem) sem_count++; sem->semcount = sem_count; -#ifdef CONFIG_PRIORITY_INHERITANCE +#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT) /* Don't let any unblocked tasks run until we complete any priority * restoration steps. Interrupts are disabled, but we do not want * the head of the ready-to-run list to be modified yet. @@ -125,8 +125,8 @@ int nxsem_post(FAR sem_t *sem) * will do nothing. */ - prioinherit = sem->flags & SEM_PRIO_MASK; - if (prioinherit == SEM_PRIO_INHERIT) + proto = sem->flags & SEM_PRIO_MASK; + if (proto != SEM_PRIO_NONE) { sched_lock(); } @@ -191,10 +191,27 @@ int nxsem_post(FAR sem_t *sem) * held the semaphore. */ -#ifdef CONFIG_PRIORITY_INHERITANCE - if (prioinherit == SEM_PRIO_INHERIT) +#if defined(CONFIG_PRIORITY_INHERITANCE) || defined(CONFIG_PRIORITY_PROTECT) + if (proto != SEM_PRIO_NONE) { - nxsem_restore_baseprio(stcb, sem); + if (proto == SEM_PRIO_INHERIT) + { +#ifdef CONFIG_PRIORITY_INHERITANCE + nxsem_restore_baseprio(stcb, sem); +#endif + } + + if (proto == SEM_PRIO_PROTECT) + { +#ifdef CONFIG_PRIORITY_PROTECT + if (sem->saved > 0) + { + nxsched_set_priority(this_task(), sem->saved); + sem->saved = 0; + } +#endif + } + sched_unlock(); } #endif diff --git a/sched/semaphore/sem_setprotocol.c b/sched/semaphore/sem_setprotocol.c index 6aab5b7d8c..f07f30a362 100644 --- a/sched/semaphore/sem_setprotocol.c +++ b/sched/semaphore/sem_setprotocol.c @@ -90,10 +90,13 @@ int nxsem_set_protocol(FAR sem_t *sem, int protocol) break; case SEM_PRIO_PROTECT: - +#ifdef CONFIG_PRIORITY_PROTECT + break; +#else /* Not yet supported */ return -ENOTSUP; +#endif default: return -EINVAL; diff --git a/sched/semaphore/sem_wait.c b/sched/semaphore/sem_wait.c index f4f06bc837..bec292be5c 100644 --- a/sched/semaphore/sem_wait.c +++ b/sched/semaphore/sem_wait.c @@ -94,6 +94,22 @@ int nxsem_wait(FAR sem_t *sem) { /* It is, let the task take the semaphore. */ +#ifdef CONFIG_PRIORITY_PROTECT + if ((sem->flags & SEM_PRIO_MASK) == SEM_PRIO_PROTECT) + { + if (rtcb->sched_priority <= sem->ceiling) + { + sem->saved = rtcb->sched_priority; + rtcb->sched_priority = sem->ceiling; + } + else + { + leave_critical_section_nonirq(flags); + return -EINVAL; + } + } +#endif + sem->semcount--; nxsem_add_holder(sem); rtcb->waitobj = NULL;