nuttx/sched/semaphore/sem_holder.c

/****************************************************************************
 * sched/semaphore/sem_holder.c
 *
 *   Copyright (C) 2009-2011, 2013, 2016 Gregory Nutt. All rights reserved.
 *   Author: Gregory Nutt <gnutt@nuttx.org>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name NuttX nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/config.h>

#include <semaphore.h>
#include <sched.h>
#include <assert.h>
#include <debug.h>
#include <nuttx/arch.h>

#include "sched/sched.h"
#include "semaphore/semaphore.h"

#ifdef CONFIG_PRIORITY_INHERITANCE

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

/* Configuration ************************************************************/

#ifndef CONFIG_SEM_PREALLOCHOLDERS
#  define CONFIG_SEM_PREALLOCHOLDERS 0
#endif

/****************************************************************************
 * Private Type Declarations
 ****************************************************************************/

typedef int (*holderhandler_t)(FAR struct semholder_s *pholder,
                               FAR sem_t *sem, FAR void *arg);

/****************************************************************************
 * Private Data
 ****************************************************************************/

/* Preallocated holder structures */

#if CONFIG_SEM_PREALLOCHOLDERS > 0
static struct semholder_s g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS];
static FAR struct semholder_s *g_freeholders;
#endif

/****************************************************************************
 * Name: sem_allocholder
 ****************************************************************************/

static inline FAR struct semholder_s *sem_allocholder(sem_t *sem)
{
  FAR struct semholder_s *pholder;

  /* Check if the "built-in" holder is being used.  We have this built-in
   * holder to optimize for the simplest case where semaphores are only
   * used to implement mutexes.
   */

#if CONFIG_SEM_PREALLOCHOLDERS > 0
  pholder = g_freeholders;
  if (pholder != NULL)
    {
      /* Remove the holder from the free list an put it into the semaphore's
       * holder list
       */

      g_freeholders    = pholder->flink;
      pholder->flink   = sem->hhead;
      sem->hhead       = pholder;

      /* Make sure the initial count is zero */

      pholder->counts  = 0;
    }
#else
  if (sem->holder[0].htcb == NULL)
    {
      pholder          = &sem->holder[0];
      pholder->counts  = 0;
    }
  else if (sem->holder[1].htcb == NULL)
    {
      pholder          = &sem->holder[1];
      pholder->counts  = 0;
    }
#endif
  else
    {
      serr("ERROR: Insufficient pre-allocated holders\n");
      pholder          = NULL;
    }

  DEBUGASSERT(pholder != NULL);
  return pholder;
}

/****************************************************************************
 * Name: sem_findholder
 ****************************************************************************/

static FAR struct semholder_s *sem_findholder(sem_t *sem,
                                              FAR struct tcb_s *htcb)
{
  FAR struct semholder_s *pholder;

#if CONFIG_SEM_PREALLOCHOLDERS > 0
  /* Try to find the holder in the list of holders associated with this
   * semaphore
   */

  for (pholder = sem->hhead; pholder != NULL; pholder = pholder->flink)
    {
      if (pholder->htcb == htcb)
        {
          /* Got it! */

          return pholder;
        }
    }
#else
  int i;
  pholder = NULL;

  /* We have two hard-allocated holder structuse in sem_t */

  for (i = 0; i < 2; i++)
    {
      pholder = &sem->holder[i];
      if (pholder->htcb == htcb)
        {
          /* Got it! */

          return pholder;
        }
    }
#endif

  /* The holder does not appear in the list */

  return NULL;
}

/****************************************************************************
 * Name: sem_findorallocateholder
 ****************************************************************************/

static inline FAR struct semholder_s *
sem_findorallocateholder(sem_t *sem, FAR struct tcb_s *htcb)
{
  FAR struct semholder_s *pholder = sem_findholder(sem, htcb);
  if (!pholder)
    {
      pholder = sem_allocholder(sem);
    }

  return pholder;
}

/****************************************************************************
 * Name: sem_freeholder
 ****************************************************************************/

static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder)
{
#if CONFIG_SEM_PREALLOCHOLDERS > 0
  FAR struct semholder_s *curr;
  FAR struct semholder_s *prev;
#endif

  /* Release the holder and counts */

  pholder->htcb   = NULL;
  pholder->counts = 0;

#if CONFIG_SEM_PREALLOCHOLDERS > 0
  /* Search the list for the matching holder */

  for (prev = NULL, curr = sem->hhead;
       curr && curr != pholder;
       prev = curr, curr = curr->flink);

  if (curr != NULL)
    {
      /* Remove the holder from the list */

      if (prev != NULL)
        {
          prev->flink = pholder->flink;
        }
      else
        {
          sem->hhead = pholder->flink;
        }

      /* And put it in the free list */

      pholder->flink = g_freeholders;
      g_freeholders  = pholder;
    }
#endif
}

/****************************************************************************
 * Name: sem_foreachholder
 ****************************************************************************/

static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler,
                             FAR void *arg)
{
  FAR struct semholder_s *pholder;
  int ret = 0;

#if CONFIG_SEM_PREALLOCHOLDERS > 0
  FAR struct semholder_s *next;

  for (pholder = sem->hhead; pholder && ret == 0; pholder = next)
    {
      /* In case this holder gets deleted */

      next = pholder->flink;

      /* Check if there is a handler... there should always be one
       * in this configuration.
       */

      if (pholder->htcb != NULL)
        {
          /* Call the handler */

          ret = handler(pholder, sem, arg);
        }
    }
#else
  int i;

  /* We have two hard-allocated holder structures in sem_t */

  for (i = 0; i < 2; i++)
    {
      pholder = &sem->holder[i];

      /* The hard-allocated containers may hold a NULL holder */

      if (pholder->htcb != NULL)
        {
          /* Call the handler */

          ret = handler(pholder, sem, arg);
        }
    }
#endif

  return ret;
}

/****************************************************************************
 * Name: sem_recoverholders
 ****************************************************************************/

#if CONFIG_SEM_PREALLOCHOLDERS > 0
static int sem_recoverholders(FAR struct semholder_s *pholder,
                              FAR sem_t *sem, FAR void *arg)
{
  sem_freeholder(sem, pholder);
  return 0;
}
#endif

/****************************************************************************
 * Name: sem_boostholderprio
 ****************************************************************************/

static int sem_boostholderprio(FAR struct semholder_s *pholder,
                               FAR sem_t *sem, FAR void *arg)
{
  FAR struct tcb_s *htcb = (FAR struct tcb_s *)pholder->htcb;
  FAR struct tcb_s *rtcb = (FAR struct tcb_s *)arg;

  /* Make sure that the holder thread is still active.  If it exited without
   * releasing its counts, then that would be a bad thing.  But we can take
   * no real action because we don't know know that the program is doing.
   * Perhaps its plan is to kill a thread, then destroy the semaphore.
   */

  if (!sched_verifytcb(htcb))
    {
      serr("ERROR: TCB 0x%08x is a stale handle, counts lost\n", htcb);
      DEBUGASSERT(!sched_verifytcb(htcb));
      sem_freeholder(sem, pholder);
    }

#if CONFIG_SEM_NNESTPRIO > 0
  /* If the priority of the thread that is waiting for a count is greater
   * than the base priority of the thread holding a count, then we may need
   * to adjust the holder's priority now or later to that priority.
   */

  else if (rtcb->sched_priority > htcb->base_priority)
    {
      /* If the new priority is greater than the current, possibly already
       * boosted priority of the holder thread, then we will have to raise
       * the holder's priority now.
       */

      if (rtcb->sched_priority > htcb->sched_priority)
        {
          /* If the current priority of holder thread has already been
           * boosted, then add the boost priority to the list of restoration
           * priorities.  When the higher priority waiter thread gets its
           * count, then we need to revert the holder thread to this saved
           * priority (not to its base priority).
           */

          if (htcb->sched_priority > htcb->base_priority)
            {
              /* Save the current, boosted priority of the holder thread. */

              if (htcb->npend_reprio < CONFIG_SEM_NNESTPRIO)
                {
                  htcb->pend_reprios[htcb->npend_reprio] = htcb->sched_priority;
                  htcb->npend_reprio++;
                }
              else
                {
                  serr("ERROR: CONFIG_SEM_NNESTPRIO exceeded\n");
                  DEBUGASSERT(htcb->npend_reprio < CONFIG_SEM_NNESTPRIO);
                }
            }

          /* Raise the priority of the thread holding of the semaphore.
           * This cannot cause a context switch because we have preemption
           * disabled.  The holder thread may be marked "pending" and the
           * switch may occur during up_block_task() processing.
           */

          (void)sched_setpriority(htcb, rtcb->sched_priority);
        }
      else
        {
          /* The new priority is above the base priority of the holder,
           * but not as high as its current working priority.  Just put it
           * in the list of pending restoration priorities so that when the
           * higher priority thread gets its count, we can revert to this
           * saved priority and not to the base priority.
           */

          if (htcb->npend_reprio < CONFIG_SEM_NNESTPRIO)
            {
              htcb->pend_reprios[htcb->npend_reprio] = rtcb->sched_priority;
              htcb->npend_reprio++;
            }
          else
            {
              serr("ERROR: CONFIG_SEM_NNESTPRIO exceeded\n");
              DEBUGASSERT(htcb->npend_reprio < CONFIG_SEM_NNESTPRIO);
            }
        }
    }

#else
  /* If the priority of the thread that is waiting for a count is less than
   * of equal to the priority of the thread holding a count, then do nothing
   * because the thread is already running at a sufficient priority.
   */

  else if (rtcb->sched_priority > htcb->sched_priority)
    {
      /* Raise the priority of the holder of the semaphore.  This
       * cannot cause a context switch because we have preemption
       * disabled.  The task will be marked "pending" and the switch
       * will occur during up_block_task() processing.
       */

      (void)sched_setpriority(htcb, rtcb->sched_priority);
    }
#endif

  return 0;
}

/****************************************************************************
 * Name: sem_verifyholder
 ****************************************************************************/

#ifdef CONFIG_DEBUG_ASSERTIONS
static int sem_verifyholder(FAR struct semholder_s *pholder, FAR sem_t *sem,
                            FAR void *arg)
{
#if 0 // Need to revisit this, but these assumptions seem to be untrue -- OR there is a bug???
  FAR struct tcb_s *htcb = (FAR struct tcb_s *)pholder->htcb;

  /* Called after a semaphore has been released (incremented), the semaphore
   * could is non-negative, and there is no thread waiting for the count.
   * In this case, the priority of the holder should not be boosted.
   */

#if CONFIG_SEM_NNESTPRIO > 0
  DEBUGASSERT(htcb->npend_reprio == 0);
#endif
  DEBUGASSERT(htcb->sched_priority == htcb->base_priority);
#endif
  return 0;
}
#endif

/****************************************************************************
 * Name: sem_dumpholder
 ****************************************************************************/

#if defined(CONFIG_DEBUG_INFO) && defined(CONFIG_SEM_PHDEBUG)
static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem,
                          FAR void *arg)
{
#if CONFIG_SEM_PREALLOCHOLDERS > 0
  _info("  %08x: %08x %08x %04x\n",
      pholder, pholder->flink, pholder->htcb, pholder->counts);
#else
  _info("  %08x: %08x %04x\n", pholder, pholder->htcb, pholder->counts);
#endif
  return 0;
}
#endif

/****************************************************************************
 * Name: sem_restoreholderprio
 ****************************************************************************/

static int sem_restoreholderprio(FAR struct semholder_s *pholder,
                                 FAR sem_t *sem, FAR void *arg)
{
  FAR struct tcb_s *htcb = (FAR struct tcb_s *)pholder->htcb;
#if CONFIG_SEM_NNESTPRIO > 0
  FAR struct tcb_s *stcb = (FAR struct tcb_s *)arg;
  int rpriority;
  int i;
  int j;
#endif

  /* Make sure that the holder thread is still active.  If it exited without
   * releasing its counts, then that would be a bad thing.  But we can take
   * no real action because we don't know know that the program is doing.
   * Perhaps its plan is to kill a thread, then destroy the semaphore.
   */

  if (!sched_verifytcb(htcb))
    {
      serr("ERROR: TCB 0x%08x is a stale handle, counts lost\n", htcb);
      DEBUGASSERT(!sched_verifytcb(htcb));
      sem_freeholder(sem, pholder);
    }

  /* Was the priority of the holder thread boosted? If so, then drop its
   * priority back to the correct level.  What is the correct level?
   */

  else if (htcb->sched_priority != htcb->base_priority)
    {
#if CONFIG_SEM_NNESTPRIO > 0
      /* Are there other, pending priority levels to revert to? */

      if (htcb->npend_reprio < 1)
        {
          /* No... the holder thread has only been boosted once.
           * npend_reprio should be 0 and the boosted priority should be the
           * priority of the task that just got the semaphore
           * (stcb->sched_priority)
           *
           * That latter assumption may not be true if the stcb's priority
           * was also boosted so that it no longer matches the htcb's
           * sched_priority.  Or if CONFIG_SEM_NNESTPRIO is too small (so
           * that we do not have a proper record of the reprioritizations).
           */

          DEBUGASSERT(/* htcb->sched_priority == stcb->sched_priority && */
                      htcb->npend_reprio == 0);

          /* Reset the holder's priority back to the base priority. */

          sched_reprioritize(htcb, htcb->base_priority);
        }

      /* There are multiple pending priority levels. The holder thread's
       * "boosted" priority could greater than or equal to
       * "stcb->sched_priority" (it could be greater if its priority we
       * boosted because it also holds another semaphore).
       */

      else if (htcb->sched_priority <= stcb->sched_priority)
        {
          /* The holder thread has been boosted to the same priority as the
           * waiter thread that just received the count.  We will simply
           * reprioritize to the next highest priority that we have in
           * rpriority.
           */

          /* Find the highest pending priority and remove it from the list */

          for (i = 1, j = 0; i < htcb->npend_reprio; i++)
            {
              if (htcb->pend_reprios[i] > htcb->pend_reprios[j])
                {
                  j = i;
                }
            }

          /* Remove the highest priority pending priority from the list */

          rpriority = htcb->pend_reprios[j];
          i = htcb->npend_reprio - 1;
          if (i > 0)
            {
              htcb->pend_reprios[j] = htcb->pend_reprios[i];
            }

          htcb->npend_reprio = i;

          /* And apply that priority to the thread (while retaining the
           * base_priority)
           */

          sched_setpriority(htcb, rpriority);
        }
      else
        {
          /* The holder thread has been boosted to a higher priority than the
           * waiter task.  The pending priority should be in the list (unless
           * it was lost because of of list overflow or because the holder
           * was reprioritized again unbeknownst to the priority inheritance
           * logic).
           *
           * Search the list for the matching priority.
           */

          for (i = 0; i < htcb->npend_reprio; i++)
            {
              /* Does this pending priority match the priority of the thread
               * that just received the count?
               */

              if (htcb->pend_reprios[i] == stcb->sched_priority)
                {
                  /* Yes, remove it from the list */

                  j = htcb->npend_reprio - 1;
                  if (j > 0)
                    {
                      htcb->pend_reprios[i] = htcb->pend_reprios[j];
                    }

                   htcb->npend_reprio = j;
                   break;
                }
            }
        }
#else
      /* There is no alternative restore priorities, drop the priority
       * of the holder thread all the way back to the threads "base"
       * priority.
       */

      sched_reprioritize(htcb, htcb->base_priority);
#endif
    }

  return 0;
}

/****************************************************************************
 * Name: sem_restoreholderprioA
 *
 * Description:
 *   Reprioritize all holders except the currently executing task
 *
 ****************************************************************************/

static int sem_restoreholderprioA(FAR struct semholder_s *pholder,
                                  FAR sem_t *sem, FAR void *arg)
{
  FAR struct tcb_s *rtcb = this_task();
  if (pholder->htcb != rtcb)
    {
      return sem_restoreholderprio(pholder, sem, arg);
    }

  return 0;
}

/****************************************************************************
 * Name: sem_restoreholderprioB
 *
 * Description:
 *   Reprioritize only the currently executing task
 *
 ****************************************************************************/

static int sem_restoreholderprioB(FAR struct semholder_s *pholder,
                                  FAR sem_t *sem, FAR void *arg)
{
  FAR struct tcb_s *rtcb = this_task();
  if (pholder->htcb == rtcb)
    {
      (void)sem_restoreholderprio(pholder, sem, arg);
      return 1;
    }

  return 0;
}

/****************************************************************************
 * Name: sem_restorebaseprio_irq
 *
 * Description:
 *   This function is called after the an interrupt handler posts a count on
 *   the semaphore.  It will check if we need to drop the priority of any
 *   threads holding a count on the semaphore.  Their priority could have
 *   been boosted while they held the count.
 *
 * Parameters:
 *   stcb - The TCB of the task that was just started (if any).  If the
 *     post action caused a count to be given to another thread, then stcb
 *     is the TCB that received the count.  Note, just because stcb received
 *     the count, it does not mean that it it is higher priority than other
 *     threads.
 *   sem - A reference to the semaphore being posted.
 *     - If the semaphore count is <0 then there are still threads waiting
 *       for a count.  stcb should be non-null and will be higher priority
 *       than all of the other threads still waiting.
 *     - If it is ==0 then stcb refers to the thread that got the last count;
 *       no other threads are waiting.
 *     - If it is >0 then there should be no threads waiting for counts and
 *       stcb should be null.
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *   The scheduler is locked.
 *
 ****************************************************************************/

static inline void sem_restorebaseprio_irq(FAR struct tcb_s *stcb,
                                           FAR sem_t *sem)
{
  /* Perform the following actions only if a new thread was given a count.
   * The thread that received the count should be the highest priority
   * of all threads waiting for a count from the semaphore.  So in that
   * case, the priority of all holder threads should be dropped to the
   * next highest pending priority.
   */

  if (stcb != NULL)
    {
      /* Drop the priority of all holder threads */

      (void)sem_foreachholder(sem, sem_restoreholderprio, stcb);
    }

  /* If there are no tasks waiting for available counts, then all holders
   * should be at their base priority.
   */

#ifdef CONFIG_DEBUG_ASSERTIONS
  else
    {
      (void)sem_foreachholder(sem, sem_verifyholder, NULL);
    }
#endif
}

/****************************************************************************
 * Name: sem_restorebaseprio_task
 *
 * Description:
 *   This function is called after the current running task releases a
 *   count on the semaphore.  It will check if we need to drop the priority
 *   of any threads holding a count on the semaphore.  Their priority could
 *   have been boosted while they held the count.
 *
 * Parameters:
 *   stcb - The TCB of the task that was just started (if any).  If the
 *     post action caused a count to be given to another thread, then stcb
 *     is the TCB that received the count.  Note, just because stcb received
 *     the count, it does not mean that it it is higher priority than other
 *     threads.
 *   sem - A reference to the semaphore being posted.
 *     - If the semaphore count is <0 then there are still threads waiting
 *       for a count.  stcb should be non-null and will be higher priority
 *       than all of the other threads still waiting.
 *     - If it is ==0 then stcb refers to the thread that got the last count;
 *       no other threads are waiting.
 *     - If it is >0 then there should be no threads waiting for counts and
 *       stcb should be null.
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *   The scheduler is locked.
 *
 ****************************************************************************/

static inline void sem_restorebaseprio_task(FAR struct tcb_s *stcb,
                                            FAR sem_t *sem)
{
  FAR struct tcb_s *rtcb = this_task();
  FAR struct semholder_s *pholder;

  /* Perform the following actions only if a new thread was given a count.
   * The thread that received the count should be the highest priority
   * of all threads waiting for a count from the semaphore.  So in that
   * case, the priority of all holder threads should be dropped to the
   * next highest pending priority.
   */

  if (stcb != NULL)
    {
      /* The currently executed thread should be the lower priority
       * thread that just posted the count and caused this action.
       * However, we cannot drop the priority of the currently running
       * thread -- because that will cause it to be suspended.
       *
       * So, do this in two passes.  First, reprioritizing all holders
       * except for the running thread.
       */

      (void)sem_foreachholder(sem, sem_restoreholderprioA, stcb);

      /* Now, find an reprioritize only the ready to run task */

      (void)sem_foreachholder(sem, sem_restoreholderprioB, stcb);
    }

  /* If there are no tasks waiting for available counts, then all holders
   * should be at their base priority.
   */

#ifdef CONFIG_DEBUG_ASSERTIONS
  else
    {
      (void)sem_foreachholder(sem, sem_verifyholder, NULL);
    }
#endif

  /* In any case, the currently executing task should have an entry in the
   * list.  Its counts were previously decremented; if it now holds no
   * counts, then we need to remove it from the list of holders.
   */

  pholder = sem_findholder(sem, rtcb);
  if (pholder != NULL)
    {
      /* When no more counts are held, remove the holder from the list.  The
       * count was decremented in sem_releaseholder.
       */

      if (pholder->counts <= 0)
        {
          sem_freeholder(sem, pholder);
        }
    }
}

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: sem_initholders
 *
 * Description:
 *   Called from sem_initialize() to set up semaphore holder information.
 *
 * Parameters:
 *   None
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *
 ****************************************************************************/

void sem_initholders(void)
{
#if CONFIG_SEM_PREALLOCHOLDERS > 0
  int i;

  /* Put all of the pre-allocated holder structures into the free list */

  g_freeholders = g_holderalloc;
  for (i = 0; i < (CONFIG_SEM_PREALLOCHOLDERS-1); i++)
    {
      g_holderalloc[i].flink = &g_holderalloc[i+1];
    }

  g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS-1].flink = NULL;
#endif
}

/****************************************************************************
 * Name: sem_destroyholder
 *
 * Description:
 *   Called from sem_destroyholder() to handle any holders of a semaphore when
 *   it is destroyed.
 *
 * Parameters:
 *   sem - A reference to the semaphore being destroyed
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *
 ****************************************************************************/

void sem_destroyholder(FAR sem_t *sem)
{
  /* It is an error if a semaphore is destroyed while there are any holders
   * (except perhaps the thread release the semaphore itself).  Hmmm.. but
   * we actually have to assume that the caller knows what it is doing because
   * could have killed another thread that is the actual holder of the semaphore.
   * We cannot make any assumptions about the state of the semaphore or the
   * state of any of the holder threads.
   *
   * So just recover any stranded holders and hope the task knows what it is
   * doing.
   */

#if CONFIG_SEM_PREALLOCHOLDERS > 0
  if (sem->hhead != NULL)
    {
      serr("ERROR: Semaphore destroyed with holders\n");
      DEBUGASSERT(sem->hhead != NULL);
      (void)sem_foreachholder(sem, sem_recoverholders, NULL);
    }
#else
  if (sem->holder[0].htcb != NULL || sem->holder[0].htcb != NULL)
    {
      DEBUGASSERT(sem->holder[0].htcb != NULL || sem->holder[0].htcb != NULL);
      serr("ERROR: Semaphore destroyed with holder\n");
    }

  sem->holder[0].htcb = NULL;
  sem->holder[1].htcb = NULL;
#endif
}

/****************************************************************************
 * Name: sem_addholder_tcb
 *
 * Description:
 *   Called from sem_wait() when the calling thread obtains the semaphore;
 *   Called from sem_post() when the waiting thread obtains the semaphore.
 *
 * Parameters:
 *   htcb - TCB of the thread that just obtained the semaphore
 *   sem  - A reference to the incremented semaphore
 *
 * Return Value:
 *   0 (OK) or -1 (ERROR) if unsuccessful
 *
 * Assumptions:
 *   Interrupts are disabled.
 *
 ****************************************************************************/

void sem_addholder_tcb(FAR struct tcb_s *htcb, FAR sem_t *sem)
{
  FAR struct semholder_s *pholder;

  /* If priority inheritance is disabled for this thread, then do not add
   * the holder.  If there are never holders of the semaphore, the priority
   * inheritance is effectively disabled.
   */

  if ((sem->flags & PRIOINHERIT_FLAGS_DISABLE) == 0)
    {
      /* Find or allocate a container for this new holder */

      pholder = sem_findorallocateholder(sem, htcb);
      if (pholder != NULL)
        {
          /* Then set the holder and increment the number of counts held by this
           * holder
           */

          pholder->htcb = htcb;
          pholder->counts++;
        }
    }
}

/****************************************************************************
 * Name: sem_addholder
 *
 * Description:
 *   Called from sem_wait() when the calling thread obtains the semaphore
 *
 * Parameters:
 *   sem - A reference to the incremented semaphore
 *
 * Return Value:
 *   0 (OK) or -1 (ERROR) if unsuccessful
 *
 * Assumptions:
 *   Interrupts are disabled.
 *
 ****************************************************************************/

void sem_addholder(FAR sem_t *sem)
{
  sem_addholder_tcb(this_task(), sem);
}

/****************************************************************************
 * Name: void sem_boostpriority(sem_t *sem)
 *
 * Description:
 *
 *
 * Parameters:
 *   None
 *
 * Return Value:
 *   0 (OK) or -1 (ERROR) if unsuccessful
 *
 * Assumptions:
 *
 ****************************************************************************/

void sem_boostpriority(FAR sem_t *sem)
{
  FAR struct tcb_s *rtcb = this_task();

  /* Boost the priority of every thread holding counts on this semaphore
   * that are lower in priority than the new thread that is waiting for a
   * count.
   */

   (void)sem_foreachholder(sem, sem_boostholderprio, rtcb);
}

/****************************************************************************
 * Name: sem_releaseholder
 *
 * Description:
 *   Called from sem_post() after a thread releases one count on the
 *   semaphore.
 *
 * Parameters:
 *   sem - A reference to the semaphore being posted
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *
 ****************************************************************************/

void sem_releaseholder(FAR sem_t *sem)
{
  FAR struct tcb_s *rtcb = this_task();
  FAR struct semholder_s *pholder;

  /* Find the container for this holder */

  pholder = sem_findholder(sem, rtcb);
  if (pholder != NULL && pholder->counts > 0)
    {
      /* Decrement the counts on this holder -- the holder will be freed
       * later in sem_restorebaseprio.
       */

      pholder->counts--;
    }
}

/****************************************************************************
 * Name: sem_restorebaseprio
 *
 * Description:
 *   This function is called after the current running task releases a
 *   count on the semaphore or an interrupt handler posts a new count.  It
 *   will check if we need to drop the priority of any threads holding a
 *   count on the semaphore.  Their priority could have been boosted while
 *   they held the count.
 *
 * Parameters:
 *   stcb - The TCB of the task that was just started (if any).  If the
 *     post action caused a count to be given to another thread, then stcb
 *     is the TCB that received the count.  Note, just because stcb received
 *     the count, it does not mean that it it is higher priority than other
 *     threads.
 *   sem - A reference to the semaphore being posted.
 *     - If the semaphore count is <0 then there are still threads waiting
 *       for a count.  stcb should be non-null and will be higher priority
 *       than all of the other threads still waiting.
 *     - If it is ==0 then stcb refers to the thread that got the last count;
 *       no other threads are waiting.
 *     - If it is >0 then there should be no threads waiting for counts and
 *       stcb should be null.
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *   The scheduler is locked.
 *
 ****************************************************************************/

void sem_restorebaseprio(FAR struct tcb_s *stcb, FAR sem_t *sem)
{
  /* Check our assumptions */

  DEBUGASSERT((sem->semcount > 0  && stcb == NULL) ||
              (sem->semcount <= 0 && stcb != NULL));

  /* Handler semaphore counts posed from an interrupt handler differently
   * from interrupts posted from threads.  The primary difference is that
   * if the semaphore is posted from a thread, then the poster thread is
   * a player in the priority inheritance scheme.  The interrupt handler
   * externally injects the new count without otherwise participating
   * itself.
   */

  if (up_interrupt_context())
    {
      sem_restorebaseprio_irq(stcb, sem);
    }
  else
    {
      sem_restorebaseprio_task(stcb, sem);
    }
}

/****************************************************************************
 * Name: sem_canceled
 *
 * Description:
 *   Called from sem_waitirq() after a thread that was waiting for a semaphore
 *   count was awakened because of a signal and the semaphore wait has been
 *   cancelled.  This function restores the correct thread priority of each
 *   holder of the semaphore.
 *
 * Parameters:
 *   sem - A reference to the semaphore no longer being waited for
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *
 ****************************************************************************/

#ifndef CONFIG_DISABLE_SIGNALS
void sem_canceled(FAR struct tcb_s *stcb, FAR sem_t *sem)
{
  /* Check our assumptions */

  DEBUGASSERT(sem->semcount <= 0);

  /* Adjust the priority of every holder as necessary */

  (void)sem_foreachholder(sem, sem_restoreholderprio, stcb);
}
#endif

/****************************************************************************
 * Name: sem_enumholders
 *
 * Description:
 *   Show information about threads currently waiting on this semaphore
 *
 * Parameters:
 *   sem - A reference to the semaphore
 *
 * Return Value:
 *   None
 *
 * Assumptions:
 *
 ****************************************************************************/

#if defined(CONFIG_DEBUG_FEATURES) && defined(CONFIG_SEM_PHDEBUG)
void sem_enumholders(FAR sem_t *sem)
{
#ifdef CONFIG_DEBUG_INFO
  (void)sem_foreachholder(sem, sem_dumpholder, NULL);
#endif
}
#endif

/****************************************************************************
 * Name: sem_nfreeholders
 *
 * Description:
 *   Return the number of available holder containers.  This is a good way
 *   to find out which threads are not calling sem_destroy.
 *
 * Parameters:
 *   sem - A reference to the semaphore
 *
 * Return Value:
 *   The number of available holder containers
 *
 * Assumptions:
 *
 ****************************************************************************/

#if defined(CONFIG_DEBUG_FEATURES) && defined(CONFIG_SEM_PHDEBUG)
int sem_nfreeholders(void)
{
#if CONFIG_SEM_PREALLOCHOLDERS > 0
  FAR struct semholder_s *pholder;
  int n;

  for (pholder = g_freeholders, n = 0; pholder; pholder = pholder->flink) n++;
  return n;
#else
  return 0;
#endif
}
#endif

#endif /* CONFIG_PRIORITY_INHERITANCE */