From 9f1cbdc0a5faf78a1e73cf7f02b257de2c88cbc2 Mon Sep 17 00:00:00 2001 From: HefnySco Date: Fri, 17 Sep 2021 05:28:40 +0200 Subject: [PATCH] AP_HAL_Linux: cpu affinity --- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 13 ++++++- libraries/AP_HAL_Linux/Scheduler.cpp | 16 ++++++++- libraries/AP_HAL_Linux/Scheduler.h | 9 +++++ libraries/AP_HAL_Linux/Thread.cpp | 1 - libraries/AP_HAL_Linux/Util.cpp | 41 ++++++++++++++++++++++ libraries/AP_HAL_Linux/Util.h | 3 ++ 6 files changed, 80 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 311c98ca37..65cd40ca14 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -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); diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 4c0e7020ab..82ed1dc28f 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/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; diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index 4e0f756476..2f0b45577f 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/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; }; } diff --git a/libraries/AP_HAL_Linux/Thread.cpp b/libraries/AP_HAL_Linux/Thread.cpp index 806fb8c5fa..17f8d2f5e9 100644 --- a/libraries/AP_HAL_Linux/Thread.cpp +++ b/libraries/AP_HAL_Linux/Thread.cpp @@ -25,7 +25,6 @@ #include #include - #include "Scheduler.h" #define STACK_POISON 0xBEBACAFE diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 21de13b9ee..be8ed44505 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/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; +} \ No newline at end of file diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 607426964e..e800570461 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/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;