int sched_reprioritize(FAR struct tcb_s *tcb, int sched_priority)
{
/* This function is equivalent to sched_setpriority() BUT it also has the
* side effect of discarding all priority inheritance history. This is
* done only on explicit, user-initiated reprioritization.
*/
int ret = sched_setpriority(tcb, sched_priority);
if (ret == 0)
{
/* Reset the base_priority -- the priority that the thread would return
* to once it posts the semaphore.
*/
tcb->base_priority = (uint8_t)sched_priority;
/* Discard any pending reprioritizations as well */
#if CONFIG_SEM_NNESTPRIO > 0
tcb->npend_reprio = 0;
#endif
}
return ret;
}
static inline void pg_alldone(void)
{
FAR struct tcb_s *wtcb = (FAR struct tcb_s *)g_readytorun.head;
g_pftcb = NULL;
pgllvdbg("New worker priority. %d->%d\n",
wtcb->sched_priority, CONFIG_PAGING_DEFPRIO);
sched_setpriority(wtcb, CONFIG_PAGING_DEFPRIO);
}
void pg_miss(void)
{
FAR struct tcb_s *ftcb = (FAR struct tcb_s*)g_readytorun.head;
FAR struct tcb_s *wtcb;
/* Sanity checking
*
* ASSERT if the currently executing task is the page fill worker thread.
* The page fill worker thread is how the page fault is resolved and
* all logic associated with the page fill worker must be "locked" and
* always present in memory.
*/
pglldbg("Blocking TCB: %p PID: %d\n", ftcb, ftcb->pid);
DEBUGASSERT(g_pgworker != ftcb->pid);
/* Block the currently executing task
* - Call up_block_task() to block the task at the head of the ready-
* to-run list. This should cause an interrupt level context switch
* to the next highest priority task.
* - The blocked task will be marked with state TSTATE_WAIT_PAGEFILL
* and will be retained in the g_waitingforfill prioritized task list.
*/
up_block_task(ftcb, TSTATE_WAIT_PAGEFILL);
/* Boost the page fill worker thread priority.
* - Check the priority of the task at the head of the g_waitingforfill
* list. If the priority of that task is higher than the current
* priority of the page fill worker thread, then boost the priority
* of the page fill worker thread to that priority.
*/
wtcb = sched_gettcb(g_pgworker);
DEBUGASSERT(wtcb != NULL);
if (wtcb->sched_priority < ftcb->sched_priority)
{
/* Reprioritize the page fill worker thread */
pgllvdbg("New worker priority. %d->%d\n",
wtcb->sched_priority, ftcb->sched_priority);
sched_setpriority(wtcb, ftcb->sched_priority);
}
/* Signal the page fill worker thread.
* - Is there a page fill pending? If not then signal the worker
* thread to start working on the queued page fill requests.
*/
if (!g_pftcb)
{
pglldbg("Signaling worker. PID: %d\n", g_pgworker);
kill(g_pgworker, SIGWORK);
}
}
int sched_yield(void)
{
FAR struct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head;
/* This equivalent to just resetting the task priority to its current value
* since this will cause the task to be rescheduled behind any other tasks
* at the same priority.
*/
return sched_setpriority(rtcb, rtcb->sched_priority);
}
static void pg_callback(FAR struct tcb_s *tcb, int result)
{
/* Verify that g_pftcb is non-NULL */
pgllvdbg("g_pftcb: %p\n", g_pftcb);
if (g_pftcb)
{
FAR struct tcb_s *htcb = (FAR struct tcb_s *)g_waitingforfill.head;
FAR struct tcb_s *wtcb = sched_gettcb(g_pgworker);
/* Find the higher priority between the task waiting for the fill to
* complete in g_pftcb and the task waiting at the head of the
* g_waitingforfill list. That will be the priority of he highest
* priority task waiting for a fill.
*/
int priority = g_pftcb->sched_priority;
if (htcb && priority < htcb->sched_priority)
{
priority = htcb->sched_priority;
}
/* If this higher priority is higher than current page fill worker
* thread, then boost worker thread's priority to that level. Thus,
* the page fill worker thread will always run at the priority of
* the highest priority task that is waiting for a fill.
*/
if (priority > wtcb->sched_priority)
{
pgllvdbg("New worker priority. %d->%d\n",
wtcb->sched_priority, priority);
sched_setpriority(wtcb, priority);
}
/* Save the page fill result (don't permit the value -EBUSY) */
if (result == -EBUSY)
{
result = -ENOSYS;
}
g_fillresult = result;
}
/* Signal the page fill worker thread (in any event) */
pglldbg("Signaling worker. PID: %d\n", g_pgworker);
kill(g_pgworker, SIGWORK);
}
static inline bool pg_dequeue(void)
{
/* Loop until either (1) the TCB of a task that requires a fill is found, OR
* (2) the g_watingforfill list becomes empty.
*/
do
{
/* Remove the TCB from the head of the list (if any) */
g_pftcb = (FAR struct tcb_s *)dq_remfirst((dq_queue_t*)&g_waitingforfill);
pgllvdbg("g_pftcb: %p\n", g_pftcb);
if (g_pftcb != NULL)
{
/* Call the architecture-specific function up_checkmapping() to see if
* the page fill still needs to be performed. In certain conditions,
* the page fault may occur on several threads for the same page and
* be queues multiple times. In this corner case, the blocked task will
* simply be restarted.
*/
if (!up_checkmapping(g_pftcb))
{
/* This page needs to be filled. pg_miss bumps up
* the priority of the page fill worker thread as each
* TCB is added to the g_waitingforfill list. So we
* may need to also drop the priority of the worker
* thread as the next TCB comes off of the list.
*
* If wtcb->sched_priority > CONFIG_PAGING_DEFPRIO,
* then the page fill worker thread is executing at
* an elevated priority that may be reduced.
*
* If wtcb->sched_priority > g_pftcb->sched_priority
* then the page fill worker thread is executing at
* a higher priority than is appropriate for this
* fill (this priority can get re-boosted by pg_miss()
* if a new higher priority fill is required).
*/
FAR struct tcb_s *wtcb = (FAR struct tcb_s *)g_readytorun.head;
if (wtcb->sched_priority > CONFIG_PAGING_DEFPRIO &&
wtcb->sched_priority > g_pftcb->sched_priority)
{
/* Don't reduce the priority of the page fill
* worker thread lower than the configured
* minimum.
*/
int priority = g_pftcb->sched_priority;
if (priority < CONFIG_PAGING_DEFPRIO)
{
priority = CONFIG_PAGING_DEFPRIO;
}
/* Reduce the priority of the page fill worker thread */
pgllvdbg("New worker priority. %d->%d\n",
wtcb->sched_priority, priority);
sched_setpriority(wtcb, priority);
}
/* Return with g_pftcb holding the pointer to
* the TCB associated with task that requires the page fill.
*/
return true;
}
/* The page need by this task has already been mapped into the
* virtual address space -- just restart it.
*/
pglldbg("Restarting TCB: %p\n", g_pftcb);
up_unblock_task(g_pftcb);
}
}
while (g_pftcb != NULL);
return false;
}
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 hdoler 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))
{
sdbg("TCB 0x%08x is a stale handle, counts lost\n", 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 becuase 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
* reprioritize 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;
}
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))
{
sdbg("TCB 0x%08x is a stale handle, counts lost\n", 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
{
sdbg("CONFIG_SEM_NNESTPRIO exceeded\n");
}
}
/* 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.
*/
htcb->pend_reprios[htcb->npend_reprio] = rtcb->sched_priority;
htcb->npend_reprio++;
}
}
#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;
}
int sched_setaffinity(pid_t pid, size_t cpusetsize, FAR const cpu_set_t *mask)
{
FAR struct tcb_s *tcb;
irqstate_t flags;
int errcode = 0;
int ret;
DEBUGASSERT(cpusetsize == sizeof(cpu_set_t) && mask != NULL);
/* Verify that the PID corresponds to a real task */
sched_lock();
if (!pid)
{
tcb = this_task();
}
else
{
tcb = sched_gettcb(pid);
}
if (tcb == NULL)
{
errcode = ESRCH;
goto errout_with_lock;
}
/* Don't permit changing the affinity mask of any task locked to a CPU
* (i.e., an IDLE task)
*/
flags = enter_critical_section();
if ((tcb->flags & TCB_FLAG_CPU_LOCKED) != 0)
{
errcode = EINVAL;
goto errout_with_csection;
}
/* Set the new affinity mask. */
tcb->affinity = *mask;
/* Is the task still executing a a CPU in its affinity mask? Will this
* change cause the task to be removed from its current assigned task
* list?
*
* First... is the task in an assigned task list?
*/
if (tcb->task_state >= FIRST_ASSIGNED_STATE &&
tcb->task_state <= LAST_ASSIGNED_STATE)
{
/* Yes... is the CPU associated with the assigned task in the new
* affinity mask?
*/
if ((tcb->affinity & (1 << tcb->cpu)) == 0)
{
/* No.. then we will need to move the task from the the assigned
* task list to some other ready to run list.
*
* sched_setpriority() will do just what we want... it will remove
* the task from its current position in the some assigned task list
* and then simply put it back in the right place. This works even
* if the task is this task.
*/
ret = sched_setpriority(tcb, tcb->sched_priority);
if (ret < 0)
{
errcode = get_errno();
}
}
}
errout_with_csection:
leave_critical_section(flags);
errout_with_lock:
sched_unlock();
set_errno(errcode);
return ERROR;
}
