From 2607ba44178fd0ba174967b57c9911dae8182128 Mon Sep 17 00:00:00 2001 From: jepler Date: Thu, 27 Sep 2007 10:13:58 -0500 Subject: [PATCH] "userspace realtime"; on my test system with gutsy kernel 2.6.22-12-rt the latency-test results look quite promising for servo-thread-only systems --- scripts/latency-test | 4 +- src/Makefile | 11 +- src/configure | 52 +------- src/configure.in | 4 - src/hal/components/timedelta.comp | 7 +- src/rtapi/Submakefile | 7 +- src/rtapi/sim_common.h | 6 - src/rtapi/sim_rtapi.c | 263 ++++++++++++++++++++----------------- src/rtapi/sim_rtapi_app.cc | 25 +++- src/rtapi/sim_ulapi.c | 6 + 10 files changed, 191 insertions(+), 194 deletions(-) diff --git a/scripts/latency-test b/scripts/latency-test index 278fb7f..3b0361f 100755 --- a/scripts/latency-test +++ b/scripts/latency-test @@ -1,6 +1,6 @@ #!/bin/bash T=`mktemp -d` -trap 'cd /; [ -d $T ] && rm -rf $T' SIGINT SIGTERM EXIT +#trap 'cd /; [ -d $T ] && rm -rf $T' SIGINT SIGTERM EXIT cd $T calc() { echo "scale=1; $1" | bc; } @@ -45,6 +45,8 @@ if [ "$BASE" -eq "$SERVO" ]; then BASE=0; fi BASE_HUMAN=$(human_time $BASE) SERVO_HUMAN=$(human_time $SERVO) + +echo "$BASE_HUMAN $SERVO_HUMAN" if [ $BASE -eq 0 ]; then cat > lat.hal <&5 -echo $ECHO_N "checking for $ac_word... $ECHO_C" >&6; } -if test "${ac_cv_path_PTH_CONFIG+set}" = set; then - echo $ECHO_N "(cached) $ECHO_C" >&6 -else - case $PTH_CONFIG in - [\\/]* | ?:[\\/]*) - ac_cv_path_PTH_CONFIG="$PTH_CONFIG" # Let the user override the test with a path. - ;; - *) - as_save_IFS=$IFS; IFS=$PATH_SEPARATOR -for as_dir in $PATH -do - IFS=$as_save_IFS - test -z "$as_dir" && as_dir=. - for ac_exec_ext in '' $ac_executable_extensions; do - if { test -f "$as_dir/$ac_word$ac_exec_ext" && $as_test_x "$as_dir/$ac_word$ac_exec_ext"; }; then - ac_cv_path_PTH_CONFIG="$as_dir/$ac_word$ac_exec_ext" - echo "$as_me:$LINENO: found $as_dir/$ac_word$ac_exec_ext" >&5 - break 2 - fi -done -done -IFS=$as_save_IFS - - test -z "$ac_cv_path_PTH_CONFIG" && ac_cv_path_PTH_CONFIG="""" - ;; -esac -fi -PTH_CONFIG=$ac_cv_path_PTH_CONFIG -if test -n "$PTH_CONFIG"; then - { echo "$as_me:$LINENO: result: $PTH_CONFIG" >&5 -echo "${ECHO_T}$PTH_CONFIG" >&6; } -else - { echo "$as_me:$LINENO: result: no" >&5 -echo "${ECHO_T}no" >&6; } -fi - - - if test "$PTH_CONFIG" = ""; then - { { echo "$as_me:$LINENO: error: GNU PTH library is required: get it with apt-get install libpth-dev" >&5 -echo "$as_me: error: GNU PTH library is required: get it with apt-get install libpth-dev" >&2;} - { (exit 1); exit 1; }; } - fi else if test -z "$RTDIR"; then DIRS="/usr/realtime-`uname -r` /usr/realtime /usr /usr/src/rtai*" @@ -10855,7 +10808,6 @@ AUTODIRS!$AUTODIRS$ac_delim PACKAGE!$PACKAGE$ac_delim EMC2VERSION!$EMC2VERSION$ac_delim SIMULATOR!$SIMULATOR$ac_delim -PTH_CONFIG!$PTH_CONFIG$ac_delim RTNAME!$RTNAME$ac_delim RTAI!$RTAI$ac_delim RTLINUX!$RTLINUX$ac_delim @@ -10911,6 +10863,7 @@ WHOAMI!$WHOAMI$ac_delim AWK!$AWK$ac_delim INSMOD!$INSMOD$ac_delim RMMOD!$RMMOD$ac_delim +LSMOD!$LSMOD$ac_delim _ACEOF if test `sed -n "s/.*$ac_delim\$/X/p" conf$$subs.sed | grep -c X` = 97; then @@ -10952,7 +10905,6 @@ _ACEOF ac_delim='%!_!# ' for ac_last_try in false false false false false :; do cat >conf$$subs.sed <<_ACEOF -LSMOD!$LSMOD$ac_delim PIDOF!$PIDOF$ac_delim IPCS!$IPCS$ac_delim FUSER!$FUSER$ac_delim @@ -11026,7 +10978,7 @@ LIBOBJS!$LIBOBJS$ac_delim LTLIBOBJS!$LTLIBOBJS$ac_delim _ACEOF - if test `sed -n "s/.*$ac_delim\$/X/p" conf$$subs.sed | grep -c X` = 72; then + if test `sed -n "s/.*$ac_delim\$/X/p" conf$$subs.sed | grep -c X` = 71; then break elif $ac_last_try; then { { echo "$as_me:$LINENO: error: could not make $CONFIG_STATUS" >&5 diff --git a/src/configure.in b/src/configure.in index 6408d2b..2bb2b65 100644 --- a/src/configure.in +++ b/src/configure.in @@ -140,10 +140,6 @@ AC_ARG_WITH(realtime, #at this point if RTDIR is empty, we need to find RT ourselves if test $SIMULATOR = yes; then RTS=sim - AC_PATH_PROG(PTH_CONFIG,pth-config,"") - if test "$PTH_CONFIG" = ""; then - AC_MSG_ERROR([GNU PTH library is required: get it with apt-get install libpth-dev]) - fi else if test -z "$RTDIR"; then DIRS="/usr/realtime-`uname -r` /usr/realtime /usr /usr/src/rtai*" diff --git a/src/hal/components/timedelta.comp b/src/hal/components/timedelta.comp index 0beb2c0..8a7430b 100644 --- a/src/hal/components/timedelta.comp +++ b/src/hal/components/timedelta.comp @@ -10,6 +10,10 @@ function _ nofp; variable __s64 last=0; variable int first=1; ;; +#ifndef max +#define max(a,b) ((a) < (b) ? (b) : (a)) +#endif + __s64 now = rtapi_get_time(); __s64 del = (now - last); out = del; @@ -24,7 +28,8 @@ if(last != 0) { } else { if(del < min_) min_ = del; if(del > max_) max_ = del; - jitter = max(max_ - period, period - min_); + jitter = max_ - period; + if(period - min_ > jitter) jitter = period - min_; } count++; avg_err = err / (double)count; diff --git a/src/rtapi/Submakefile b/src/rtapi/Submakefile index b8fbcbf..bb58b92 100644 --- a/src/rtapi/Submakefile +++ b/src/rtapi/Submakefile @@ -11,13 +11,10 @@ RTAPI_APP_SRCS := \ rtapi/sim_rtapi_app.cc \ rtapi/sim_rtapi.c USERSRCS += $(RTAPI_APP_SRCS) -PTH_CONFIG ?= pth-config -PTH_CFLAGS := $(shell $(PTH_CONFIG) --cflags 2>/dev/null) -PTH_LINK := $(shell $(PTH_CONFIG) --ldflags --libs 2>/dev/null) -$(call TOOBJSDEPS, $(RTAPI_APP_SRCS)): EXTRAFLAGS += $(PTH_CFLAGS) -DSIM +$(call TOOBJSDEPS, $(RTAPI_APP_SRCS)): EXTRAFLAGS += -pthread -DSIM ../bin/rtapi_app: $(call TOOBJS, $(RTAPI_APP_SRCS)) $(ECHO) Linking $(notdir $@) - @$(CXX) -rdynamic $(LDFLAGS) -o $@ $^ -ldl $(PTH_LINK) + @$(CXX) -rdynamic $(LDFLAGS) -o $@ $^ -ldl -pthread -lrt TARGETS += ../bin/rtapi_app endif diff --git a/src/rtapi/sim_common.h b/src/rtapi/sim_common.h index a9db450..cbba419 100644 --- a/src/rtapi/sim_common.h +++ b/src/rtapi/sim_common.h @@ -192,12 +192,6 @@ int rtapi_get_msg_level() { return msg_level; } -long long rtapi_get_time(void) { - struct timeval tv; - gettimeofday(&tv, 0); - return tv.tv_sec * 1000 * 1000 * 1000 + tv.tv_usec * 1000; -} - #if defined(__i386__) || defined(____x86_64__) #define rdtscll(val) \ __asm__ __volatile__("rdtsc" : "=A" (val)) diff --git a/src/rtapi/sim_rtapi.c b/src/rtapi/sim_rtapi.c index 8946261..25ffe1a 100644 --- a/src/rtapi/sim_rtapi.c +++ b/src/rtapi/sim_rtapi.c @@ -14,18 +14,20 @@ * $Date$ ********************************************************************/ +#define _GNU_SOURCE #include /* vprintf() */ #include /* malloc(), sizeof() */ #include /* va_* */ -#include /* pth_uctx_* */ #include /* usleep() */ #include /* IPC_* */ #include /* shmget() */ -#include /* gettimeofday */ -#include /* gettimeofday */ +#include /* shmget() */ +#include /* clock_gettime etc */ #include "rtapi.h" /* these decls */ #include #include +#include +#include /* These structs hold data associated with objects like tasks, etc. */ /* Task handles are pointers to these structs. */ @@ -37,19 +39,18 @@ struct rtapi_module { struct rtapi_task { int magic; /* to check for valid handle */ int owner; - pth_uctx_t ctx; /* thread's context */ + int deleted; + pthread_t thread; /* thread's context */ size_t stacksize; int prio; int period; int ratio; + int destroyed; + struct timespec next_time; void *arg; void (*taskcode) (void*); /* pointer to task function */ }; -static struct timeval schedule; -static int base_periods; -static pth_uctx_t main_ctx, this_ctx; - #define MODULE_MAGIC 30812 #define TASK_MAGIC 21979 /* random numbers used as signatures */ #define SHMEM_MAGIC 25453 @@ -62,40 +63,65 @@ static pth_uctx_t main_ctx, this_ctx; static struct rtapi_task task_array[MAX_TASKS] = {{0},}; static struct rtapi_module module_array[MAX_MODULES] = {{0},}; +/* local functions and data */ + +static pthread_key_t task_key; +static pthread_once_t task_key_once = PTHREAD_ONCE_INIT; + +static void rtapi_advance_time(struct timespec *tv, unsigned long ns, unsigned long s) { + ns += tv->tv_nsec; + if(ns > 1000000000) { s ++; ns -= 1000000000; } + tv->tv_nsec = ns; + tv->tv_sec += s; +} + +static void rtapi_key_alloc() { + pthread_key_create(&task_key, NULL); +} + +void rtapi_set_task(struct rtapi_task* t) { + pthread_once(&task_key_once, rtapi_key_alloc); + pthread_setspecific(task_key, (void*)t); +} + +static struct rtapi_task * rtapi_this_task() { + pthread_once(&task_key_once, rtapi_key_alloc); + return (struct rtapi_task*)pthread_getspecific(task_key); +} /* Priority functions. SIM uses 0 as the highest priority, as the number increases, the actual priority of the task decreases. */ int rtapi_prio_highest(void) { - return 0; + return sched_get_priority_max(SCHED_FIFO); } int rtapi_prio_lowest(void) { - return 31; + return sched_get_priority_min(SCHED_FIFO); } int rtapi_prio_next_higher(int prio) { /* return a valid priority for out of range arg */ - if (prio <= rtapi_prio_highest()) + if (prio >= rtapi_prio_highest()) return rtapi_prio_highest(); - if (prio > rtapi_prio_lowest()) + if (prio < rtapi_prio_lowest()) return rtapi_prio_lowest(); /* return next higher priority for in-range arg */ - return prio - 1; + return prio + 1; } int rtapi_prio_next_lower(int prio) { /* return a valid priority for out of range arg */ - if (prio >= rtapi_prio_lowest()) + if (prio <= rtapi_prio_lowest()) return rtapi_prio_lowest(); - if (prio < rtapi_prio_highest()) + if (prio > rtapi_prio_highest()) return rtapi_prio_highest(); /* return next lower priority for in-range arg */ - return prio + 1; + return prio - 1; } @@ -127,13 +153,17 @@ int rtapi_exit(int id) static int period = 0; int rtapi_clock_set_period(unsigned long int nsecs) { + struct timespec res = {0,0}; if(nsecs == 0) return period; if(period != 0) { rtapi_print_msg(RTAPI_MSG_ERR, "attempt to set period twice\n"); return RTAPI_INVAL; } - period = nsecs; - gettimeofday(&schedule, NULL); + clock_getres(CLOCK_MONOTONIC, &res); + period = (nsecs / res.tv_nsec) * res.tv_nsec; + if(period < 1) period = res.tv_nsec; +rtapi_print_msg(RTAPI_MSG_ERR, "rtapi_clock_set_period (res=%ld) -> %d\n", + res.tv_nsec, period); return period; } @@ -157,17 +187,22 @@ int rtapi_task_new(void (*taskcode) (void*), void *arg, task = &(task_array[n]); /* check requested priority */ - if ((prio < rtapi_prio_highest()) || (prio > rtapi_prio_lowest())) - return RTAPI_INVAL; + { + int highest = rtapi_prio_highest(); + int lowest = rtapi_prio_lowest(); + rtapi_print_msg(RTAPI_MSG_ERR, "request=%d highest=%d lowest=%d\n", + prio, highest, lowest); + if(prio < lowest || prio > highest) { return RTAPI_INVAL; } + } /* label as a valid task structure */ /*! \todo FIXME - end of non-threadsafe window */ if(stacksize < 16384) stacksize = 16384; task->magic = TASK_MAGIC; task->owner = owner; - task->ctx = NULL; task->arg = arg; task->stacksize = stacksize; + task->destroyed = 0; task->taskcode = taskcode; task->prio = prio; @@ -187,34 +222,72 @@ int rtapi_task_delete(int id) { if (task->magic != TASK_MAGIC) return RTAPI_INVAL; - pth_uctx_destroy(task->ctx); + task->deleted = 1; - task->magic = 0; return RTAPI_SUCCESS; } +#ifndef CPU_SETSIZE +#define CPU_SETSIZE (8*sizeof(cpu_set_t)) +#endif -static void wrapper(void *arg) +void rtapi_set_affinity() { + cpu_set_t set; + static int use_cpu = CPU_SETSIZE; + int result; + + if(use_cpu == CPU_SETSIZE) { + int cpu_nr; + sched_getaffinity(0, sizeof(set), &set); + for(cpu_nr = CPU_SETSIZE - 1; cpu_nr > 0; cpu_nr --) { + if(CPU_ISSET(cpu_nr, &set)) break; + } + use_cpu = cpu_nr; + } + rtapi_print_msg(RTAPI_MSG_ERR, "Using CPU %d\n", use_cpu); + CPU_ZERO(&set); + CPU_SET(use_cpu, &set); + result = sched_setaffinity(0, sizeof(set), &set); + rtapi_print_msg(RTAPI_MSG_ERR, "sched_setaffinity() -> %d\n", result); +} + +static void *wrapper(void *arg) { struct rtapi_task *task; + struct sched_param schedp; /* use the argument to point to the task data */ task = (struct rtapi_task*)arg; if(task->period < period) task->period = period; + rtapi_set_task(task); + + memset(&schedp, 0, sizeof(schedp)); + schedp.sched_priority = task->prio; + if(sched_setscheduler(0, SCHED_FIFO, &schedp)) perror("sched_setscheduler"); + task->ratio = task->period / period; rtapi_print_msg(RTAPI_MSG_INFO, "task %p period = %d ratio=%d\n", task, task->period, task->ratio); + rtapi_set_affinity(); + + clock_gettime(CLOCK_MONOTONIC, &task->next_time); + rtapi_advance_time(&task->next_time, task->period, 0); + /* call the task function with the task argument */ (task->taskcode) (task->arg); rtapi_print("ERROR: reached end of wrapper for task %d\n", task - task_array); + + return NULL; } int rtapi_task_start(int task_id, unsigned long int period_nsec) { struct rtapi_task *task; + pthread_attr_t attr; + int retval; if(task_id < 0 || task_id >= MAX_TASKS) return RTAPI_INVAL; @@ -231,14 +304,15 @@ int rtapi_task_start(int task_id, unsigned long int period_nsec) /* create the thread - use the wrapper function, pass it a pointer to the task structure so it can call the actual task function */ - retval = pth_uctx_create(&task->ctx); - if (retval == FALSE) - return RTAPI_NOMEM; - retval = pth_uctx_make(task->ctx, NULL, task->stacksize, NULL, - wrapper, (void*)task, 0); - if (retval == FALSE) - return RTAPI_NOMEM; + pthread_attr_init(&attr); + pthread_attr_setstacksize(&attr, task->stacksize); + retval = pthread_create(&task->thread, &attr, wrapper, (void*)task); + pthread_attr_destroy(&attr); + + if (retval != 0) { + return RTAPI_NOMEM; + } return RTAPI_SUCCESS; } @@ -254,7 +328,7 @@ int rtapi_task_stop(int task_id) if (task->magic != TASK_MAGIC) return RTAPI_INVAL; - pth_uctx_destroy(task->ctx); + task->destroyed = 1; return RTAPI_SUCCESS; } @@ -307,19 +381,49 @@ int rtapi_task_set_period(int task_id, int rtapi_wait(void) { - pth_uctx_switch(this_ctx, main_ctx); + struct timespec ts; + struct rtapi_task *task = rtapi_this_task(); + + if(task->deleted) { + task->magic = 0; + pthread_exit(0); + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &task->next_time, NULL); + rtapi_advance_time(&task->next_time, task->period, 0); + clock_gettime(CLOCK_MONOTONIC, &ts); + if(ts.tv_sec > task->next_time.tv_sec + || (ts.tv_sec == task->next_time.tv_sec + && ts.tv_nsec > task->next_time.tv_nsec)) { + static int failures = 0; + failures ++; + if(failures == 1) { + rtapi_print_msg(RTAPI_MSG_ERR, + "ERROR: Missed scheduling deadline for task %d\n", + task - task_array); + rtapi_print_msg(RTAPI_MSG_ERR, "%9ld.%09ld vs %9ld.%09ld\n", + (long)ts.tv_sec, (long)ts.tv_nsec, + (long)task->next_time.tv_sec, (long)task->next_time.tv_nsec); + } else if(failures < 10 || (failures % 10000 == 0)) { + rtapi_print_msg(RTAPI_MSG_WARN, + "ERROR: Missed scheduling deadline for task %d\n [%d times]", + task - task_array, failures); + rtapi_print_msg(RTAPI_MSG_WARN, "%9ld.%09ld vs %9ld.%09ld\n", + (long)ts.tv_sec, (long)ts.tv_nsec, + (long)task->next_time.tv_sec, (long)task->next_time.tv_nsec); + } + } return RTAPI_SUCCESS; } - void rtapi_outb(unsigned char byte, unsigned int port) { - return; + outb(byte, port); } unsigned char rtapi_inb(unsigned int port) { - return 0; + return inb(port); } /*! \todo @@ -405,88 +509,9 @@ long int simple_strtol(const char *nptr, char **endptr, int base) { return strtol(nptr, endptr, base); } -#define MIN_RUNS 13 - -static int maybe_sleep(int fd) { - struct timeval now; - struct timeval interval; - - if(period == 0) { - fd_set fds; - FD_ZERO(&fds); - FD_SET(fd, &fds); - - return select(fd+1, &fds, NULL, NULL, NULL); - } else { - schedule.tv_usec += period / 1000; - if(schedule.tv_usec > 1000000) { - schedule.tv_usec -= 1000000; - schedule.tv_sec ++; - } - - if(period < 100000) { - // if base_period is fast (<.1ms) then run 10 times (e.g., enough - // for .5ms if base_period is 50uS) without any syscalls - if(base_periods % MIN_RUNS) return 0; - } - gettimeofday(&now, NULL); - interval.tv_sec = schedule.tv_sec - now.tv_sec; - interval.tv_usec = schedule.tv_usec - now.tv_usec; - - if(interval.tv_usec < 0) { - interval.tv_sec --; - interval.tv_usec += 1000000; - } - - if(interval.tv_sec < -10) { - // Something happened, like getting stopped in the debugger - // for a long time. Instead of playing catch-up, just forget - // about it - rtapi_print_msg(RTAPI_MSG_DBG, "Long pause, resetting schedule\n"); - memcpy(&schedule, &now, sizeof(struct timeval)); - } - if(interval.tv_sec > 0 - || (interval.tv_sec == 0 && interval.tv_usec >= 0)) { - fd_set fds; - FD_ZERO(&fds); - FD_SET(fd, &fds); - - return select(fd+1, &fds, NULL, NULL, &interval); - } - } - return 0; +long long rtapi_get_time(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts.tv_sec * 1000 * 1000 * 1000 + ts.tv_nsec; } - - -int sim_rtapi_run_threads(int fd) { - static int first_time = 1; - if(first_time) { - int result = pth_uctx_create(&main_ctx); - if(result == FALSE) _exit(1); - first_time = 0; - } - while(1) { - int result = maybe_sleep(fd); - if(result) { - return result; - } - - if(period) { - int t; - base_periods++; - for(t=0; tmagic == TASK_MAGIC && task->ctx && - (base_periods % task->ratio == 0)) { - int result; - this_ctx = task->ctx; - result = pth_uctx_switch(main_ctx, task->ctx); - if(result == FALSE) _exit(1); - } - } - } - } -} - - #include "rtapi/sim_common.h" diff --git a/src/rtapi/sim_rtapi_app.cc b/src/rtapi/sim_rtapi_app.cc index bc126af..b3a22cc 100644 --- a/src/rtapi/sim_rtapi_app.cc +++ b/src/rtapi/sim_rtapi_app.cc @@ -11,7 +11,11 @@ #include #include #include + +#include #include +#include +#include #include #include "config.h" @@ -20,7 +24,10 @@ #include "hal.h" #include "hal/hal_priv.h" -extern "C" int sim_rtapi_run_threads(int fd); +extern "C" { +int capset(cap_user_header_t hdrp, const cap_user_data_t datap); +int capget(cap_user_header_t hdrp, cap_user_data_t datap); +} using namespace std; @@ -313,8 +320,6 @@ static int master(int fd, vector args) { memset(&client_addr, 0, sizeof(client_addr)); socklen_t len = sizeof(client_addr); - sim_rtapi_run_threads(fd); - int fd1 = accept(fd, (sockaddr*)&client_addr, &len); if(fd1 < 0) { perror("accept"); @@ -332,6 +337,20 @@ static int master(int fd, vector args) { } int main(int argc, char **argv) { + if(iopl(3)) perror("iopl"); + if(mlockall(MCL_FUTURE)) perror("mlockall"); + +#if 0 + cap_user_header_t header; + cap_user_data_t data; + capget(header, data); + data->effective |= CAP_SYS_NICE | CAP_SYS_RAWIO; + if(capset(header, data) != 0) { + perror("capset"); + } + seteuid(getuid()); +#endif + vector args; for(int i=1; i