AP_HAL_Linux: cpu affinity

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -2,6 +2,7 @@
 
 #include <assert.h>
 #include <signal.h>
+#include <sched.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
@@ -296,6 +297,8 @@ void _usage(void)
     printf("\t                   --module-directory %s\n", AP_MODULE_DEFAULT_DIRECTORY);
     printf("\t                   -M %s\n", AP_MODULE_DEFAULT_DIRECTORY);
 #endif
+    printf("\tcpu affinity       --cpu-affinity 0,3 or 0,3\n");
+    printf("\t                   -c 0,3  or range -c 1,3\n");
 }
 
 void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
@@ -320,11 +323,12 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
         {"storage-directory",   true,  0, 's'},
         {"module-directory",    true,  0, 'M'},
         {"defaults",            true,  0, 'd'},
+        {"cpu-affinity",        true,  0, 'c'},
         {"help",                false,  0, 'h'},
         {0, false, 0, 0}
     };
 
-    GetOptLong gopt(argc, argv, "A:B:C:D:E:F:G:H:l:t:s:he:SM:",
+    GetOptLong gopt(argc, argv, "A:B:C:D:E:F:G:H:l:t:s:he:SM:c:",
                     options);
 
     /*
@@ -376,6 +380,13 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
         case 'd':
             utilInstance.set_custom_defaults_path(gopt.optarg);
             break;
+        case 'c':
+            cpu_set_t cpu_affinity;
+            if (!utilInstance.parse_cpu_set(gopt.optarg, &cpu_affinity)) {
+                exit(1);
+            }
+            Linux::Scheduler::from(scheduler)->set_cpu_affinity(cpu_affinity);
+            break;
         case 'h':
             _usage();
             exit(0);

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -58,7 +58,9 @@ extern const AP_HAL::HAL& hal;
     }
 
 Scheduler::Scheduler()
-{ }
+{
+    CPU_ZERO(&_cpu_affinity);
+}
 
 
 void Scheduler::init_realtime()
@@ -84,6 +86,17 @@ void Scheduler::init_realtime()
     }
 }
 
+void Scheduler::init_cpu_affinity()
+{
+    if (!CPU_COUNT(&_cpu_affinity)) {
+        return;
+    }
+
+    if (sched_setaffinity(0, sizeof(_cpu_affinity), &_cpu_affinity) != 0) {
+        AP_HAL::panic("Failed to set affinity for main process: %m");
+    }
+}
+
 void Scheduler::init()
 {
     int ret;
@@ -103,6 +116,7 @@ void Scheduler::init()
     _main_ctx = pthread_self();
 
     init_realtime();
+    init_cpu_affinity();
 
     /* set barrier to N + 1 threads: worker threads + main */
     unsigned n_threads = ARRAY_SIZE(sched_table) + 1;

libraries/AP_HAL_Linux/Scheduler.h

@@ -54,6 +54,12 @@ public:
      */
     bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
     
+    /*
+      set cpu affinity mask to be applied on initialization - setting it
+      later has no effect.
+     */
+    void set_cpu_affinity(const cpu_set_t &cpu_affinity) { _cpu_affinity = cpu_affinity; }
+
 private:
     class SchedulerThread : public PeriodicThread {
     public:
@@ -70,6 +76,8 @@ private:
 
     void     init_realtime();
 
+    void     init_cpu_affinity();
+
     void _wait_all_threads();
 
     void     _debug_stack();
@@ -108,6 +116,7 @@ private:
     pthread_t _main_ctx;
 
     Semaphore _io_semaphore;
+    cpu_set_t _cpu_affinity;
 };
 
 }

libraries/AP_HAL_Linux/Thread.cpp

@@ -25,7 +25,6 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
-
 #include "Scheduler.h"
 
 #define STACK_POISON 0xBEBACAFE

libraries/AP_HAL_Linux/Util.cpp

@@ -305,3 +305,44 @@ bool Util::get_random_vals(uint8_t* data, size_t size)
     close(dev_random);
     return true;
 }
+
+bool Util::parse_cpu_set(const char *str, cpu_set_t *cpu_set)  const
+{
+    unsigned long cpu1, cpu2;
+    char *endptr, sep;
+
+    CPU_ZERO(cpu_set);
+
+    do {
+        cpu1 = strtoul(str, &endptr, 10);
+        if (str == endptr) {
+            fprintf(stderr, "Invalid option for cpu-affinity: %s missing cpu number\n", str);
+            return false;
+        }
+
+        str = endptr + 1;
+        sep = *endptr;
+        if (sep == ',' || sep == '\0') {
+            CPU_SET(cpu1, cpu_set);
+            continue;
+        }
+
+        if (sep != '-') {
+            fprintf(stderr, "Invalid option for cpu-affinity: %s did you means separator - e.g. 1-3\n", str);
+            return false;
+        }
+
+        cpu2 = strtoul(str, &endptr, 10);
+        if (str == endptr) {
+            fprintf(stderr, "Invalid option for cpu-affinity: %s missing end cpu number\n", str);
+            return false;
+        }
+
+        str = endptr + 1;
+        for (; cpu1 <= cpu2; cpu1++) {
+            CPU_SET(cpu1, cpu_set);
+        }
+    } while (*endptr != '\0');
+
+    return true;
+}

libraries/AP_HAL_Linux/Util.h

@@ -58,6 +58,9 @@ public:
         return custom_defaults;
     }
 
+    /* Parse cpu set in the form 0,2 or 0-2 or 0:2*/
+    bool parse_cpu_set(const char *s, cpu_set_t *cpu_set) const;
+
     bool is_chardev_node(const char *path);
     void set_imu_temp(float current) override;
     void set_imu_target_temp(int8_t *target) override;