From b9648c396b1fae393e6a20c7ddd3debee89b5cb9 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 10 Mar 2009 23:52:46 +0000 Subject: Repartition functionality in preparation for nested, pending reprioritization git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1591 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/common/up_reprioritizertr.c | 3 - nuttx/arch/pjrc-8051/src/up_reprioritizertr.c | 3 - nuttx/arch/sh/src/common/up_reprioritizertr.c | 3 - nuttx/arch/sim/src/up_reprioritizertr.c | 3 - nuttx/arch/z16/src/common/up_reprioritizertr.c | 3 - nuttx/arch/z80/src/common/up_reprioritizertr.c | 3 - nuttx/examples/ostest/prioinherit.c | 10 +- nuttx/include/nuttx/sched.h | 4 + nuttx/sched/Makefile | 5 +- nuttx/sched/os_internal.h | 7 +- nuttx/sched/sched_reprioritize.c | 128 +++++++++++ nuttx/sched/sched_setparam.c | 2 +- nuttx/sched/sched_setpriority.c | 230 +++++++++++++++++++ nuttx/sched/sched_setscheduler.c | 2 +- nuttx/sched/sched_settcbprio.c | 294 ------------------------- nuttx/sched/sched_yield.c | 2 +- nuttx/sched/sem_holder.c | 20 +- nuttx/tools/mkconfig.c | 15 +- 18 files changed, 407 insertions(+), 330 deletions(-) create mode 100644 nuttx/sched/sched_reprioritize.c create mode 100644 nuttx/sched/sched_setpriority.c delete mode 100644 nuttx/sched/sched_settcbprio.c diff --git a/nuttx/arch/arm/src/common/up_reprioritizertr.c b/nuttx/arch/arm/src/common/up_reprioritizertr.c index 2fb7eb631..41aa0d5e2 100644 --- a/nuttx/arch/arm/src/common/up_reprioritizertr.c +++ b/nuttx/arch/arm/src/common/up_reprioritizertr.c @@ -109,9 +109,6 @@ void up_reprioritize_rtr(_TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c index eef37c16f..7e9ddd9fd 100644 --- a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c +++ b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c @@ -109,9 +109,6 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/arch/sh/src/common/up_reprioritizertr.c b/nuttx/arch/sh/src/common/up_reprioritizertr.c index 230dbfff7..be465fd0a 100644 --- a/nuttx/arch/sh/src/common/up_reprioritizertr.c +++ b/nuttx/arch/sh/src/common/up_reprioritizertr.c @@ -109,9 +109,6 @@ void up_reprioritize_rtr(_TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/arch/sim/src/up_reprioritizertr.c b/nuttx/arch/sim/src/up_reprioritizertr.c index 307369bf3..8d6f436e6 100644 --- a/nuttx/arch/sim/src/up_reprioritizertr.c +++ b/nuttx/arch/sim/src/up_reprioritizertr.c @@ -109,9 +109,6 @@ void up_reprioritize_rtr(_TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/arch/z16/src/common/up_reprioritizertr.c b/nuttx/arch/z16/src/common/up_reprioritizertr.c index f94d30ad0..f8fff390b 100644 --- a/nuttx/arch/z16/src/common/up_reprioritizertr.c +++ b/nuttx/arch/z16/src/common/up_reprioritizertr.c @@ -113,9 +113,6 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/arch/z80/src/common/up_reprioritizertr.c b/nuttx/arch/z80/src/common/up_reprioritizertr.c index 0b71e510f..31fab37de 100644 --- a/nuttx/arch/z80/src/common/up_reprioritizertr.c +++ b/nuttx/arch/z80/src/common/up_reprioritizertr.c @@ -114,9 +114,6 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority) /* Setup up the new task priority */ tcb->sched_priority = (ubyte)priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)priority; -#endif /* Return the task to the specified blocked task list. * sched_addreadytorun will return TRUE if the task was diff --git a/nuttx/examples/ostest/prioinherit.c b/nuttx/examples/ostest/prioinherit.c index e4b2fa7dd..7754ca587 100644 --- a/nuttx/examples/ostest/prioinherit.c +++ b/nuttx/examples/ostest/prioinherit.c @@ -237,7 +237,10 @@ static void *lowpri_thread(void *parameter) } else { - printf("lowpri_thread: Priority before sem_post: %d\n", sparam.sched_priority); + printf("lowpri_thread: %s priority before sem_post: %d\n", + sparam.sched_priority != g_highpri ? "ERROR" : "SUCCESS", + sparam.sched_priority); + if (sparam.sched_priority != g_highpri) { printf(" ERROR should have been %d\n", g_highpri); @@ -252,7 +255,10 @@ static void *lowpri_thread(void *parameter) } else { - printf("lowpri_thread: Final priority: %d\n", sparam.sched_priority); + printf("lowpri_thread: %s final priority: %d\n", + sparam.sched_priority != g_lowpri ? "ERROR" : "SUCCESS", + sparam.sched_priority); + if (sparam.sched_priority != g_lowpri) { printf(" ERROR should have been %d\n", g_lowpri); diff --git a/nuttx/include/nuttx/sched.h b/nuttx/include/nuttx/sched.h index 500ae0c97..74b420bfc 100644 --- a/nuttx/include/nuttx/sched.h +++ b/nuttx/include/nuttx/sched.h @@ -178,6 +178,10 @@ struct _TCB exitfunc_t exitfunc; /* Called if exit is called. */ ubyte sched_priority; /* Current priority of the thread */ #ifdef CONFIG_PRIORITY_INHERITANCE +# if CONFIG_SEM_NNESTPRIO > 0 + ubyte npend_reprio; /* Number of nested reprioritizations */ + ubyte pend_reprios[CONFIG_SEM_NNESTPRIO]; +# endif ubyte base_priority; /* "Normal" priority of the thread */ #endif ubyte task_state; /* Current state of the thread */ diff --git a/nuttx/sched/Makefile b/nuttx/sched/Makefile index b846163d8..51554323d 100644 --- a/nuttx/sched/Makefile +++ b/nuttx/sched/Makefile @@ -50,11 +50,14 @@ TSK_SRCS = task_create.c task_init.c task_setup.c task_activate.c \ sched_mergepending.c sched_addblocked.c sched_removeblocked.c \ sched_free.c sched_gettcb.c sched_releasetcb.c -SCHED_SRCS = sched_setparam.c sched_settcbprio.c sched_getparam.c \ +SCHED_SRCS = sched_setparam.c sched_setpriority.c sched_getparam.c \ sched_setscheduler.c sched_getscheduler.c \ sched_yield.c sched_rrgetinterval.c sched_foreach.c \ sched_getprioritymax.c sched_getprioritymin.c \ sched_lock.c sched_unlock.c sched_lockcount.c +ifeq ($(CONFIG_PRIORITY_INHERITANCE),y) +SEM_SRCS += sched_reprioritize.c +endif ENV_SRCS = env_getenvironptr.c env_dup.c env_share.c env_release.c \ env_findvar.c env_removevar.c \ diff --git a/nuttx/sched/os_internal.h b/nuttx/sched/os_internal.h index e6e1849f4..56a4e8ec6 100644 --- a/nuttx/sched/os_internal.h +++ b/nuttx/sched/os_internal.h @@ -252,7 +252,12 @@ extern boolean sched_addprioritized(FAR _TCB *newTcb, extern boolean sched_mergepending(void); extern void sched_addblocked(FAR _TCB *btcb, tstate_t task_state); extern void sched_removeblocked(FAR _TCB *btcb); -extern int sched_settcbprio(FAR _TCB *tcb, int sched_priority); +extern int sched_setpriority(FAR _TCB *tcb, int sched_priority); +#ifdef CONFIG_PRIORITY_INHERITANCE +extern int sched_reprioritize(FAR _TCB *tcb, int sched_priority); +#else +# define sched_reprioritize(tcb,sched_priority) sched_setpriority(tcb,sched_priority) +#endif extern FAR _TCB *sched_gettcb(pid_t pid); #if CONFIG_NFILE_DESCRIPTORS > 0 || CONFIG_NSOCKET_DESCRIPTORS > 0 diff --git a/nuttx/sched/sched_reprioritize.c b/nuttx/sched/sched_reprioritize.c new file mode 100644 index 000000000..c9e165104 --- /dev/null +++ b/nuttx/sched/sched_reprioritize.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * sched/sched_reprioritize.c + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 +#include +#include +#include + +#include "os_internal.h" + +#ifdef CONFIG_PRIORITY_INHERITANCE + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Global Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: sched_reprioritize + * + * Description: + * This function sets the priority of a specified task. + * + * NOTE: Setting a task's priority to the same value has a similar + * effect to sched_yield() -- The task will be moved to after all other + * tasks with the same priority. + * + * Inputs: + * tcb - the TCB of task to reprioritize. + * sched_priority - The new task priority + * + * Return Value: + * On success, sched_setparam() returns 0 (OK). On error, -1 + * (ERROR) is returned, and errno is set appropriately. + * + * EINVAL The parameter 'param' is invalid or does not make + * sense for the current scheduling policy. + * EPERM The calling task does not have appropriate privileges. + * ESRCH The task whose ID is pid could not be found. + * + * Assumptions: + * + ****************************************************************************/ + +int sched_reprioritize(FAR _TCB *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 = (ubyte)sched_priority; + + /* Discard any pending reprioritizations as well */ + +# if CONFIG_SEM_NNESTPRIO > 0 + tcb->npend_reprio = 0; +# endif + } + return ret; +} +#endif /* CONFIG_PRIORITY_INHERITANCE */ diff --git a/nuttx/sched/sched_setparam.c b/nuttx/sched/sched_setparam.c index b08611169..2b0f804a6 100644 --- a/nuttx/sched/sched_setparam.c +++ b/nuttx/sched/sched_setparam.c @@ -147,7 +147,7 @@ int sched_setparam(pid_t pid, const struct sched_param *param) /* Then perform the reprioritization */ - ret = sched_settcbprio(tcb, param->sched_priority); + ret = sched_reprioritize(tcb, param->sched_priority); sched_unlock(); return ret; } diff --git a/nuttx/sched/sched_setpriority.c b/nuttx/sched/sched_setpriority.c new file mode 100644 index 000000000..a071a7259 --- /dev/null +++ b/nuttx/sched/sched_setpriority.c @@ -0,0 +1,230 @@ +/**************************************************************************** + * sched/sched_setpriority.c + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 +#include +#include +#include +#include +#include "os_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Global Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: sched_setpriority + * + * Description: + * This function sets the priority of a specified task. + * + * NOTE: Setting a task's priority to the same value has a similar + * effect to sched_yield() -- The task will be moved to after all other + * tasks with the same priority. + * + * Inputs: + * tcb - the TCB of task to reprioritize. + * sched_priority - The new task priority + * + * Return Value: + * On success, sched_setparam() returns 0 (OK). On error, -1 + * (ERROR) is returned, and errno is set appropriately. + * + * EINVAL The parameter 'param' is invalid or does not make + * sense for the current scheduling policy. + * EPERM The calling task does not have appropriate privileges. + * ESRCH The task whose ID is pid could not be found. + * + * Assumptions: + * + ****************************************************************************/ + +int sched_setpriority(FAR _TCB *tcb, int sched_priority) +{ + FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; + tstate_t task_state; + irqstate_t saved_state; + + /* Verify that the requested priority is in the valid range */ + + if (sched_priority < SCHED_PRIORITY_MIN || + sched_priority > SCHED_PRIORITY_MAX) + { + errno = EINVAL; + return ERROR; + } + + /* We need to assure that there there is no interrupt activity while + * performing the following. + */ + + saved_state = irqsave(); + + /* There are four cases that must be considered: */ + + task_state = tcb->task_state; + switch (task_state) + { + /* CASE 1. The task is running or ready-to-run and a context switch + * may be caused by the re-prioritization + */ + + case TSTATE_TASK_RUNNING: + + /* A context switch will occur if the new priority of the running + * task becomes less than OR EQUAL TO the next highest priority + * ready to run task. + */ + + if (sched_priority <= tcb->flink->sched_priority) + { + /* A context switch will occur. */ + + up_reprioritize_rtr(tcb, (ubyte)sched_priority); + } + + /* Otherwise, we can just change priority since it has no effect */ + + else + { + /* Change the task priority */ + + tcb->sched_priority = (ubyte)sched_priority; + } + break; + + /* CASE 2. The task is running or ready-to-run and a context switch + * may be caused by the re-prioritization + */ + + case TSTATE_TASK_READYTORUN: + + /* A context switch will occur if the new priority of the ready-to + * run task is (strictly) greater than the current running task + */ + + if (sched_priority > rtcb->sched_priority) + { + /* A context switch will occur. */ + + up_reprioritize_rtr(tcb, (ubyte)sched_priority); + } + + /* Otherwise, we can just change priority and re-schedule (since it + * have no other effect). + */ + + else + { + /* Remove the TCB from the ready-to-run task list */ + + ASSERT(!sched_removereadytorun(tcb)); + + /* Change the task priority */ + + tcb->sched_priority = (ubyte)sched_priority; + + /* Put it back into the ready-to-run task list */ + + ASSERT(!sched_addreadytorun(tcb)); + } + break; + + /* CASE 3. The task is not in the ready to run list. Changing its + * Priority cannot effect the currently executing task. + */ + + default: + /* CASE 3a. The task resides in a prioritized list. */ + + if (g_tasklisttable[task_state].prioritized) + { + /* Remove the TCB from the prioritized task list */ + + dq_rem((FAR dq_entry_t*)tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list); + + /* Change the task priority */ + + tcb->sched_priority = (ubyte)sched_priority; + + /* Put it back into the prioritized list at the correct + * position + */ + + sched_addprioritized(tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list); + } + + /* CASE 3b. The task resides in a non-prioritized list. */ + + else + { + /* Just change the task's priority */ + + tcb->sched_priority = (ubyte)sched_priority; + } + break; + } + + irqrestore(saved_state); + return OK; +} diff --git a/nuttx/sched/sched_setscheduler.c b/nuttx/sched/sched_setscheduler.c index 929cc58e8..2ca4849bb 100644 --- a/nuttx/sched/sched_setscheduler.c +++ b/nuttx/sched/sched_setscheduler.c @@ -173,7 +173,7 @@ int sched_setscheduler(pid_t pid, int policy, /* Set the new priority */ - ret = sched_settcbprio(tcb, param->sched_priority); + ret = sched_reprioritize(tcb, param->sched_priority); sched_unlock(); if (ret != OK) diff --git a/nuttx/sched/sched_settcbprio.c b/nuttx/sched/sched_settcbprio.c deleted file mode 100644 index 78b96b83c..000000000 --- a/nuttx/sched/sched_settcbprio.c +++ /dev/null @@ -1,294 +0,0 @@ -/**************************************************************************** - * sched/sched_settcbprio.c - * - * Copyright (C) 2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 -#include -#include -#include -#include -#include "os_internal.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Type Declarations - ****************************************************************************/ - -/**************************************************************************** - * Global Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: sched_settcbprio - * - * Description: - * This function sets the priority of a specified task. - * - * NOTE: Setting a task's priority to the same value has a similar - * effect to sched_yield() -- The task will be moved to after all other - * tasks with the same priority. - * - * Inputs: - * tcb - the TCB of task to reprioritize. - * sched_priority - The new task priority - * - * Return Value: - * On success, sched_setparam() returns 0 (OK). On error, -1 - * (ERROR) is returned, and errno is set appropriately. - * - * EINVAL The parameter 'param' is invalid or does not make - * sense for the current scheduling policy. - * EPERM The calling task does not have appropriate privileges. - * ESRCH The task whose ID is pid could not be found. - * - * Assumptions: - * - ****************************************************************************/ - -int sched_settcbprio(FAR _TCB *tcb, int sched_priority) -{ - FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; - tstate_t task_state; - irqstate_t saved_state; - - /* Verify that the requested priority is in the valid range */ - - if (sched_priority < SCHED_PRIORITY_MIN || - sched_priority > SCHED_PRIORITY_MAX) - { - errno = EINVAL; - return ERROR; - } - - /* We need to assure that there there is no interrupt activity while - * performing the following. - */ - - saved_state = irqsave(); - - /* There are four cases that must be considered: */ - - task_state = tcb->task_state; - switch (task_state) - { - /* CASE 1. The task is running or ready-to-run and a context switch - * may be caused by the re-prioritization - */ - - case TSTATE_TASK_RUNNING: - - /* A context switch will occur if the new priority of the running - * task becomes less than OR EQUAL TO the next highest priority - * ready to run task. - */ - - if (sched_priority <= tcb->flink->sched_priority) - { - /* A context switch will occur. */ - - up_reprioritize_rtr(tcb, (ubyte)sched_priority); - } - - /* Otherwise, we can just change priority since it has no effect */ - - else - { - /* Change the task priority */ - - tcb->sched_priority = (ubyte)sched_priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)sched_priority; -#endif - } - break; - - /* CASE 2. The task is running or ready-to-run and a context switch - * may be caused by the re-prioritization - */ - - case TSTATE_TASK_READYTORUN: - - /* A context switch will occur if the new priority of the ready-to - * run task is (strictly) greater than the current running task - */ - - if (sched_priority > rtcb->sched_priority) - { - /* A context switch will occur. */ - - up_reprioritize_rtr(tcb, (ubyte)sched_priority); - } - - /* Otherwise, we can just change priority and re-schedule (since it - * have no other effect). - */ - - else - { - /* Remove the TCB from the ready-to-run task list */ - - ASSERT(!sched_removereadytorun(tcb)); - - /* Change the task priority */ - - tcb->sched_priority = (ubyte)sched_priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)sched_priority; -#endif - - /* Put it back into the ready-to-run task list */ - - ASSERT(!sched_addreadytorun(tcb)); - } - break; - - /* CASE 3. The task is not in the ready to run list. Changing its - * Priority cannot effect the currently executing task. - */ - - default: - /* CASE 3a. The task resides in a prioritized list. */ - - if (g_tasklisttable[task_state].prioritized) - { - /* Remove the TCB from the prioritized task list */ - - dq_rem((FAR dq_entry_t*)tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list); - - /* Change the task priority */ - - tcb->sched_priority = (ubyte)sched_priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)sched_priority; -#endif - - /* Put it back into the prioritized list at the correct - * position - */ - - sched_addprioritized(tcb, (FAR dq_queue_t*)g_tasklisttable[task_state].list); - } - - /* CASE 3b. The task resides in a non-prioritized list. */ - - else - { - /* Just change the task's priority */ - - tcb->sched_priority = (ubyte)sched_priority; -#ifdef CONFIG_PRIORITY_INHERITANCE - tcb->base_priority = (ubyte)sched_priority; -#endif - } - break; - } - - irqrestore(saved_state); - return OK; -} - -int sched_setparam(pid_t pid, const struct sched_param *param) -{ - FAR _TCB *rtcb; - FAR _TCB *tcb; - int ret; - - /* Verify that the requested priority is in the valid range */ - - if (!param || - param->sched_priority < SCHED_PRIORITY_MIN || - param->sched_priority > SCHED_PRIORITY_MAX) - { - errno = EINVAL; - return ERROR; - } - - /* Prohibit modifications to the head of the ready-to-run task - * list while adjusting the priority - */ - - sched_lock(); - - /* Check if the task to reprioritize is the calling task */ - - rtcb = (FAR _TCB*)g_readytorun.head; - if (pid == 0 || pid == rtcb->pid) - { - tcb = rtcb; - } - - /* The pid is not the calling task, we will have to search for it */ - - else - { - tcb = sched_gettcb(pid); - if (!tcb) - { - /* No task with this pid was found */ - - errno = ESRCH; - sched_unlock(); - return ERROR; - } - } - - /* Then perform the reprioritization */ - - ret = sched_settcbprio(tcb, param->sched_priority); - sched_unlock(); - return ret; -} diff --git a/nuttx/sched/sched_yield.c b/nuttx/sched/sched_yield.c index 9e4560ad1..79a8515ed 100644 --- a/nuttx/sched/sched_yield.c +++ b/nuttx/sched/sched_yield.c @@ -96,5 +96,5 @@ int sched_yield(void) * be rescheduled behind any other tasks at the same priority. */ - return sched_settcbprio(rtcb, rtcb->sched_priority); + return sched_setpriority(rtcb, rtcb->sched_priority); } diff --git a/nuttx/sched/sem_holder.c b/nuttx/sched/sem_holder.c index f4ee07569..b222512fd 100644 --- a/nuttx/sched/sem_holder.c +++ b/nuttx/sched/sem_holder.c @@ -55,7 +55,7 @@ /* Configuration ************************************************************/ #ifndef CONFIG_SEM_PREALLOCHOLDERS -# define CONFIG_SEM_PREALLOCHOLDERS (4*CONFIG_MAX_TASKS) +# define CONFIG_SEM_PREALLOCHOLDERS 0 #endif /**************************************************************************** @@ -229,7 +229,7 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder) void sem_initholders(void) { -#ifdef CONFIG_SEM_PREALLOCHOLDERS +#if CONFIG_SEM_PREALLOCHOLDERS > 0 int i; /* Put all of the pre-allocated holder structures into free list */ @@ -352,20 +352,19 @@ void sem_boostpriority(sem_t *sem) if (pholder->counts > 0) { htcb = pholder->holder; +#if CONFIG_SEM_NNESTPRIO > 0 +# error "Missing implementation" +#else if (htcb && htcb->sched_priority < rtcb->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. - * - * NOTE that we have to restore base_priority because - * sched_setparam() should set both. */ - int base_priority = htcb->base_priority; - (void)sched_settcbprio(htcb, rtcb->sched_priority); - htcb->base_priority = base_priority; + (void)sched_setpriority(htcb, rtcb->sched_priority); +#endif } } } @@ -450,6 +449,9 @@ void sem_restorebaseprio(sem_t *sem) htcb = pholder->holder; if (htcb) { +#if CONFIG_SEM_NNESTPRIO > 0 +# error "Missing implementation" +#else /* Was the priority of this thread boosted? NOTE: There is * a logical flaw here: If the thread holds multiple semaphore * and has been boosted multiple times, then there is no mechanism @@ -461,7 +463,7 @@ void sem_restorebaseprio(sem_t *sem) { up_reprioritize_rtr(rtcb, htcb->base_priority); } - +#endif /* When no more counts are held, remove the holder from the list */ if (pholder->counts <= 0) diff --git a/nuttx/tools/mkconfig.c b/nuttx/tools/mkconfig.c index 071f0588a..82cff503b 100644 --- a/nuttx/tools/mkconfig.c +++ b/nuttx/tools/mkconfig.c @@ -1,7 +1,7 @@ /**************************************************************************** - * mkconfig.c + * tools/mkconfig.c * - * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -240,6 +240,17 @@ int main(int argc, char **argv, char **envp) printf("# define CONFIG_NFILE_STREAMS 3\n"); printf("# endif\n"); printf("#endif\n\n"); + printf("/* If priority inheritance is disabled, then do not allocate any\n"); + printf(" * associated resources.\n"); + printf(" */\n\n"); + printf("#if !defined(CONFIG_PROIRITY_INHERITANCE) || !defined(CONFIG_SEM_PREALLOCHOLDERSS)\n"); + printf("# undef CONFIG_SEM_PREALLOCHOLDERS\n"); + printf("# define CONFIG_SEM_PREALLOCHOLDERSS 0\n"); + printf("#endif\n\n"); + printf("#if !defined(CONFIG_PROIRITY_INHERITANCE) || !defined(CONFIG_SEM_NNESTPRIO)\n"); + printf("# undef CONFIG_SEM_NNESTPRIO\n"); + printf("# define CONFIG_SEM_NNESTPRIO 0\n"); + printf("#endif\n\n"); printf("/* If no file descriptors are configured, then make certain no\n"); printf(" * streams are configured either.\n"); printf(" */\n\n"); -- cgit v1.2.3