modify nuttx bug of lack of files

Ubiquitous/Nuttx/nuttx/sched/semaphore/Make.defs

@@ -0,0 +1,38 @@
+############################################################################
+# sched/semaphore/Make.defs
+#
+# 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.
+#
+############################################################################
+
+# Add semaphore-related files to the build
+
+CSRCS += sem_destroy.c sem_wait.c sem_trywait.c sem_tickwait.c
+CSRCS += sem_timedwait.c sem_clockwait.c sem_timeout.c sem_post.c
+CSRCS += sem_recover.c sem_reset.c sem_waitirq.c
+
+ifeq ($(CONFIG_PRIORITY_INHERITANCE),y)
+CSRCS += sem_initialize.c sem_holder.c sem_setprotocol.c
+endif
+
+ifeq ($(CONFIG_SPINLOCK),y)
+CSRCS += spinlock.c
+endif
+
+# Include semaphore build support
+
+DEPPATH += --dep-path semaphore
+VPATH += :semaphore

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_clockwait.c

@@ -0,0 +1,276 @@
+/****************************************************************************
+ * sched/semaphore/sem_clockwait.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 <nuttx/config.h>
+
+#include <stdint.h>
+#include <unistd.h>
+#include <time.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <nuttx/wdog.h>
+#include <nuttx/cancelpt.h>
+
+#include "sched/sched.h"
+#include "clock/clock.h"
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_clockwait
+ *
+ * Description:
+ *   This function will lock the semaphore referenced by sem as in the
+ *   sem_wait() function. However, if the semaphore cannot be locked without
+ *   waiting for another process or thread to unlock the semaphore by
+ *   performing a sem_post() function, this wait will be terminated when the
+ *   specified timeout expires.
+ *
+ *   The timeout will expire when the absolute time specified by abstime
+ *   passes, as measured by the clock on which timeouts are based (that is,
+ *   when the value of that clock equals or exceeds abstime), or if the
+ *   absolute time specified by abstime has already been passed at the
+ *   time of the call.
+ *
+ *   This is an internal OS interface.  It is functionally equivalent to
+ *   sem_wait except that:
+ *
+ *   - It is not a cancellation point, and
+ *   - It does not modify the errno value.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   clockid - The timing source to use in the conversion
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned 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.
+ *   That may be one of:
+ *
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   EINTR     A signal interrupted this function.
+ *
+ ****************************************************************************/
+
+int nxsem_clockwait(FAR sem_t *sem, clockid_t clockid,
+                    FAR const struct timespec *abstime)
+{
+  FAR struct tcb_s *rtcb = this_task();
+  sclock_t ticks;
+  int status;
+  int ret = ERROR;
+
+  DEBUGASSERT(up_interrupt_context() == false);
+
+  /* Verify the input parameters and, in case of an error, set
+   * errno appropriately.
+   */
+
+#ifdef CONFIG_DEBUG_FEATURES
+  if (!abstime || !sem)
+    {
+      return -EINVAL;
+    }
+#endif
+
+  /* NOTE: We do not need a critical section here, because
+   * nxsem_wait() and nxsem_timeout() use a critical section
+   * in the functions.
+   */
+
+  /* Try to take the semaphore without waiting. */
+
+  ret = nxsem_trywait(sem);
+  if (ret == OK)
+    {
+      /* We got it! */
+
+      goto out;
+    }
+
+  /* We will have to wait for the semaphore.  Make sure that we were provided
+   * with a valid timeout.
+   */
+
+  if (abstime->tv_nsec < 0 || abstime->tv_nsec >= 1000000000)
+    {
+      ret = -EINVAL;
+      goto out;
+    }
+
+  /* Convert the timespec to clock ticks.  We must have interrupts
+   * disabled here so that this time stays valid until the wait begins.
+   *
+   * clock_abstime2ticks() returns zero on success or a POSITIVE errno
+   * value on failure.
+   */
+
+  status = clock_abstime2ticks(clockid, abstime, &ticks);
+
+  /* If the time has already expired return immediately. */
+
+  if (status == OK && ticks <= 0)
+    {
+      ret = -ETIMEDOUT;
+      goto out;
+    }
+
+  /* Handle any time-related errors */
+
+  if (status != OK)
+    {
+      ret = -status;
+      goto out;
+    }
+
+  /* Start the watchdog */
+
+  wd_start(&rtcb->waitdog, ticks, nxsem_timeout, getpid());
+
+  /* Now perform the blocking wait.  If nxsem_wait() fails, the
+   * negated errno value will be returned below.
+   */
+
+  ret = nxsem_wait(sem);
+
+  /* Stop the watchdog timer */
+
+  wd_cancel(&rtcb->waitdog);
+
+out:
+  return ret;
+}
+
+/****************************************************************************
+ * Name: nxsem_timedwait_uninterruptible
+ *
+ * Description:
+ *   This function is wrapped version of nxsem_timedwait(), which is
+ *   uninterruptible and convenient for use.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   clockid - The timing source to use in the conversion
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned Value:
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   ECANCELED May be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int nxsem_clockwait_uninterruptible(FAR sem_t *sem, clockid_t clockid,
+                                    FAR const struct timespec *abstime)
+{
+  int ret;
+
+  do
+    {
+      /* Take the semaphore (perhaps waiting) */
+
+      ret = nxsem_clockwait(sem, clockid, abstime);
+    }
+  while (ret == -EINTR);
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: sem_clockwait
+ *
+ * Description:
+ *   This function will lock the semaphore referenced by sem as in the
+ *   sem_wait() function. However, if the semaphore cannot be locked without
+ *   waiting for another process or thread to unlock the semaphore by
+ *   performing a sem_post() function, this wait will be terminated when the
+ *   specified timeout expires.
+ *
+ *   The timeout will expire when the absolute time specified by abstime
+ *   passes, as measured by the clock on which timeouts are based (that is,
+ *   when the value of that clock equals or exceeds abstime), or if the
+ *   absolute time specified by abstime has already been passed at the
+ *   time of the call.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   clockid - The timing source to use in the conversion
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success.  On failure, -1 (ERROR) is returned
+ *   and the errno is set appropriately:
+ *
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   EINTR     A signal interrupted this function.
+ *   ECANCELED May be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int sem_clockwait(FAR sem_t *sem, clockid_t clockid,
+                  FAR const struct timespec *abstime)
+{
+  int ret;
+
+  /* sem_timedwait() is a cancellation point */
+
+  enter_cancellation_point();
+
+  /* Let nxsem_timedout() do the work */
+
+  ret = nxsem_clockwait(sem, clockid, abstime);
+  if (ret < 0)
+    {
+      set_errno(-ret);
+      ret = ERROR;
+    }
+
+  leave_cancellation_point();
+  return ret;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_destroy.c

@@ -0,0 +1,125 @@
+/****************************************************************************
+ * sched/semaphore/sem_destroy.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 <nuttx/config.h>
+
+#include <errno.h>
+
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_destroy
+ *
+ * Description:
+ *   This function is used to destroy the un-named semaphore indicated by
+ *   'sem'.  Only a semaphore that was created using nxsem_init() may be
+ *   destroyed using nxsem_destroy(); the effect of calling nxsem_destroy()
+ *   with a named semaphore is undefined.  The effect of subsequent use of
+ *   the semaphore sem is undefined until sem is re-initialized by another
+ *   call to nxsem_init().
+ *
+ *   The effect of destroying a semaphore upon which other processes are
+ *   currently blocked is undefined.
+ *
+ * Input Parameters:
+ *   sem - Semaphore to be destroyed.
+ *
+ * Returned 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_destroy (FAR sem_t *sem)
+{
+  /* Assure a valid semaphore is specified */
+
+  if (sem != NULL)
+    {
+      /* There is really no particular action that we need
+       * take to destroy a semaphore.  We will just reset
+       * the count to some reasonable value (1) and release
+       * ownership.
+       *
+       * Check if other threads are waiting on the semaphore.
+       * In this case, the behavior is undefined.  We will:
+       * leave the count unchanged but still return OK.
+       */
+
+      if (sem->semcount >= 0)
+        {
+          sem->semcount = 1;
+        }
+
+      /* Release holders of the semaphore */
+
+      nxsem_destroyholder(sem);
+      return OK;
+    }
+
+  return -EINVAL;
+}
+
+/****************************************************************************
+ * Name: sem_destroy
+ *
+ * Description:
+ *   This function is used to destroy the un-named semaphore indicated by
+ *   'sem'.  Only a semaphore that was created using nxsem_init() may be
+ *   destroyed using sem_destroy(); the effect of calling sem_destroy() with
+ *   a named semaphore is undefined.  The effect of subsequent use of the
+ *   semaphore sem is undefined until sem is re-initialized by another call
+ *   to nxsem_init().
+ *
+ *   The effect of destroying a semaphore upon which other processes are
+ *   currently blocked is undefined.
+ *
+ * Input Parameters:
+ *   sem - Semaphore to be destroyed.
+ *
+ * Returned Value:
+ *   This function is a application interface.  It returns zero (OK) if
+ *   successful.  Otherwise, -1 (ERROR) is returned and the errno value is
+ *   set appropriately.
+ *
+ ****************************************************************************/
+
+int sem_destroy (FAR sem_t *sem)
+{
+  int ret;
+
+  ret = nxsem_destroy(sem);
+  if (ret < 0)
+    {
+      set_errno(-ret);
+      ret = ERROR;
+    }
+
+  return ret;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_initialize.c

@@ -0,0 +1,62 @@
+/****************************************************************************
+ * sched/semaphore/sem_initialize.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 <nuttx/config.h>
+
+#include "semaphore/semaphore.h"
+
+/* Currently only need to setup priority inheritance logic */
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_initialize
+ *
+ * Description:
+ *   The control structures for all semaphores may be initialized by calling
+ *   nxsem_initialize().  This should be done once at power-on.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *   Called once during OS startup initialization
+ *
+ ****************************************************************************/
+
+void nxsem_initialize(void)
+{
+  /* Initialize holder structures needed to support priority inheritance */
+
+  nxsem_initialize_holders();
+}
+
+#endif /* CONFIG_PRIORITY_INHERITANCE */

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_post.c

@@ -0,0 +1,228 @@
+/****************************************************************************
+ * sched/semaphore/sem_post.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 <nuttx/config.h>
+
+#include <limits.h>
+#include <errno.h>
+#include <sched.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+
+#include "sched/sched.h"
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_post
+ *
+ * Description:
+ *   When a kernel thread has finished with a semaphore, it will call
+ *   nxsem_post().  This function unlocks the semaphore referenced by sem
+ *   by performing the semaphore unlock operation on that semaphore.
+ *
+ *   If the semaphore value resulting from this operation is positive, then
+ *   no tasks were blocked waiting for the semaphore to become unlocked; the
+ *   semaphore is simply incremented.
+ *
+ *   If the value of the semaphore resulting from this operation is zero,
+ *   then one of the tasks blocked waiting for the semaphore shall be
+ *   allowed to return successfully from its call to nxsem_wait().
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor
+ *
+ * Returned 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.
+ *
+ * Assumptions:
+ *   This function may be called from an interrupt handler.
+ *
+ ****************************************************************************/
+
+int nxsem_post(FAR sem_t *sem)
+{
+  FAR struct tcb_s *stcb = NULL;
+  irqstate_t flags;
+  int ret = -EINVAL;
+
+  /* Make sure we were supplied with a valid semaphore. */
+
+  if (sem != NULL)
+    {
+      /* The following operations must be performed with interrupts
+       * disabled because sem_post() may be called from an interrupt
+       * handler.
+       */
+
+      flags = enter_critical_section();
+
+      /* Check the maximum allowable value */
+
+      if (sem->semcount >= SEM_VALUE_MAX)
+        {
+          leave_critical_section(flags);
+          return -EOVERFLOW;
+        }
+
+      /* Perform the semaphore unlock operation, releasing this task as a
+       * holder then also incrementing the count on the semaphore.
+       *
+       * NOTE:  When semaphores are used for signaling purposes, the holder
+       * of the semaphore may not be this thread!  In this case,
+       * nxsem_release_holder() will do nothing.
+       *
+       * In the case of a mutex this could be simply resolved since there is
+       * only one holder but for the case of counting semaphores, there may
+       * be many holders and if the holder is not this thread, then it is
+       * not possible to know which thread/holder should be released.
+       *
+       * For this reason, it is recommended that priority inheritance be
+       * disabled via nxsem_set_protocol(SEM_PRIO_NONE) when the semaphore is
+       * initialized if the semaphore is to used for signaling purposes.
+       */
+
+      nxsem_release_holder(sem);
+      sem->semcount++;
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+      /* 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.
+       *
+       * NOTE: If this sched_lock is called from an interrupt handler, it
+       * will do nothing.
+       */
+
+      sched_lock();
+#endif
+      /* If the result of semaphore unlock is non-positive, then
+       * there must be some task waiting for the semaphore.
+       */
+
+      if (sem->semcount <= 0)
+        {
+          /* Check if there are any tasks in the waiting for semaphore
+           * task list that are waiting for this semaphore. This is a
+           * prioritized list so the first one we encounter is the one
+           * that we want.
+           */
+
+          for (stcb = (FAR struct tcb_s *)g_waitingforsemaphore.head;
+               (stcb && stcb->waitsem != sem);
+               stcb = stcb->flink);
+
+          if (stcb != NULL)
+            {
+              /* The task will be the new holder of the semaphore when
+               * it is awakened.
+               */
+
+              nxsem_add_holder_tcb(stcb, sem);
+
+              /* It is, let the task take the semaphore */
+
+              stcb->waitsem = NULL;
+
+              /* Restart the waiting task. */
+
+              up_unblock_task(stcb);
+            }
+#if 0 /* REVISIT:  This can fire on IOB throttle semaphore */
+          else
+            {
+              /* This should not happen. */
+
+              DEBUGPANIC();
+            }
+#endif
+        }
+
+      /* Check if we need to drop the priority of any threads holding
+       * this semaphore.  The priority could have been boosted while they
+       * held the semaphore.
+       */
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+      nxsem_restore_baseprio(stcb, sem);
+      sched_unlock();
+#endif
+      ret = OK;
+
+      /* Interrupts may now be enabled. */
+
+      leave_critical_section(flags);
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: sem_post
+ *
+ * Description:
+ *   When a task has finished with a semaphore, it will call sem_post().
+ *   This function unlocks the semaphore referenced by sem by performing the
+ *   semaphore unlock operation on that semaphore.
+ *
+ *   If the semaphore value resulting from this operation is positive, then
+ *   no tasks were blocked waiting for the semaphore to become unlocked; the
+ *   semaphore is simply incremented.
+ *
+ *   If the value of the semaphore resulting from this operation is zero,
+ *   then one of the tasks blocked waiting for the semaphore shall be
+ *   allowed to return successfully from its call to nxsem_wait().
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor
+ *
+ * Returned Value:
+ *   This function is a standard, POSIX application interface.  It will
+ *   return zero (OK) if successful.  Otherwise, -1 (ERROR) is returned and
+ *   the errno value is set appropriately.
+ *
+ * Assumptions:
+ *   This function may be called from an interrupt handler.
+ *
+ ****************************************************************************/
+
+int sem_post(FAR sem_t *sem)
+{
+  int ret;
+
+  ret = nxsem_post(sem);
+  if (ret < 0)
+    {
+      set_errno(-ret);
+      ret = ERROR;
+    }
+
+  return ret;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_recover.c

@@ -0,0 +1,110 @@
+/****************************************************************************
+ * sched/semaphore/sem_recover.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 <nuttx/config.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <nuttx/sched.h>
+
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_recover
+ *
+ * Description:
+ *   This function is called from nxtask_recover() when a task is deleted via
+ *   task_delete() or via pthread_cancel().  It current only checks on the
+ *   case where a task is waiting for semaphore at the time that is was
+ *   killed.
+ *
+ *   REVISIT:  A more complete implementation would release counts on all
+ *   semaphores held by the thread.  That would, however, require some
+ *   significant extension to the semaphore data structures because given
+ *   only the task, there is not mechanism to traverse all of the semaphores
+ *   with counts held by the task.
+ *
+ * Input Parameters:
+ *   tcb - The TCB of the terminated task or thread
+ *
+ * Returned Value:
+ *   None.
+ *
+ * Assumptions:
+ *   This function is called from task deletion logic in a safe context.
+ *
+ ****************************************************************************/
+
+void nxsem_recover(FAR struct tcb_s *tcb)
+{
+  irqstate_t flags;
+
+  /* The task is being deleted.  If it is waiting for a semaphore, then
+   * increment the count on the semaphores.  This logic is almost identical
+   * to what you see in nxsem_wait_irq() except that no attempt is made to
+   * restart the exiting task.
+   *
+   * NOTE:  In the case that the task is waiting we can assume: (1) That the
+   * task state is TSTATE_WAIT_SEM and (2) that the 'waitsem' in the TCB is
+   * non-null.  If we get here via pthread_cancel() or via task_delete(),
+   * then the task state should be preserved; it will be altered in other
+   * cases but in those cases waitsem should be NULL anyway (but we do not
+   * enforce that here).
+   */
+
+  flags = enter_critical_section();
+  if (tcb->task_state == TSTATE_WAIT_SEM)
+    {
+      sem_t *sem = tcb->waitsem;
+      DEBUGASSERT(sem != NULL && sem->semcount < 0);
+
+      /* Restore the correct priority of all threads that hold references
+       * to this semaphore.
+       */
+
+      nxsem_canceled(tcb, sem);
+
+      /* And increment the count on the semaphore.  This releases the count
+       * that was taken by sem_wait().  This count decremented the semaphore
+       * count to negative and caused the thread to be blocked in the first
+       * place.
+       */
+
+      sem->semcount++;
+
+      /* Clear the semaphore to assure that it is not reused.  But leave the
+       * state as TSTATE_WAIT_SEM.  This is necessary because this is a
+       * necessary indication that the TCB still resides in the waiting-for-
+       * semaphore list.
+       */
+
+      tcb->waitsem = NULL;
+    }
+
+  leave_critical_section(flags);
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_reset.c

@@ -0,0 +1,109 @@
+/****************************************************************************
+ * sched/semaphore/sem_reset.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 <nuttx/config.h>
+
+#include <sched.h>
+#include <assert.h>
+
+#include <nuttx/irq.h>
+
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_reset
+ *
+ * Description:
+ *   Reset a semaphore count to a specific value.  This is similar to part
+ *   of the operation of nxsem_init().  But nxsem_reset() may need to wake up
+ *   tasks waiting on a count.  This kind of operation is sometimes required
+ *   within the OS (only) for certain error handling conditions.
+ *
+ * Input Parameters:
+ *   sem   - Semaphore descriptor to be reset
+ *   count - The requested semaphore count
+ *
+ * Returned Value:
+ *   This is an internal OS interface, not available to applications, and
+ *   hence follows the NuttX internal error return policy:  Zero (OK) is
+ *   returned on success.  A negated errno value is returned on failure.
+ *
+ ****************************************************************************/
+
+int nxsem_reset(FAR sem_t *sem, int16_t count)
+{
+  irqstate_t flags;
+
+  DEBUGASSERT(sem != NULL && count >= 0);
+
+  /* Don't allow any context switches that may result from the following
+   * nxsem_post() operations.
+   */
+
+  sched_lock();
+
+  /* Prevent any access to the semaphore by interrupt handlers while we are
+   * performing this operation.
+   */
+
+  flags = enter_critical_section();
+
+  /* A negative count indicates that the negated number of threads are
+   * waiting to take a count from the semaphore.  Loop here, handing
+   * out counts to any waiting threads.
+   */
+
+  while (sem->semcount < 0 && count > 0)
+    {
+      /* Give out one counting, waking up one of the waiting threads
+       * and, perhaps, kicking off a lot of priority inheritance
+       * logic (REVISIT).
+       */
+
+      DEBUGVERIFY(nxsem_post(sem));
+      count--;
+    }
+
+  /* We exit the above loop with either (1) no threads waiting for the
+   * (i.e., with sem->semcount >= 0).  In this case, 'count' holds the
+   * the new value of the semaphore count.  OR (2) with threads still
+   * waiting but all of the semaphore counts exhausted:  The current
+   * value of sem->semcount is already correct in this case.
+   */
+
+  if (sem->semcount >= 0)
+    {
+      sem->semcount = count;
+    }
+
+  /* Allow any pending context switches to occur now */
+
+  leave_critical_section(flags);
+  sched_unlock();
+  return OK;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_setprotocol.c

@@ -0,0 +1,163 @@
+/****************************************************************************
+ * sched/semaphore/sem_setprotocol.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 <nuttx/config.h>
+
+#include <assert.h>
+#include <errno.h>
+
+#include "semaphore/semaphore.h"
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_set_protocol
+ *
+ * Description:
+ *    Set semaphore protocol attribute.
+ *
+ *    One particularly important use of this function is when a semaphore
+ *    is used for inter-task communication like:
+ *
+ *      TASK A                 TASK B
+ *      nxsem_init(sem, 0, 0);
+ *      nxsem_wait(sem);
+ *                             nxsem_post(sem);
+ *      Awakens as holder
+ *
+ *    In this case priority inheritance can interfere with the operation of
+ *    the semaphore.  The problem is that when TASK A is restarted it is a
+ *    holder of the semaphore.  However, it never calls nxsem_post(sem) so it
+ *    becomes *permanently* a holder of the semaphore and may have its
+ *    priority boosted when any other task tries to acquire the semaphore.
+ *
+ *    The fix is to call nxsem_set_protocol(SEM_PRIO_NONE) immediately after
+ *    the sem_init() call so that there will be no priority inheritance
+ *    operations on this semaphore.
+ *
+ * Input Parameters:
+ *    sem      - A pointer to the semaphore whose attributes are to be
+ *               modified
+ *    protocol - The new protocol to use
+ *
+ * Returned 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_set_protocol(FAR sem_t *sem, int protocol)
+{
+  DEBUGASSERT(sem != NULL);
+
+  switch (protocol)
+    {
+      case SEM_PRIO_NONE:
+
+        /* Disable priority inheritance */
+
+        sem->flags |= PRIOINHERIT_FLAGS_DISABLE;
+
+        /* Remove any current holders */
+
+        nxsem_destroyholder(sem);
+        return OK;
+
+      case SEM_PRIO_INHERIT:
+
+        /* Enable priority inheritance (dangerous) */
+
+        sem->flags &= ~PRIOINHERIT_FLAGS_DISABLE;
+        return OK;
+
+      case SEM_PRIO_PROTECT:
+
+        /* Not yet supported */
+
+        return -ENOSYS;
+
+      default:
+        break;
+    }
+
+  return -EINVAL;
+}
+
+/****************************************************************************
+ * Name: sem_setprotocol
+ *
+ * Description:
+ *    Set semaphore protocol attribute.
+ *
+ *    One particularly important use of this function is when a semaphore
+ *    is used for inter-task communication like:
+ *
+ *      TASK A                 TASK B
+ *      sem_init(sem, 0, 0);
+ *      nxsem_wait(sem);
+ *                             sem_post(sem);
+ *      Awakens as holder
+ *
+ *    In this case priority inheritance can interfere with the operation of
+ *    the semaphore.  The problem is that when TASK A is restarted it is a
+ *    holder of the semaphore.  However, it never calls sem_post(sem) so it
+ *    becomes *permanently* a holder of the semaphore and may have its
+ *    priority boosted when any other task tries to acquire the semaphore.
+ *
+ *    The fix is to call sem_setprotocol(SEM_PRIO_NONE) immediately after
+ *    the sem_init() call so that there will be no priority inheritance
+ *    operations on this semaphore.
+ *
+ * Input Parameters:
+ *    sem      - A pointer to the semaphore whose attributes are to be
+ *               modified
+ *    protocol - The new protocol to use
+ *
+ * Returned Value:
+ *   This function is exposed as a non-standard application interface.  It
+ *   returns zero (OK) if successful.  Otherwise, -1 (ERROR) is returned and
+ *   the errno value is set appropriately.
+ *
+ ****************************************************************************/
+
+int sem_setprotocol(FAR sem_t *sem, int protocol)
+{
+  int ret;
+
+  ret = nxsem_set_protocol(sem, protocol);
+  if (ret < 0)
+    {
+      set_errno(-ret);
+      ret = ERROR;
+    }
+
+  return ret;
+}
+
+#endif /* CONFIG_PRIORITY_INHERITANCE */

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_tickwait.c

@@ -0,0 +1,174 @@
+/****************************************************************************
+ * sched/semaphore/sem_tickwait.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 <nuttx/config.h>
+
+#include <stdint.h>
+#include <unistd.h>
+#include <time.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+#include <nuttx/wdog.h>
+
+#include "sched/sched.h"
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_tickwait
+ *
+ * Description:
+ *   This function is a lighter weight version of sem_timedwait().  It is
+ *   non-standard and intended only for use within the RTOS.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   start   - The system time that the delay is relative to.  If the
+ *             current time is not the same as the start time, then the
+ *             delay will be adjust so that the end time will be the same
+ *             in any event.
+ *   delay   - Ticks to wait from the start time until the semaphore is
+ *             posted.  If ticks is zero, then this function is equivalent
+ *             to nxsem_trywait().
+ *
+ * Returned Value:
+ *   This is an internal OS interface, not available to applications, and
+ *   hence follows the NuttX internal error return policy:  Zero (OK) is
+ *   returned on success.  A negated errno value is returned on failure.
+ *   -ETIMEDOUT is returned on the timeout condition.
+ *
+ ****************************************************************************/
+
+int nxsem_tickwait(FAR sem_t *sem, clock_t start, uint32_t delay)
+{
+  FAR struct tcb_s *rtcb = this_task();
+  clock_t elapsed;
+  int ret;
+
+  DEBUGASSERT(sem != NULL && up_interrupt_context() == false);
+
+  /* NOTE: We do not need a critical section here, because
+   * nxsem_wait() and nxsem_timeout() use a critical section
+   * in the functions.
+   */
+
+  /* Try to take the semaphore without waiting. */
+
+  ret = nxsem_trywait(sem);
+  if (ret == OK)
+    {
+      /* We got it! */
+
+      goto out;
+    }
+
+  /* We will have to wait for the semaphore.  Make sure that we were provided
+   * with a valid timeout.
+   */
+
+  if (delay == 0)
+    {
+      /* Return the errno from nxsem_trywait() */
+
+      goto out;
+    }
+
+  /* Adjust the delay for any time since the delay was calculated */
+
+  elapsed = clock_systime_ticks() - start;
+  if (/* elapsed >= (UINT32_MAX / 2) || */ elapsed >= delay)
+    {
+      ret = -ETIMEDOUT;
+      goto out;
+    }
+
+  delay -= elapsed;
+
+  /* Start the watchdog with interrupts still disabled */
+
+  wd_start(&rtcb->waitdog, delay, nxsem_timeout, getpid());
+
+  /* Now perform the blocking wait */
+
+  ret = nxsem_wait(sem);
+
+  /* Stop the watchdog timer */
+
+  wd_cancel(&rtcb->waitdog);
+
+out:
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: nxsem_tickwait_uninterruptible
+ *
+ * Description:
+ *   This function is wrapped version of nxsem_tickwait(), which is
+ *   uninterruptible and convenient for use.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   start   - The system time that the delay is relative to.  If the
+ *             current time is not the same as the start time, then the
+ *             delay will be adjust so that the end time will be the same
+ *             in any event.
+ *   delay   - Ticks to wait from the start time until the semaphore is
+ *             posted.  If ticks is zero, then this function is equivalent
+ *             to sem_trywait().
+ *
+ * Returned Value:
+ *   This is an internal OS interface, not available to applications, and
+ *   hence follows the NuttX internal error return policy:  Zero (OK) is
+ *   returned on success.  A negated errno value is returned on failure:
+ *
+ *     -ETIMEDOUT is returned on the timeout condition.
+ *     -ECANCELED may be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int nxsem_tickwait_uninterruptible(FAR sem_t *sem, clock_t start,
+                                   uint32_t delay)
+{
+  int ret;
+
+  do
+    {
+      /* Take the semaphore (perhaps waiting) */
+
+      ret = nxsem_tickwait(sem, start, delay);
+    }
+  while (ret == -EINTR);
+
+  return ret;
+}
+

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_timedwait.c

@@ -0,0 +1,147 @@
+/****************************************************************************
+ * sched/semaphore/sem_timedwait.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 <nuttx/semaphore.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_timedwait
+ *
+ * Description:
+ *   This function will lock the semaphore referenced by sem as in the
+ *   sem_wait() function. However, if the semaphore cannot be locked without
+ *   waiting for another process or thread to unlock the semaphore by
+ *   performing a sem_post() function, this wait will be terminated when the
+ *   specified timeout expires.
+ *
+ *   The timeout will expire when the absolute time specified by abstime
+ *   passes, as measured by the clock on which timeouts are based (that is,
+ *   when the value of that clock equals or exceeds abstime), or if the
+ *   absolute time specified by abstime has already been passed at the
+ *   time of the call.
+ *
+ *   This is an internal OS interface.  It is functionally equivalent to
+ *   sem_wait except that:
+ *
+ *   - It is not a cancellation point, and
+ *   - It does not modify the errno value.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned 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.
+ *   That may be one of:
+ *
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   EINTR     A signal interrupted this function.
+ *
+ ****************************************************************************/
+
+int nxsem_timedwait(FAR sem_t *sem, FAR const struct timespec *abstime)
+{
+  return nxsem_clockwait(sem, CLOCK_REALTIME, abstime);
+}
+
+/****************************************************************************
+ * Name: nxsem_timedwait_uninterruptible
+ *
+ * Description:
+ *   This function is wrapped version of nxsem_timedwait(), which is
+ *   uninterruptible and convenient for use.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned Value:
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   ECANCELED May be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int nxsem_timedwait_uninterruptible(FAR sem_t *sem,
+                                    FAR const struct timespec *abstime)
+{
+  return nxsem_clockwait_uninterruptible(sem, CLOCK_REALTIME, abstime);
+}
+
+/****************************************************************************
+ * Name: sem_timedwait
+ *
+ * Description:
+ *   This function will lock the semaphore referenced by sem as in the
+ *   sem_wait() function. However, if the semaphore cannot be locked without
+ *   waiting for another process or thread to unlock the semaphore by
+ *   performing a sem_post() function, this wait will be terminated when the
+ *   specified timeout expires.
+ *
+ *   The timeout will expire when the absolute time specified by abstime
+ *   passes, as measured by the clock on which timeouts are based (that is,
+ *   when the value of that clock equals or exceeds abstime), or if the
+ *   absolute time specified by abstime has already been passed at the
+ *   time of the call.
+ *
+ * Input Parameters:
+ *   sem     - Semaphore object
+ *   abstime - The absolute time to wait until a timeout is declared.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success.  On failure, -1 (ERROR) is returned
+ *   and the errno is set appropriately:
+ *
+ *   EINVAL    The sem argument does not refer to a valid semaphore.  Or the
+ *             thread would have blocked, and the abstime parameter specified
+ *             a nanoseconds field value less than zero or greater than or
+ *             equal to 1000 million.
+ *   ETIMEDOUT The semaphore could not be locked before the specified timeout
+ *             expired.
+ *   EDEADLK   A deadlock condition was detected.
+ *   EINTR     A signal interrupted this function.
+ *   ECANCELED May be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int sem_timedwait(FAR sem_t *sem, FAR const struct timespec *abstime)
+{
+  return sem_clockwait(sem, CLOCK_REALTIME, abstime);
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_timeout.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ * sched/semaphore/sem_timeout.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 <nuttx/config.h>
+
+#include <errno.h>
+
+#include <nuttx/sched.h>
+#include <nuttx/wdog.h>
+
+#include <nuttx/irq.h>
+
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_timeout
+ *
+ * Description:
+ *   This function is called if the timeout elapses before before a
+ *   semaphore is acquired.
+ *
+ * Input Parameters:
+ *   pid  - The task ID of the task to wakeup
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *   - Called from the context of the timer interrupt handler.
+ *
+ ****************************************************************************/
+
+void nxsem_timeout(wdparm_t pid)
+{
+  FAR struct tcb_s *wtcb;
+  irqstate_t flags;
+
+  /* Disable interrupts to avoid race conditions */
+
+  flags = enter_critical_section();
+
+  /* Get the TCB associated with this PID.  It is possible that
+   * task may no longer be active when this watchdog goes off.
+   */
+
+  wtcb = nxsched_get_tcb(pid);
+
+  /* It is also possible that an interrupt/context switch beat us to the
+   * punch and already changed the task's state.
+   */
+
+  if (wtcb && wtcb->task_state == TSTATE_WAIT_SEM)
+    {
+      /* Cancel the semaphore wait */
+
+      nxsem_wait_irq(wtcb, ETIMEDOUT);
+    }
+
+  /* Interrupts may now be enabled. */
+
+  leave_critical_section(flags);
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_trywait.c

@@ -0,0 +1,147 @@
+/****************************************************************************
+ * sched/semaphore/sem_trywait.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 <nuttx/config.h>
+
+#include <stdbool.h>
+#include <sched.h>
+#include <errno.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+
+#include "sched/sched.h"
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_trywait
+ *
+ * Description:
+ *   This function locks the specified semaphore only if the semaphore is
+ *   currently not locked.  In either case, the call returns without
+ *   blocking.
+ *
+ * Input Parameters:
+ *   sem - the semaphore descriptor
+ *
+ * Returned 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.
+ *   Possible returned errors:
+ *
+ *     EINVAL - Invalid attempt to get the semaphore
+ *     EAGAIN - The semaphore is not available.
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+int nxsem_trywait(FAR sem_t *sem)
+{
+  FAR struct tcb_s *rtcb = this_task();
+  irqstate_t flags;
+  int ret;
+
+  /* This API should not be called from interrupt handlers */
+
+  DEBUGASSERT(sem != NULL && up_interrupt_context() == false);
+
+  if (sem != NULL)
+    {
+      /* The following operations must be performed with interrupts disabled
+       * because sem_post() may be called from an interrupt handler.
+       */
+
+      flags = enter_critical_section();
+
+      /* If the semaphore is available, give it to the requesting task */
+
+      if (sem->semcount > 0)
+        {
+          /* It is, let the task take the semaphore */
+
+          sem->semcount--;
+          rtcb->waitsem = NULL;
+          ret = OK;
+        }
+      else
+        {
+          /* Semaphore is not available */
+
+          ret = -EAGAIN;
+        }
+
+      /* Interrupts may now be enabled. */
+
+      leave_critical_section(flags);
+    }
+  else
+    {
+      ret = -EINVAL;
+    }
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: sem_trywait
+ *
+ * Description:
+ *   This function locks the specified semaphore only if the semaphore is
+ *   currently not locked.  In either case, the call returns without
+ *   blocking.
+ *
+ * Input Parameters:
+ *   sem - the semaphore descriptor
+ *
+ * Returned Value:
+ *   Zero (OK) on success or -1 (ERROR) if unsuccessful. If this function
+ *   returns -1(ERROR), then the cause of the failure will be reported in
+ *   errno variable as:
+ *
+ *     EINVAL - Invalid attempt to get the semaphore
+ *     EAGAIN - The semaphore is not available.
+ *
+ ****************************************************************************/
+
+int sem_trywait(FAR sem_t *sem)
+{
+  int ret;
+
+  /* Let nxsem_trywait do the real work */
+
+  ret = nxsem_trywait(sem);
+  if (ret < 0)
+    {
+      set_errno(-ret);
+      ret = ERROR;
+    }
+
+  return ret;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_wait.c

@@ -0,0 +1,285 @@
+/****************************************************************************
+ * sched/semaphore/sem_wait.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 <nuttx/config.h>
+
+#include <stdbool.h>
+#include <errno.h>
+#include <assert.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <nuttx/cancelpt.h>
+
+#include "sched/sched.h"
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_wait
+ *
+ * Description:
+ *   This function attempts to lock the semaphore referenced by 'sem'.  If
+ *   the semaphore value is (<=) zero, then the calling task will not return
+ *   until it successfully acquires the lock.
+ *
+ *   This is an internal OS interface.  It is functionally equivalent to
+ *   sem_wait except that:
+ *
+ *   - It is not a cancellation point, and
+ *   - It does not modify the errno value.
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor.
+ *
+ * Returned 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.
+ *   Possible returned errors:
+ *
+ *   - EINVAL:  Invalid attempt to get the semaphore
+ *   - EINTR:   The wait was interrupted by the receipt of a signal.
+ *
+ ****************************************************************************/
+
+int nxsem_wait(FAR sem_t *sem)
+{
+  FAR struct tcb_s *rtcb = this_task();
+  irqstate_t flags;
+  int ret = -EINVAL;
+
+  /* This API should not be called from interrupt handlers */
+
+  DEBUGASSERT(sem != NULL && up_interrupt_context() == false);
+
+  /* The following operations must be performed with interrupts
+   * disabled because nxsem_post() may be called from an interrupt
+   * handler.
+   */
+
+  flags = enter_critical_section();
+
+  /* Make sure we were supplied with a valid semaphore. */
+
+  if (sem != NULL)
+    {
+      /* Check if the lock is available */
+
+      if (sem->semcount > 0)
+        {
+          /* It is, let the task take the semaphore. */
+
+          sem->semcount--;
+          nxsem_add_holder(sem);
+          rtcb->waitsem = NULL;
+          ret = OK;
+        }
+
+      /* The semaphore is NOT available, We will have to block the
+       * current thread of execution.
+       */
+
+      else
+        {
+          /* First, verify that the task is not already waiting on a
+           * semaphore
+           */
+
+          DEBUGASSERT(rtcb->waitsem == NULL);
+
+          /* Handle the POSIX semaphore (but don't set the owner yet) */
+
+          sem->semcount--;
+
+          /* Save the waited on semaphore in the TCB */
+
+          rtcb->waitsem = sem;
+
+          /* If priority inheritance is enabled, then check the priority of
+           * the holder of the semaphore.
+           */
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+          /* Disable context switching.  The following operations must be
+           * atomic with regard to the scheduler.
+           */
+
+          sched_lock();
+
+          /* Boost the priority of any threads holding a count on the
+           * semaphore.
+           */
+
+          nxsem_boost_priority(sem);
+#endif
+          /* Set the errno value to zero (preserving the original errno)
+           * value).  We reuse the per-thread errno to pass information
+           * between sem_waitirq() and this functions.
+           */
+
+          rtcb->errcode = OK;
+
+          /* Add the TCB to the prioritized semaphore wait queue, after
+           * checking this is not the idle task - descheduling that
+           * isn't going to end well.
+           */
+
+          DEBUGASSERT(NULL != rtcb->flink);
+          up_block_task(rtcb, TSTATE_WAIT_SEM);
+
+          /* When we resume at this point, either (1) the semaphore has been
+           * assigned to this thread of execution, or (2) the semaphore wait
+           * has been interrupted by a signal or a timeout.  We can detect
+           * these latter cases be examining the per-thread errno value.
+           *
+           * In the event that the semaphore wait was interrupted by a
+           * signal or a timeout, certain semaphore clean-up operations have
+           * already been performed (see sem_waitirq.c).  Specifically:
+           *
+           * - nxsem_canceled() was called to restore the priority of all
+           *   threads that hold a reference to the semaphore,
+           * - The semaphore count was decremented, and
+           * - tcb->waitsem was nullifed.
+           *
+           * It is necessary to do these things in sem_waitirq.c because a
+           * long time may elapse between the time that the signal was issued
+           * and this thread is awakened and this leaves a door open to
+           * several race conditions.
+           */
+
+          /* Check if an error occurred while we were sleeping.  Expected
+           * errors include EINTR meaning that we were awakened by a signal
+           * or ETIMEDOUT meaning that the timer expired for the case of
+           * sem_timedwait().
+           *
+           * If we were not awakened by a signal or a timeout, then
+           * nxsem_add_holder() was called by logic in sem_wait() fore this
+           * thread was restarted.
+           */
+
+          ret = rtcb->errcode != OK ? -rtcb->errcode : OK;
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+          sched_unlock();
+#endif
+        }
+    }
+
+  leave_critical_section(flags);
+  return ret;
+}
+
+/****************************************************************************
+ * Name: nxsem_wait_uninterruptible
+ *
+ * Description:
+ *   This function is wrapped version of nxsem_wait(), which is
+ *   uninterruptible and convenient for use.
+ *
+ * Parameters:
+ *   sem - Semaphore descriptor.
+ *
+ * Return Value:
+ *   Zero(OK)  - On success
+ *   EINVAL    - Invalid attempt to get the semaphore
+ *   ECANCELED - May be returned if the thread is canceled while waiting.
+ *
+ ****************************************************************************/
+
+int nxsem_wait_uninterruptible(FAR sem_t *sem)
+{
+  int ret;
+
+  do
+    {
+      /* Take the semaphore (perhaps waiting) */
+
+      ret = nxsem_wait(sem);
+    }
+  while (ret == -EINTR);
+
+  return ret;
+}
+
+/****************************************************************************
+ * Name: sem_wait
+ *
+ * Description:
+ *   This function attempts to lock the semaphore referenced by 'sem'.  If
+ *   the semaphore value is (<=) zero, then the calling task will not return
+ *   until it successfully acquires the lock.
+ *
+ * Input Parameters:
+ *   sem - Semaphore descriptor.
+ *
+ * Returned Value:
+ *   This function is a standard, POSIX application interface.  It returns
+ *   zero (OK) if successful.  Otherwise, -1 (ERROR) is returned and
+ *   the errno value is set appropriately.  Possible errno values include:
+ *
+ *   - EINVAL:  Invalid attempt to get the semaphore
+ *   - EINTR:   The wait was interrupted by the receipt of a signal.
+ *
+ ****************************************************************************/
+
+int sem_wait(FAR sem_t *sem)
+{
+  int errcode;
+  int ret;
+
+  /* sem_wait() is a cancellation point */
+
+  if (enter_cancellation_point())
+    {
+#ifdef CONFIG_CANCELLATION_POINTS
+      /* If there is a pending cancellation, then do not perform
+       * the wait.  Exit now with ECANCELED.
+       */
+
+      errcode = ECANCELED;
+      goto errout_with_cancelpt;
+#endif
+    }
+
+  /* Let nxsem_wait() do the real work */
+
+  ret = nxsem_wait(sem);
+  if (ret < 0)
+    {
+      errcode = -ret;
+      goto errout_with_cancelpt;
+    }
+
+  leave_cancellation_point();
+  return OK;
+
+errout_with_cancelpt:
+  set_errno(errcode);
+  leave_cancellation_point();
+  return ERROR;
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/sem_waitirq.c

@@ -0,0 +1,115 @@
+/****************************************************************************
+ * sched/semaphore/sem_waitirq.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 <nuttx/config.h>
+
+#include <sched.h>
+#include <errno.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+
+#include "semaphore/semaphore.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsem_wait_irq
+ *
+ * Description:
+ *   This function is called when either:
+ *
+ *   1. A signal is received by a task that is waiting on a semaphore.
+ *      According to the POSIX spec, "...the calling thread shall not return
+ *      from the call to [nxsem_wait] until it either locks the semaphore or
+ *      the call is interrupted by a signal."
+ *   2. From logic associated with sem_timedwait().  This function is called
+ *      when the timeout elapses without receiving the semaphore.
+ *
+ * Input Parameters:
+ *   wtcb    - A pointer to the TCB of the task that is waiting on a
+ *             semphaphore, but has received a signal or timeout instead.
+ *   errcode - EINTR if the semaphore wait was awakened by a signal;
+ *             ETIMEDOUT if awakened by a timeout
+ *
+ * Returned Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ****************************************************************************/
+
+void nxsem_wait_irq(FAR struct tcb_s *wtcb, int errcode)
+{
+  irqstate_t flags;
+
+  /* Disable interrupts.  This is necessary (unfortunately) because an
+   * interrupt handler may attempt to post the semaphore while we are
+   * doing this.
+   */
+
+  flags = enter_critical_section();
+
+  /* It is possible that an interrupt/context switch beat us to the punch
+   * and already changed the task's state.
+   */
+
+  if (wtcb->task_state == TSTATE_WAIT_SEM)
+    {
+      sem_t *sem = wtcb->waitsem;
+      DEBUGASSERT(sem != NULL && sem->semcount < 0);
+
+      /* Restore the correct priority of all threads that hold references
+       * to this semaphore.
+       */
+
+      nxsem_canceled(wtcb, sem);
+
+      /* And increment the count on the semaphore.  This releases the count
+       * that was taken by sem_post().  This count decremented the semaphore
+       * count to negative and caused the thread to be blocked in the first
+       * place.
+       */
+
+      sem->semcount++;
+
+      /* Indicate that the semaphore wait is over. */
+
+      wtcb->waitsem = NULL;
+
+      /* Mark the errno value for the thread. */
+
+      wtcb->errcode = errcode;
+
+      /* Restart the task. */
+
+      up_unblock_task(wtcb);
+    }
+
+  /* Interrupts may now be enabled. */
+
+  leave_critical_section(flags);
+}

Ubiquitous/Nuttx/nuttx/sched/semaphore/semaphore.h

@@ -0,0 +1,98 @@
+/****************************************************************************
+ * sched/semaphore/semaphore.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __SCHED_SEMAPHORE_SEMAPHORE_H
+#define __SCHED_SEMAPHORE_SEMAPHORE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <nuttx/semaphore.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sched.h>
+#include <queue.h>
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C"
+{
+#else
+#define EXTERN extern
+#endif
+
+/* Common semaphore logic */
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+void nxsem_initialize(void);
+#else
+#  define nxsem_initialize()
+#endif
+
+/* Wake up a thread that is waiting on semaphore */
+
+void nxsem_wait_irq(FAR struct tcb_s *wtcb, int errcode);
+
+/* Handle semaphore timer expiration */
+
+void nxsem_timeout(wdparm_t pid);
+
+/* Recover semaphore resources with a task or thread is destroyed  */
+
+void nxsem_recover(FAR struct tcb_s *tcb);
+
+/* Special logic needed only by priority inheritance to manage collections of
+ * holders of semaphores.
+ */
+
+#ifdef CONFIG_PRIORITY_INHERITANCE
+void nxsem_initialize_holders(void);
+void nxsem_destroyholder(FAR sem_t *sem);
+void nxsem_add_holder(FAR sem_t *sem);
+void nxsem_add_holder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem);
+void nxsem_boost_priority(FAR sem_t *sem);
+void nxsem_release_holder(FAR sem_t *sem);
+void nxsem_restore_baseprio(FAR struct tcb_s *stcb, FAR sem_t *sem);
+void nxsem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem);
+#else
+#  define nxsem_initialize_holders()
+#  define nxsem_destroyholder(sem)
+#  define nxsem_add_holder(sem)
+#  define nxsem_add_holder_tcb(htcb,sem)
+#  define nxsem_boost_priority(sem)
+#  define nxsem_release_holder(sem)
+#  define nxsem_restore_baseprio(stcb,sem)
+#  define nxsem_canceled(stcb,sem)
+#endif
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SCHED_SEMAPHORE_SEMAPHORE_H */

Ubiquitous/Nuttx/nuttx/sched/semaphore/spinlock.c

@@ -0,0 +1,387 @@
+/****************************************************************************
+ * sched/semaphore/spinlock.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 <nuttx/config.h>
+
+#include <sys/types.h>
+#include <sched.h>
+#include <assert.h>
+
+#include <nuttx/spinlock.h>
+#include <nuttx/sched_note.h>
+#include <arch/irq.h>
+
+#include "sched/sched.h"
+
+#ifdef CONFIG_SPINLOCK
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: spin_lock
+ *
+ * Description:
+ *   If this CPU does not already hold the spinlock, then loop until the
+ *   spinlock is successfully locked.
+ *
+ *   This implementation is non-reentrant and is prone to deadlocks in
+ *   the case that any logic on the same CPU attempts to take the lock
+ *   more than one
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to lock.
+ *
+ * Returned Value:
+ *   None.  When the function returns, the spinlock was successfully locked
+ *   by this CPU.
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+void spin_lock(FAR volatile spinlock_t *lock)
+{
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  /* Notify that we are waiting for a spinlock */
+
+  sched_note_spinlock(this_task(), lock);
+#endif
+
+  while (up_testset(lock) == SP_LOCKED)
+    {
+      SP_DSB();
+      SP_WFE();
+    }
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  /* Notify that we have the spinlock */
+
+  sched_note_spinlocked(this_task(), lock);
+#endif
+  SP_DMB();
+}
+
+/****************************************************************************
+ * Name: spin_lock_wo_note
+ *
+ * Description:
+ *   If this CPU does not already hold the spinlock, then loop until the
+ *   spinlock is successfully locked.
+ *
+ *   This implementation is the same as the above spin_lock() except that
+ *   it does not perform instrumentation logic.
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to lock.
+ *
+ * Returned Value:
+ *   None.  When the function returns, the spinlock was successfully locked
+ *   by this CPU.
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+void spin_lock_wo_note(FAR volatile spinlock_t *lock)
+{
+  while (up_testset(lock) == SP_LOCKED)
+    {
+      SP_DSB();
+      SP_WFE();
+    }
+
+  SP_DMB();
+}
+
+/****************************************************************************
+ * Name: spin_trylock
+ *
+ * Description:
+ *   Try once to lock the spinlock.  Do not wait if the spinlock is already
+ *   locked.
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to lock.
+ *
+ * Returned Value:
+ *   SP_LOCKED   - Failure, the spinlock was already locked
+ *   SP_UNLOCKED - Success, the spinlock was successfully locked
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+spinlock_t spin_trylock(FAR volatile spinlock_t *lock)
+{
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  /* Notify that we are waiting for a spinlock */
+
+  sched_note_spinlock(this_task(), lock);
+#endif
+
+  if (up_testset(lock) == SP_LOCKED)
+    {
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+      /* Notify that we abort for a spinlock */
+
+      sched_note_spinabort(this_task(), &lock);
+#endif
+      SP_DSB();
+      return SP_LOCKED;
+    }
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  /* Notify that we have the spinlock */
+
+  sched_note_spinlocked(this_task(), lock);
+#endif
+  SP_DMB();
+  return SP_UNLOCKED;
+}
+
+/****************************************************************************
+ * Name: spin_trylock_wo_note
+ *
+ * Description:
+ *   Try once to lock the spinlock.  Do not wait if the spinlock is already
+ *   locked.
+ *
+ *   This implementation is the same as the above spin_trylock() except that
+ *   it does not perform instrumentation logic.
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to lock.
+ *
+ * Returned Value:
+ *   SP_LOCKED   - Failure, the spinlock was already locked
+ *   SP_UNLOCKED - Success, the spinlock was successfully locked
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+spinlock_t spin_trylock_wo_note(FAR volatile spinlock_t *lock)
+{
+  if (up_testset(lock) == SP_LOCKED)
+    {
+      SP_DSB();
+      return SP_LOCKED;
+    }
+
+  SP_DMB();
+  return SP_UNLOCKED;
+}
+
+/****************************************************************************
+ * Name: spin_unlock
+ *
+ * Description:
+ *   Release one count on a non-reentrant spinlock.
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to unlock.
+ *
+ * Returned Value:
+ *   None.
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+#ifdef __SP_UNLOCK_FUNCTION
+void spin_unlock(FAR volatile spinlock_t *lock)
+{
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  /* Notify that we are unlocking the spinlock */
+
+  sched_note_spinunlock(this_task(), lock);
+#endif
+
+  SP_DMB();
+  *lock = SP_UNLOCKED;
+  SP_DSB();
+  SP_SEV();
+}
+#endif
+
+/****************************************************************************
+ * Name: spin_unlock_wo_note
+ *
+ * Description:
+ *   Release one count on a non-reentrant spinlock.
+ *
+ *   This implementation is the same as the above spin_unlock() except that
+ *   it does not perform instrumentation logic.
+ *
+ * Input Parameters:
+ *   lock - A reference to the spinlock object to unlock.
+ *
+ * Returned Value:
+ *   None.
+ *
+ * Assumptions:
+ *   Not running at the interrupt level.
+ *
+ ****************************************************************************/
+
+void spin_unlock_wo_note(FAR volatile spinlock_t *lock)
+{
+  SP_DMB();
+  *lock = SP_UNLOCKED;
+  SP_DSB();
+  SP_SEV();
+}
+
+/****************************************************************************
+ * Name: spin_setbit
+ *
+ * Description:
+ *   Makes setting a CPU bit in a bitset an atomic action
+ *
+ * Input Parameters:
+ *   set     - A reference to the bitset to set the CPU bit in
+ *   cpu     - The bit number to be set
+ *   setlock - A reference to the lock protecting the set
+ *   orlock  - Will be set to SP_LOCKED while holding setlock
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+void spin_setbit(FAR volatile cpu_set_t *set, unsigned int cpu,
+                 FAR volatile spinlock_t *setlock,
+                 FAR volatile spinlock_t *orlock)
+{
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  cpu_set_t prev;
+#endif
+  irqstate_t flags;
+
+  /* Disable local interrupts to prevent being re-entered from an interrupt
+   * on the same CPU.  This may not effect interrupt behavior on other CPUs.
+   */
+
+  flags = up_irq_save();
+
+  /* Then, get the 'setlock' spinlock */
+
+  spin_lock(setlock);
+
+  /* Then set the bit and mark the 'orlock' as locked */
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  prev    = *set;
+#endif
+  *set   |= (1 << cpu);
+  *orlock = SP_LOCKED;
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  if (prev == 0)
+    {
+      /* Notify that we have locked the spinlock */
+
+      sched_note_spinlocked(this_task(), orlock);
+    }
+#endif
+
+  /* Release the 'setlock' and restore local interrupts */
+
+  spin_unlock(setlock);
+  up_irq_restore(flags);
+}
+#endif
+
+/****************************************************************************
+ * Name: spin_clrbit
+ *
+ * Description:
+ *   Makes clearing a CPU bit in a bitset an atomic action
+ *
+ * Input Parameters:
+ *   set     - A reference to the bitset to set the CPU bit in
+ *   cpu     - The bit number to be set
+ *   setlock - A reference to the lock protecting the set
+ *   orlock  - Will be set to SP_UNLOCKED if all bits become cleared in set
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+void spin_clrbit(FAR volatile cpu_set_t *set, unsigned int cpu,
+                 FAR volatile spinlock_t *setlock,
+                 FAR volatile spinlock_t *orlock)
+{
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  cpu_set_t prev;
+#endif
+  irqstate_t flags;
+
+  /* Disable local interrupts to prevent being re-entered from an interrupt
+   * on the same CPU.  This may not effect interrupt behavior on other CPUs.
+   */
+
+  flags = up_irq_save();
+
+  /* First, get the 'setlock' spinlock */
+
+  spin_lock(setlock);
+
+  /* Then clear the bit in the CPU set.  Set/clear the 'orlock' depending
+   * upon the resulting state of the CPU set.
+   */
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  prev    = *set;
+#endif
+  *set   &= ~(1 << cpu);
+  *orlock = (*set != 0) ? SP_LOCKED : SP_UNLOCKED;
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION_SPINLOCKS
+  if (prev != 0 && *set == 0)
+    {
+      /* Notify that we have unlocked the spinlock */
+
+      sched_note_spinunlock(this_task(), orlock);
+    }
+#endif
+
+  /* Release the 'setlock' and restore local interrupts */
+
+  spin_unlock(setlock);
+  up_irq_restore(flags);
+}
+#endif
+
+#endif /* CONFIG_SPINLOCK */