From 25cfa2b65a546ae344ee78ed3154ac67ea5ee2bc Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Mon, 18 Oct 2021 13:21:24 -0700
Subject: [PATCH] tici: affine loggerd to efficiency cores (#22581)

old-commit-hash: 84de248fa7cbee37ecb2fd6a9d4df50ba60a1e04
---
 selfdrive/boardd/boardd.cc       |  2 +-
 selfdrive/camerad/main.cc        | 11 +++++------
 selfdrive/common/util.cc         | 12 +++++++-----
 selfdrive/common/util.h          |  3 ++-
 selfdrive/debug/check_timings.py |  2 +-
 selfdrive/loggerd/loggerd.cc     | 11 ++++++++++-
 selfdrive/modeld/modeld.cc       | 11 +++++------
 7 files changed, 31 insertions(+), 21 deletions(-)

diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 0ec94c187b..edf5e5e8e2 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -564,7 +564,7 @@ int main() {
   int err = set_realtime_priority(54);
   LOG("set priority returns %d", err);
 
-  err = set_core_affinity(Hardware::TICI() ? 4 : 3);
+  err = set_core_affinity({Hardware::TICI() ? 4 : 3});
   LOG("set affinity returns %d", err);
 
   LOGW("attempting to connect");
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index b4f5ff796d..1f90b417b4 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -44,12 +44,11 @@ void party(cl_device_id device_id, cl_context context) {
 #endif
 
 int main(int argc, char *argv[]) {
-  set_realtime_priority(53);
-  if (Hardware::EON()) {
-    set_core_affinity(2);
-  } else if (Hardware::TICI()) {
-    set_core_affinity(6);
-  }
+  int ret;
+  ret = set_realtime_priority(53);
+  assert(ret == 0);
+  ret = set_core_affinity({Hardware::EON() ? 2 : 6});
+  assert(ret == 0);
 
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
 
diff --git a/selfdrive/common/util.cc b/selfdrive/common/util.cc
index 99cf2ae887..cd00b626b1 100644
--- a/selfdrive/common/util.cc
+++ b/selfdrive/common/util.cc
@@ -40,14 +40,16 @@ int set_realtime_priority(int level) {
 #endif
 }
 
-int set_core_affinity(int core) {
+int set_core_affinity(std::vector<int> cores) {
 #ifdef __linux__
   long tid = syscall(SYS_gettid);
-  cpu_set_t rt_cpu;
+  cpu_set_t cpu;
 
-  CPU_ZERO(&rt_cpu);
-  CPU_SET(core, &rt_cpu);
-  return sched_setaffinity(tid, sizeof(rt_cpu), &rt_cpu);
+  CPU_ZERO(&cpu);
+  for (const int n : cores) {
+    CPU_SET(n, &cpu);
+  }
+  return sched_setaffinity(tid, sizeof(cpu), &cpu);
 #else
   return -1;
 #endif
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index 987bf95a9d..d3d2a2e43d 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -12,6 +12,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <vector>
 
 // keep trying if x gets interrupted by a signal
 #define HANDLE_EINTR(x)                                       \
@@ -31,7 +32,7 @@ typedef void (*sighandler_t)(int sig);
 void set_thread_name(const char* name);
 
 int set_realtime_priority(int level);
-int set_core_affinity(int core);
+int set_core_affinity(std::vector<int> cores);
 
 namespace util {
 
diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py
index 8b289138d3..03e39fd70d 100755
--- a/selfdrive/debug/check_timings.py
+++ b/selfdrive/debug/check_timings.py
@@ -20,5 +20,5 @@ if __name__ == "__main__":
 
       if len(ts[s]):
         d = np.diff(ts[s])
-        print(f"{s:17} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}")
+        print(f"{s:25} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}")
     time.sleep(1)
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index d0f0ac1836..1d3a005ffa 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -299,7 +299,16 @@ void rotate_if_needed() {
 } // namespace
 
 int main(int argc, char** argv) {
-  setpriority(PRIO_PROCESS, 0, -20);
+  if (Hardware::EON()) {
+    setpriority(PRIO_PROCESS, 0, -20);
+  } else {
+    int ret;
+    ret = set_core_affinity({0, 1, 2, 3});
+    assert(ret == 0);
+    // TODO: why does this impact camerad timings?
+    //ret = set_realtime_priority(1);
+    //assert(ret == 0);
+  }
 
   clear_locks();
 
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 460e33c561..ef9d59bf35 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -131,13 +131,12 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
 }
 
 int main(int argc, char **argv) {
-  set_realtime_priority(54);
+  int ret;
+  ret = set_realtime_priority(54);
+  assert(ret == 0);
+  set_core_affinity({Hardware::EON() ? 2 : 7});
+  assert(ret == 0);
 
-  if (Hardware::EON()) {
-    set_core_affinity(2);
-  } else if (Hardware::TICI()) {
-    set_core_affinity(7);  
-  }
   bool wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
 
   // start calibration thread