Merge pull request #3903 from hdiethelm/xenomai4

Xenomai4 Userspace + other fixes

Author: andypugh

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@

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,

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

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<T*>(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))

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() {

src/rtapi/uspace_rtai.cc

@@ -81,13 +81,18 @@ struct RtaiApp : RtapiApp {
     static void *wrapper(void *arg) {
         auto task = reinterpret_cast<RtaiTask*>(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
     }

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<top_probe && i<CPU_SETSIZE; i++) CPU_SET(i, &cpuset);
-
-    r = sched_setaffinity(getpid(), sizeof(cpuset), &cpuset);
-    if(r < 0)
-        // if setaffinity fails, (it shouldn't be able to), go on with
-        // whatever the default CPUs were.
-        perror("sched_setaffinity");
-
-    r = sched_getaffinity(getpid(), sizeof(cpuset), &cpuset);
-    if(r < 0) {
-        // if getaffinity fails, (it shouldn't be able to), copy the
-        // original affinity list in and use it
-        perror("sched_getaffinity");
-        CPU_AND(&cpuset, &cpuset_orig, &cpuset);
+
+    parse_cpu_list(isolated_file, &cpuset);
+
+    //Print list
+    rtapi_print_msg(RTAPI_MSG_INFO, "cpuset isolated ");
+    for(int i=0; i<CPU_SETSIZE; i++) {
+        if(CPU_ISSET(i, &cpuset)){
+            rtapi_print_msg(RTAPI_MSG_INFO, "%i ", i);
+        }
     }
+    rtapi_print_msg(RTAPI_MSG_INFO, "\n");
 
     int top = -1;
     for(int i=0; i<CPU_SETSIZE; i++) {
         if(CPU_ISSET(i, &cpuset)) top = i;
     }
+    if(top == -1){
+        rtapi_print_msg(RTAPI_MSG_ERR, "No isolated CPU's found, expect some latency or set RTAPI_CPU_NUMBER to select CPU\n");
+    }
     return top;
 #else
     return (-1);
@@ -1091,6 +1128,7 @@ int Posix::task_start(int task_id, unsigned long int period_nsec)
       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) {
 #ifdef __FreeBSD__
           cpuset_t cpuset;

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<void*>(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;
     }