diff --git a/src/Makefile.inc.in b/src/Makefile.inc.in index 31d4b6abf94..6e070fbf139 100644 --- a/src/Makefile.inc.in +++ b/src/Makefile.inc.in @@ -232,6 +232,10 @@ CONFIG_USPACE_XENOMAI=@CONFIG_USPACE_XENOMAI@ XENOMAI_CFLAGS=@XENOMAI_CFLAGS@ XENOMAI_LDFLAGS=@XENOMAI_LDFLAGS@ +CONFIG_USPACE_XENOMAI_EVL=@CONFIG_USPACE_XENOMAI_EVL@ +XENOMAI_EVL_CFLAGS=@XENOMAI_EVL_CFLAGS@ +XENOMAI_EVL_LDFLAGS=@XENOMAI_EVL_LDFLAGS@ + LIBTIRPC_CFLAGS=@LIBTIRPC_CFLAGS@ LIBTIRPC_LIBS=@LIBTIRPC_LIBS@ PYTHON_CPPFLAGS=@PYTHON_CPPFLAGS@ diff --git a/src/configure.ac b/src/configure.ac index 4f9394d149b..04e54184d4c 100644 --- a/src/configure.ac +++ b/src/configure.ac @@ -212,6 +212,12 @@ AC_SUBST([RTAI_CONFIG]) AC_PATH_PROG([XENOMAI_CONFIG], [xeno-config], [none]) AC_SUBST([XENOMAI_CONFIG]) +#Xenomai evl +HAS_EVL=n +AC_CHECK_HEADER([evl/evl.h],[ + HAS_EVL=y +]) + AC_MSG_CHECKING([for realtime API(s) to use]) if test "$RTS" = uspace || test -z "$RTS" -a "$RTAI_CONFIG" = "none" ; then RTS=uspace @@ -243,11 +249,29 @@ if test "$RTS" = uspace || test -z "$RTS" -a "$RTAI_CONFIG" = "none" ; then XENOMAI_LDFLAGS=`$XENOMAI_CONFIG --skin posix --ldflags` AC_DEFINE([USPACE_XENOMAI], [], [Define if uspace realtime should optionally support Xenomai]) fi + AC_SUBST([CONFIG_USPACE_XENOMAI]) AC_SUBST([XENOMAI_CFLAGS]) AC_SUBST([XENOMAI_LDFLAGS]) - AC_MSG_RESULT([$RTS$USPACE_RTAI$USPACE_XENOMAI]) + if test "$HAS_EVL" = "n" ; then + USPACE_XENOMAI_EVL= + CONFIG_USPACE_XENOMAI_EVL=n + XENOMAI_EVL_CFLAGS= + XENOMAI_EVL_LDFLAGS= + else + USPACE_XENOMAI_EVL=+xenomai4 + CONFIG_USPACE_XENOMAI_EVL=y + XENOMAI_EVL_CFLAGS= + XENOMAI_EVL_LDFLAGS=-levl + AC_DEFINE([USPACE_XENOMAI_EVL], [], [Define if uspace realtime should optionally support Xenomai-evl]) + fi + + AC_SUBST([CONFIG_USPACE_XENOMAI_EVL]) + AC_SUBST([XENOMAI_EVL_CFLAGS]) + AC_SUBST([XENOMAI_EVL_LDFLAGS]) + + AC_MSG_RESULT([$RTS$USPACE_RTAI$USPACE_XENOMAI$USPACE_XENOMAI_EVL]) AC_MSG_CHECKING([whether to enable userspace PCI access]) AC_ARG_ENABLE(userspace-pci, diff --git a/src/rtapi/Submakefile b/src/rtapi/Submakefile index 59769534f25..a51231dc1af 100644 --- a/src/rtapi/Submakefile +++ b/src/rtapi/Submakefile @@ -68,3 +68,14 @@ $(call TOOBJSDEPS, $(USPACE_XENOMAI_SRCS)): EXTRAFLAGS += -fPIC $(XENOMAI_CFLAGS TARGETS += ../lib/libuspace-xenomai.so.0 TARGETS += ../lib/libuspace-xenomai.so endif + +ifeq ($(CONFIG_USPACE_XENOMAI_EVL),y) +USPACE_XENOMAI_EVL_SRCS := rtapi/uspace_xenomai_evl.cc +USERSRCS += $(USPACE_XENOMAI_EVL_SRCS) +$(call TOOBJSDEPS, $(USPACE_XENOMAI_EVL_SRCS)): EXTRAFLAGS += -fPIC $(XENOMAI_EVL_CFLAGS) +../lib/libuspace-xenomai-evl.so.0: $(call TOOBJS, $(USPACE_XENOMAI_EVL_SRCS)) + $(ECHO) Linking $(notdir $@) + $(Q)$(CXX) -shared $(LDFLAGS) -o $@ $^ $(XENOMAI_EVL_LDFLAGS) -Wl,-soname,$(notdir $@) +TARGETS += ../lib/libuspace-xenomai-evl.so.0 +TARGETS += ../lib/libuspace-xenomai-evl.so +endif diff --git a/src/rtapi/rtapi_uspace.hh b/src/rtapi/rtapi_uspace.hh index c157ecc56db..bc589c12536 100644 --- a/src/rtapi/rtapi_uspace.hh +++ b/src/rtapi/rtapi_uspace.hh @@ -75,6 +75,7 @@ struct RtapiApp virtual int prio_lowest() const; int prio_higher_delta() const; int prio_bound(int prio) const; + bool prio_check(int prio) const; int prio_next_higher(int prio) const; int prio_next_lower(int prio) const; long clock_set_period(long int period_nsec); @@ -106,6 +107,8 @@ T *rtapi_get_task(int task_id) { return static_cast(RtapiApp::get_task(task_id)); } +int find_rt_cpu_number(); + #define MAX_TASKS 64 #define TASK_MAGIC 21979 /* random numbers used as signatures */ #define TASK_MAGIC_INIT ((rtapi_task*)(-1)) diff --git a/src/rtapi/uspace_common.h b/src/rtapi/uspace_common.h index 6a4248e0909..9c82c3a397e 100644 --- a/src/rtapi/uspace_common.h +++ b/src/rtapi/uspace_common.h @@ -382,15 +382,27 @@ static int detect_rtai() { #endif #ifdef USPACE_XENOMAI static int detect_xenomai() { - struct utsname u; - uname(&u); - return strcasestr (u.release, "-xenomai") != 0; + struct stat sb; + //Running xenomai has /proc/xenomai + return stat("/proc/xenomai", &sb) == 0; } #else static int detect_xenomai() { return 0; } #endif +#ifdef USPACE_XENOMAI_EVL +static int detect_xenomai_evl() { + struct stat sb; + //Running xenomai evl has /dev/evl but no /proc/xenomai + return stat("/dev/evl", &sb) == 0; +} +#else +static int detect_xenomai_evl() { + return 0; +} +#endif + static int detect_env_override() { char *p = getenv("LINUXCNC_FORCE_REALTIME"); return p != NULL && atoi(p) != 0; @@ -401,7 +413,7 @@ static int detect_realtime() { if ((stat(EMC2_BIN_DIR "/rtapi_app", &st) < 0) || st.st_uid != 0 || !(st.st_mode & S_ISUID)) return 0; - return detect_env_override() || detect_preempt_rt() || detect_rtai() || detect_xenomai(); + return detect_env_override() || detect_preempt_rt() || detect_rtai() || detect_xenomai() || detect_xenomai_evl(); } int rtapi_is_realtime() { diff --git a/src/rtapi/uspace_rtai.cc b/src/rtapi/uspace_rtai.cc index 39c01d570bc..ff5f9d784ed 100644 --- a/src/rtapi/uspace_rtai.cc +++ b/src/rtapi/uspace_rtai.cc @@ -81,13 +81,18 @@ struct RtaiApp : RtapiApp { static void *wrapper(void *arg) { auto task = reinterpret_cast(arg); pthread_setspecific(key, arg); - task->rt_task = rt_task_init(task->id, task->prio, 0, 0); + + int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); + int cpus_allowed = 1 << (nprocs-1); //Use last CPU as default + const static int rt_cpu_number = find_rt_cpu_number(); + if(rt_cpu_number != -1) { + rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); + cpus_allowed = 1 << rt_cpu_number; + } + task->rt_task = rt_task_init_schmod(task->id, task->prio, 0, 0, SCHED_FIFO, cpus_allowed); rt_set_periodic_mode(); start_rt_timer(nano2count(task->period)); if(task->uses_fp) rt_task_use_fpu(task->rt_task, 1); - // assumes processor numbers are contiguous - int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); - rt_set_runnable_on_cpus(task->rt_task, 1u << (nprocs - 1)); rt_make_hard_real_time(); rt_task_make_periodic_relative_ns(task->rt_task, task->period, task->period); (task->taskcode) (task->arg); @@ -115,6 +120,7 @@ struct RtaiApp : RtapiApp { } int task_pll_set_correction(long value) { + (void)value; // PLL functions not supported return -EINVAL; } @@ -133,6 +139,7 @@ struct RtaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return inb(port); #else + (void)port; return 0; #endif } @@ -141,6 +148,7 @@ struct RtaiApp : RtapiApp { #ifdef HAVE_SYS_IO_H return outb(val, port); #else + (void)port; return 0; #endif } diff --git a/src/rtapi/uspace_rtapi_app.cc b/src/rtapi/uspace_rtapi_app.cc index c491178d981..30a159dc983 100644 --- a/src/rtapi/uspace_rtapi_app.cc +++ b/src/rtapi/uspace_rtapi_app.cc @@ -855,7 +855,10 @@ static RtapiApp *makeApp() } WithRoot r; void *dll = nullptr; - if(detect_xenomai()) { + if(detect_xenomai_evl()) { + dll = dlopen(EMC2_HOME "/lib/libuspace-xenomai-evl.so.0", RTLD_NOW); + if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); + }else if(detect_xenomai()) { dll = dlopen(EMC2_HOME "/lib/libuspace-xenomai.so.0", RTLD_NOW); if(!dll) fprintf(stderr, "dlopen: %s\n", dlerror()); } else if(detect_rtai()) { @@ -918,6 +921,14 @@ int RtapiApp::prio_bound(int prio) const { return prio; } +bool RtapiApp::prio_check(int prio) const { + if(rtapi_prio_highest() > rtapi_prio_lowest()) { + return (prio <= rtapi_prio_highest()) && (prio >= rtapi_prio_lowest()); + } else { + return (prio <= rtapi_prio_lowest()) && (prio >= rtapi_prio_highest()); + } +} + int RtapiApp::prio_next_higher(int prio) const { prio = prio_bound(prio); @@ -948,8 +959,10 @@ int RtapiApp::allocate_task_id() int RtapiApp::task_new(void (*taskcode) (void*), void *arg, int prio, int owner, unsigned long int stacksize, int uses_fp) { /* check requested priority */ - if ((prio > rtapi_prio_highest()) || (prio < rtapi_prio_lowest())) + if (!prio_check(prio)) { + rtapi_print_msg(RTAPI_MSG_ERR,"rtapi:task_new prio is not in bound lowest %i prio %i highest %i\n", + rtapi_prio_lowest(), prio, rtapi_prio_highest()); return -EINVAL; } @@ -1010,48 +1023,72 @@ int Posix::task_delete(int id) return 0; } -static int find_rt_cpu_number() { +//parse_cpu_list from https://gitlab.com/Xenomai/xenomai4/libevl/-/blob/11e6a1fb183a315ae861762e7650fd5e10d83ff5/tests/helpers.c +//License: MIT +static void parse_cpu_list(const char *path, cpu_set_t *cpuset) +{ + char *p, *range, *range_p = NULL, *id, *id_r; + int start, end, cpu; + char buf[BUFSIZ]; + FILE *fp; + + CPU_ZERO(cpuset); + + fp = fopen(path, "r"); + if (fp == NULL) + return; + + if (!fgets(buf, sizeof(buf), fp)) + goto out; + + p = buf; + while ((range = strtok_r(p, ",", &range_p)) != NULL) { + if (*range == '\0' || *range == '\n') + goto next; + end = -1; + id = strtok_r(range, "-", &id_r); + if (id) { + start = atoi(id); + id = strtok_r(NULL, "-", &id_r); + if (id) + end = atoi(id); + else if (end < 0) + end = start; + for (cpu = start; cpu <= end; cpu++) + CPU_SET(cpu, cpuset); + } + next: + p = NULL; + } +out: + fclose(fp); +} + +int find_rt_cpu_number() { if(getenv("RTAPI_CPU_NUMBER")) return atoi(getenv("RTAPI_CPU_NUMBER")); #ifdef __linux__ - cpu_set_t cpuset_orig; - int r = sched_getaffinity(getpid(), sizeof(cpuset_orig), &cpuset_orig); - if(r < 0) - // if getaffinity fails, (it shouldn't be able to), just use CPU#0 - return 0; - + const char* isolated_file="/sys/devices/system/cpu/isolated"; cpu_set_t cpuset; - CPU_ZERO(&cpuset); - long top_probe = sysconf(_SC_NPROCESSORS_CONF); - // in old glibc versions, it was an error to pass to sched_setaffinity bits - // that are higher than an imagined/probed kernel-side CPU mask size. - // this caused the message - // sched_setaffinity: Invalid argument - // to be printed at startup, and the probed CPU would not take into - // account CPUs masked from this process by default (whether by - // isolcpus or taskset). By only setting bits up to the "number of - // processes configured", the call is successful on glibc versions such as - // 2.19 and older. - for(long i=0; i 1) { const static int rt_cpu_number = find_rt_cpu_number(); + rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); if(rt_cpu_number != -1) { #ifdef __FreeBSD__ cpuset_t cpuset; diff --git a/src/rtapi/uspace_xenomai.cc b/src/rtapi/uspace_xenomai.cc index ac28b2f7244..c3c6e432ea5 100644 --- a/src/rtapi/uspace_xenomai.cc +++ b/src/rtapi/uspace_xenomai.cc @@ -52,10 +52,7 @@ struct XenomaiApp : RtapiApp { task->pll_correction_limit = period_nsec / 100; task->pll_correction = 0; - cpu_set_t cpuset; - CPU_ZERO(&cpuset); int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); - CPU_SET(nprocs-1, &cpuset); // assumes processor numbers are contiguous int ret; pthread_attr_t attr; @@ -69,9 +66,17 @@ struct XenomaiApp : RtapiApp { return -ret; if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) return -ret; - if(nprocs > 1) - if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) - return -ret; + if(nprocs > 1){ + const static int rt_cpu_number = find_rt_cpu_number(); + rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); + if(rt_cpu_number != -1) { + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(rt_cpu_number, &cpuset); + if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) + return -ret; + } + } if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) return -ret; @@ -99,10 +104,12 @@ struct XenomaiApp : RtapiApp { } int task_pause(int task_id) { + (void)task_id; return -ENOSYS; } int task_resume(int task_id) { + (void)task_id; return -ENOSYS; } diff --git a/src/rtapi/uspace_xenomai_evl.cc b/src/rtapi/uspace_xenomai_evl.cc new file mode 100644 index 00000000000..b4f65f2f4c2 --- /dev/null +++ b/src/rtapi/uspace_xenomai_evl.cc @@ -0,0 +1,226 @@ +#include "config.h" +#include "rtapi.h" +#include "rtapi_uspace.hh" + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#ifdef HAVE_SYS_IO_H +#include +#endif + +namespace +{ +struct RtaiTask : rtapi_task { + RtaiTask() : rtapi_task{}, cancel{}, thr{} {} + std::atomic_int cancel; + pthread_t thr; +}; + + +struct XenomaiApp : RtapiApp { + XenomaiApp() : RtapiApp(SCHED_FIFO) { + pthread_once(&key_once, init_key); + } + + RtaiTask *do_task_new() { + return new RtaiTask; + } + + int task_delete(int id) { + auto task = ::rtapi_get_task(id); + if(!task) return -EINVAL; + + task->cancel = 1; + pthread_join(task->thr, nullptr); + task->magic = 0; + task_array[id] = 0; + delete task; + return 0; + } + + int task_start(int task_id, unsigned long period_nsec) { + auto task = ::rtapi_get_task(task_id); + if(!task) return -EINVAL; + + task->period = period_nsec; + struct sched_param param; + memset(¶m, 0, sizeof(param)); + param.sched_priority = task->prio; + + // limit PLL correction values to +/-1% of cycle time + task->pll_correction_limit = period_nsec / 100; + task->pll_correction = 0; + + int nprocs = sysconf( _SC_NPROCESSORS_ONLN ); + + int ret; + pthread_attr_t attr; + if((ret = pthread_attr_init(&attr)) != 0) + return -ret; + if((ret = pthread_attr_setstacksize(&attr, task->stacksize)) != 0) + return -ret; + if((ret = pthread_attr_setschedpolicy(&attr, policy)) != 0) + return -ret; + if((ret = pthread_attr_setschedparam(&attr, ¶m)) != 0) + return -ret; + if((ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0) + return -ret; + if(nprocs > 1){ + const static int rt_cpu_number = find_rt_cpu_number(); + rtapi_print_msg(RTAPI_MSG_INFO, "rt_cpu_number = %i\n", rt_cpu_number); + if(rt_cpu_number != -1) { + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + CPU_SET(rt_cpu_number, &cpuset); + if((ret = pthread_attr_setaffinity_np(&attr, sizeof(cpuset), &cpuset)) != 0) + return -ret; + } + } + if((ret = pthread_create(&task->thr, &attr, &wrapper, reinterpret_cast(task))) != 0) + return -ret; + + return 0; + } + + static void *wrapper(void *arg) { + auto task = reinterpret_cast(arg); + pthread_setspecific(key, arg); + + { + WithRoot r; + /* Attach to the core. */ + rtapi_print("linuxcnc-task:%d\n", gettid()); + int tfd = evl_attach_self("linuxcnc-thread:%d", gettid()); + if (tfd < 0){ + rtapi_print("evl_attach_self() failed ret %i errno %i\n", tfd, errno); + } + } + + struct timespec now; + evl_read_clock(EVL_CLOCK_MONOTONIC, &now); + + // originally, I used pthread_make_periodic_np here, and + // pthread_wait_np in wait(), but in about 1 run in 50 this led to + // "xenomai: watchdog triggered" and rtapi_app was killed. + // + // encountered on: 3.18.20-xenomai-2.6.5 with a 2-thread SMP system + rtapi_timespec_advance(task->nextstart, now, task->period + task->pll_correction); + + (task->taskcode) (task->arg); + + rtapi_print("ERROR: reached end of wrapper for task %d\n", task->id); + return nullptr; + } + + int task_pause(int task_id) { + (void)task_id; + return -ENOSYS; + } + + int task_resume(int task_id) { + (void)task_id; + return -ENOSYS; + } + + long long task_pll_get_reference(void) { + struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); + if(!task) return 0; + return task->nextstart.tv_sec * 1000000000LL + task->nextstart.tv_nsec; + } + + int task_pll_set_correction(long value) { + struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); + if(!task) return -EINVAL; + if (value > task->pll_correction_limit) value = task->pll_correction_limit; + if (value < -(task->pll_correction_limit)) value = -(task->pll_correction_limit); + task->pll_correction = value; + return 0; + } + + void wait() { + int task_id = task_self(); + auto task = ::rtapi_get_task(task_id); + if(task->cancel) { + pthread_exit(nullptr); + } + rtapi_timespec_advance(task->nextstart, task->nextstart, task->period + task->pll_correction); + struct timespec now; + evl_read_clock(EVL_CLOCK_MONOTONIC, &now); + if(rtapi_timespec_less(task->nextstart, now)) + { + if(policy == SCHED_FIFO) + unexpected_realtime_delay(task); + } + else + { + int res = evl_sleep_until(EVL_CLOCK_MONOTONIC, &task->nextstart); + if(res < 0) perror("evl_sleep_until"); + } + } + + unsigned char do_inb(unsigned int port) { +#ifdef HAVE_SYS_IO_H + return inb(port); +#else + return 0; +#endif + } + + void do_outb(unsigned char val, unsigned int port) { +#ifdef HAVE_SYS_IO_H + return outb(val, port); +#else + return 0; +#endif + } + + int run_threads(int fd, int (*callback)(int fd)) { + while(callback(fd)) { /* nothing */ } + return 0; + } + + int task_self() { + struct rtapi_task *task = reinterpret_cast(pthread_getspecific(key)); + if(!task) return -EINVAL; + return task->id; + } + + static pthread_once_t key_once; + static pthread_key_t key; + static void init_key(void) { + pthread_key_create(&key, NULL); + } + + long long do_get_time() { + struct timespec ts; + evl_read_clock(EVL_CLOCK_MONOTONIC, &ts); + return ts.tv_sec * 1000000000LL + ts.tv_nsec; + } + + void do_delay(long ns) { + struct timespec ts; + evl_read_clock(EVL_CLOCK_MONOTONIC, &ts); + rtapi_timespec_advance(ts, ts, ns); + evl_sleep_until(EVL_CLOCK_MONOTONIC, &ts); + } +}; + +pthread_once_t XenomaiApp::key_once; +pthread_key_t XenomaiApp::key; +} + +extern "C" RtapiApp *make(); + +RtapiApp *make() { + rtapi_print_msg(RTAPI_MSG_ERR, "Note: Using XENOMAI4 EVL realtime\n"); + return new XenomaiApp; +}