EVL: Fix cpu assignement for posix/lxrt/xenomai/evl

Author: hdiethelm

src/rtapi/rtapi_uspace.hh

@@ -106,6 +106,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_rtai.cc

@@ -85,9 +85,11 @@ struct RtaiApp : RtapiApp {
         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));
+        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);
+            rt_set_runnable_on_cpus(task->rt_task, 1u << rt_cpu_number);
+        }
         rt_make_hard_real_time();
         rt_task_make_periodic_relative_ns(task->rt_task, task->period, task->period);
         (task->taskcode) (task->arg);

src/rtapi/uspace_rtapi_app.cc

@@ -1013,48 +1013,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/master/tests/helpers.c?ref_type=heads
+//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!");
+    }
     return top;
 #else
     return (-1);
@@ -1094,6 +1118,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;
 

src/rtapi/uspace_xenomai_evl.cc

@@ -60,10 +60,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;
@@ -77,9 +74,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;