From e9faebc794c798462a34d5fae9c922d46e95a374 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 17 Mar 2007 21:32:21 +0000 Subject: Add test of roundrobin scheduler (still does not work) git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@81 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/c5471/src/up_reprioritizertr.c | 3 +- nuttx/arch/dm320/src/up_reprioritizertr.c | 3 +- nuttx/arch/pjrc-8051/src/up_reprioritizertr.c | 4 +- nuttx/arch/sim/defconfig | 2 +- nuttx/examples/ostest/Makefile | 2 +- nuttx/examples/ostest/main.c | 8 + nuttx/examples/ostest/ostest.h | 4 + nuttx/examples/ostest/roundrobin.c | 212 ++++++++++++++++++++++++++ nuttx/sched/sched_addreadytorun.c | 10 +- nuttx/sched/sched_processtimer.c | 44 +++--- 10 files changed, 265 insertions(+), 27 deletions(-) create mode 100644 nuttx/examples/ostest/roundrobin.c diff --git a/nuttx/arch/c5471/src/up_reprioritizertr.c b/nuttx/arch/c5471/src/up_reprioritizertr.c index 414b410d0..b416d7dfe 100644 --- a/nuttx/arch/c5471/src/up_reprioritizertr.c +++ b/nuttx/arch/c5471/src/up_reprioritizertr.c @@ -155,12 +155,13 @@ void up_reprioritize_rtr(_TCB *tcb, ubyte priority) up_copystate(current_regs, rtcb->xcp.regs); } + /* Copy the exception context into the TCB at the (old) head of the * g_readytorun Task list. if up_saveusercontext returns a non-zero * value, then this is really the previously running task restarting! */ - if (!up_saveusercontext(rtcb->xcp.regs)) + else if (!up_saveusercontext(rtcb->xcp.regs)) { /* Restore the exception context of the rtcb at the (new) head * of the g_readytorun task list. diff --git a/nuttx/arch/dm320/src/up_reprioritizertr.c b/nuttx/arch/dm320/src/up_reprioritizertr.c index 414b410d0..b416d7dfe 100644 --- a/nuttx/arch/dm320/src/up_reprioritizertr.c +++ b/nuttx/arch/dm320/src/up_reprioritizertr.c @@ -155,12 +155,13 @@ void up_reprioritize_rtr(_TCB *tcb, ubyte priority) up_copystate(current_regs, rtcb->xcp.regs); } + /* Copy the exception context into the TCB at the (old) head of the * g_readytorun Task list. if up_saveusercontext returns a non-zero * value, then this is really the previously running task restarting! */ - if (!up_saveusercontext(rtcb->xcp.regs)) + else if (!up_saveusercontext(rtcb->xcp.regs)) { /* Restore the exception context of the rtcb at the (new) head * of the g_readytorun task list. diff --git a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c index 1b8a3adbf..b148e755c 100644 --- a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c +++ b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c @@ -134,7 +134,7 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority) sched_mergepending(); } - /* Are we in an interrupt handler? */ + /* Are we in an interrupt handler? */ if (g_irqtos) { @@ -163,7 +163,7 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority) * value, then this is really the previously running task restarting! */ - if (!up_savecontext(&rtcb->xcp)) + else if (!up_savecontext(&rtcb->xcp)) { /* Restore the exception context of the rtcb at the (new) head * of the g_readytorun task list. diff --git a/nuttx/arch/sim/defconfig b/nuttx/arch/sim/defconfig index bff4ac404..910623d1c 100644 --- a/nuttx/arch/sim/defconfig +++ b/nuttx/arch/sim/defconfig @@ -73,7 +73,7 @@ CONFIG_DEBUG=y CONFIG_DEBUG_VERBOSE=y CONFIG_MM_REGIONS=1 CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=200 +CONFIG_RR_INTERVAL=0 CONFIG_SCHED_INSTRUMENTATION=n CONFIG_TASK_NAME_SIZE=32 CONFIG_START_YEAR=2007 diff --git a/nuttx/examples/ostest/Makefile b/nuttx/examples/ostest/Makefile index 3e071e146..ed7859fc8 100644 --- a/nuttx/examples/ostest/Makefile +++ b/nuttx/examples/ostest/Makefile @@ -42,7 +42,7 @@ ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = main.c dev_null.c ifneq ($(CONFIG_DISABLE_PTHREAD),y) -CSRCS += cancel.c cond.c mutex.c sem.c +CSRCS += cancel.c cond.c mutex.c sem.c roundrobin.c endif ifneq ($(CONFIG_DISABLE_SIGNALS),y) CSRCS += sighand.c diff --git a/nuttx/examples/ostest/main.c b/nuttx/examples/ostest/main.c index 8272103a5..7afba05c9 100644 --- a/nuttx/examples/ostest/main.c +++ b/nuttx/examples/ostest/main.c @@ -164,6 +164,14 @@ static int user_main(int argc, char *argv[]) sighand_test(); #endif +#if 0 /* Does not work yet */ +#if !defined(CONFIG_DISABLE_PTHREAD) && CONFIG_RR_INTERVAL > 0 + /* Verify round robin scheduling */ + + rr_test(); +#endif +#endif + printf("user_main: Exitting\n"); return 0; } diff --git a/nuttx/examples/ostest/ostest.h b/nuttx/examples/ostest/ostest.h index 8d567cb37..af5b490b4 100644 --- a/nuttx/examples/ostest/ostest.h +++ b/nuttx/examples/ostest/ostest.h @@ -92,4 +92,8 @@ extern void timedwait_test(void); extern void sighand_test(void); +/* roundrobin.c *********************************************/ + +extern void rr_test(void); + #endif /* __OSTEST_H */ diff --git a/nuttx/examples/ostest/roundrobin.c b/nuttx/examples/ostest/roundrobin.c new file mode 100644 index 000000000..d8217e451 --- /dev/null +++ b/nuttx/examples/ostest/roundrobin.c @@ -0,0 +1,212 @@ +/******************************************************************************** + * dev_null.c + * + * Copyright (C) 2007 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 Gregory Nutt 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 "ostest.h" + +#if CONFIG_RR_INTERVAL > 0 + +/******************************************************************************** + * Definitions + ********************************************************************************/ + +#define CONFIG_NINTEGERS 32768 + +/******************************************************************************** + * Private Data + ********************************************************************************/ + +static int prime1[CONFIG_NINTEGERS]; +static int prime2[CONFIG_NINTEGERS]; + +/******************************************************************************** + * Private Functions + ********************************************************************************/ + +/******************************************************************************** + * Name: dosieve + * + * Description + * This implements a "sieve of aristophanes" algorithm for finding prime number. + * Credit for this belongs to someone, but I am not sure who anymore. Anyway, + * the only purpose here is that we need some algorithm that takes a long period + * of time to execute. + * + ********************************************************************************/ + +static void dosieve(int *prime) +{ + int a,d; + int i; + int j; + + a = 2; + d = a; + + for (i = 0; i < CONFIG_NINTEGERS; i++) + { + prime[i] = i+2; + } + + for (i = 1; i < 10; i++) + { + for (j = 0; j < CONFIG_NINTEGERS; j++) + { + d = a + d; + if (d < CONFIG_NINTEGERS) + { + prime[d]=0; + } + } + a++; + d = a; + i++; + } + +#if 0 /* We don't really care what the numbers are */ + for (i = 0, j= 0; i < CONFIG_NINTEGERS; i++) + { + if (prime[i] != 0) + { + printf(" Prime %d: %d\n", j, prime[i]); + j++; + } + } +#endif +} + +/******************************************************************************** + * Name: sieve1 + ********************************************************************************/ + +static void *sieve1(void *parameter) +{ + int i; + + printf("sieve1 started\n"); + for (i = 0; i < 1000; i++) + { + dosieve(prime1); + } + pthread_exit(NULL); +} + +/******************************************************************************** + * Name: sieve2 + ********************************************************************************/ + +static void *sieve2(void *parameter) +{ + int i; + + printf("sieve2 started\n"); + for (i = 0; i < 1000; i++) + { + dosieve(prime2); + } + pthread_exit(NULL); +} + +/******************************************************************************** + * Public Functions + ********************************************************************************/ + +/******************************************************************************** + * Name: rr_test + ********************************************************************************/ + +void rr_test(void) +{ + pthread_t sieve1_thread; + pthread_t sieve2_thread; + struct sched_param sparam; + pthread_attr_t attr; + pthread_addr_t result; + int status; + + printf("rr_test: Starting sieve1 thread \n"); + status = pthread_attr_init(&attr); + if (status != OK) + { + printf("rr_test: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = sched_get_priority_min(SCHED_FIFO); + status = pthread_attr_setschedparam(&attr, &sparam); + if (status != OK) + { + printf("rr_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("rr_test: Set thread priority to %d\n", sparam.sched_priority); + } + + status = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (status != OK) + { + printf("rr_test: pthread_attr_setschedpolicy failed, status=%d\n", status); + } + else + { + printf("rr_test: Set thread policty to SCHED_RR\n"); + } + + status = pthread_create(&sieve1_thread, &attr, sieve1, NULL); + if (status != 0) + { + printf("rr_test: Error in thread 1 creation, status=%d\n", status); + } + + printf("rr_test: Starting sieve1 thread \n"); + + status = pthread_create(&sieve2_thread, &attr, sieve2, NULL); + if (status != 0) + { + printf("rr_test: Error in thread 2 creation, status=%d\n", status); + } + + printf("rr_test: Waiting for sieves to complete\n"); + pthread_join(sieve2_thread, &result); + pthread_join(sieve1_thread, &result); + printf("rr_test: Done\n"); +} + +#endif /* CONFIG_RR_INTERVAL */ diff --git a/nuttx/sched/sched_addreadytorun.c b/nuttx/sched/sched_addreadytorun.c index 3192fc7f9..4344caf31 100644 --- a/nuttx/sched/sched_addreadytorun.c +++ b/nuttx/sched/sched_addreadytorun.c @@ -92,6 +92,7 @@ * whatever list it was in. * - The caller handles the condition that occurs if the * the head of the g_readytorun list is changed. + * ************************************************************/ boolean sched_addreadytorun(FAR _TCB *btcb) @@ -99,14 +100,15 @@ boolean sched_addreadytorun(FAR _TCB *btcb) FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; boolean ret; - /* Check if pre-emption is disabled for the current running task and - * if the rtrTask would cause the current running task to be preempted. + /* Check if pre-emption is disabled for the current running + * task and if the new ready-to-run task would cause the + * current running task to be preempted. */ if (rtcb->lockcount && rtcb->sched_priority < btcb->sched_priority) { - /* Yes. Preemption would occur! Add the btcb to the g_pendingtasks - * task list for now. + /* Yes. Preemption would occur! Add the new ready-to-run + * task to the g_pendingtasks task list for now. */ sched_addprioritized(btcb, &g_pendingtasks); diff --git a/nuttx/sched/sched_processtimer.c b/nuttx/sched/sched_processtimer.c index c6ee8fdb8..3decd12e7 100644 --- a/nuttx/sched/sched_processtimer.c +++ b/nuttx/sched/sched_processtimer.c @@ -91,31 +91,41 @@ static void sched_process_timeslice(void) if (rtcb->timeslice <= 1) { - /* We know we are at the head of the ready to run - * prioritized list. We must be the highest priority - * task eligible for execution. Check the next task - * in the ready to run list. If it is the same - * priority, then we need to relinquish the CPU and - * give that task a shot. + + /* Yes, Now check if the task has pre-emption disabled. + * If so, then we will freeze the timeslice count at + * the value until the next tick after pre-emption + * has been enabled. */ - if (rtcb->flink && - rtcb->flink->sched_priority >= rtcb->sched_priority) + if (!rtcb->lockcount) { - struct sched_param param; - - /* Reset the timeslice */ + /* Reset the timeslice in any case. */ rtcb->timeslice = CONFIG_RR_INTERVAL; - /* Just resetting the task priority to its current - * value. This this will cause the task to be - * rescheduled behind any other tasks at the same - * priority. + /* We know we are at the head of the ready to run + * prioritized list. We must be the highest priority + * task eligible for execution. Check the next task + * in the ready to run list. If it is the same + * priority, then we need to relinquish the CPU and + * give that task a shot. */ - param.sched_priority = rtcb->sched_priority; - (void)sched_setparam(0, ¶m); + if (rtcb->flink && + rtcb->flink->sched_priority >= rtcb->sched_priority) + { + struct sched_param param; + + /* Just resetting the task priority to its current + * value. This this will cause the task to be + * rescheduled behind any other tasks at the same + * priority. + */ + + param.sched_priority = rtcb->sched_priority; + (void)sched_setparam(0, ¶m); + } } } else -- cgit v1.2.3