[hal] Set HAL Notifier thread as RT by default (#3482)

This PR gives the Notifier HAL thread RT priority 40 in RobotBase after
HAL initialization and before the user code is run. This drastically
improves scheduling jitter for TimedRobot's AddPeriodic() functions (in
3512's experience).

It's too risky to set user code as RT because badly behaved code
will lock up the Rio (potentially requiring safe mode to recover).

This needs the user program to be setuid admin to succeed.

hal/src/main/native/athena/Threads.cpp

@@ -6,9 +6,42 @@
 
 #include <pthread.h>
 #include <sched.h>
+#include <unistd.h>
+
+#include <cerrno>
+#include <cstdlib>
+#include <system_error>
+
+#include <fmt/format.h>
 
 #include "hal/Errors.h"
 
+namespace {
+class UidSetter {
+ public:
+  explicit UidSetter(uid_t uid) {
+    m_uid = geteuid();
+    if (uid == 0 && setuid(uid) == -1) {
+      throw std::system_error(errno, std::generic_category(),
+                              fmt::format("setuid({}) failed", uid));
+    } else if (uid != 0 && seteuid(uid) == -1) {
+      throw std::system_error(errno, std::generic_category(),
+                              fmt::format("seteuid({}) failed", uid));
+    }
+  }
+
+  ~UidSetter() noexcept(false) {
+    if (seteuid(m_uid) == -1) {
+      throw std::system_error(errno, std::generic_category(),
+                              fmt::format("seteuid({}) failed", m_uid));
+    }
+  }
+
+ private:
+  uid_t m_uid;
+};
+}  // namespace
+
 namespace hal::init {
 void InitializeThreads() {}
 }  // namespace hal::init
@@ -70,13 +103,21 @@ HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
     // Only need to set 0 priority for non RT thread
     sch.sched_priority = 0;
   }
-  if (pthread_setschedparam(*reinterpret_cast<const pthread_t*>(handle),
-                            scheduler, &sch)) {
-    *status = HAL_THREAD_PRIORITY_ERROR;
+
+  try {
+    UidSetter uidSetter{0};
+
+    if (pthread_setschedparam(*reinterpret_cast<const pthread_t*>(handle),
+                              scheduler, &sch)) {
+      *status = HAL_THREAD_PRIORITY_ERROR;
+      return false;
+    } else {
+      *status = 0;
+      return true;
+    }
+  } catch (const std::system_error& e) {
+    *status = HAL_SETUID_ERROR;
     return false;
-  } else {
-    *status = 0;
-    return true;
   }
 }
 

hal/src/main/native/cpp/jni/ThreadsJNI.cpp

@@ -26,7 +26,7 @@ Java_edu_wpi_first_hal_ThreadsJNI_getCurrentThreadPriority
   HAL_Bool isRT = false;
   auto ret = HAL_GetCurrentThreadPriority(&isRT, &status);
   CheckStatus(env, status);
-  return (jint)ret;
+  return static_cast<jint>(ret);
 }
 
 /*
@@ -42,7 +42,7 @@ Java_edu_wpi_first_hal_ThreadsJNI_getCurrentThreadIsRealTime
   HAL_Bool isRT = false;
   HAL_GetCurrentThreadPriority(&isRT, &status);
   CheckStatus(env, status);
-  return (jboolean)isRT;
+  return static_cast<jboolean>(isRT);
 }
 
 /*
@@ -56,9 +56,9 @@ Java_edu_wpi_first_hal_ThreadsJNI_setCurrentThreadPriority
 {
   int32_t status = 0;
   auto ret = HAL_SetCurrentThreadPriority(
-      (HAL_Bool)realTime, static_cast<int32_t>(priority), &status);
-  CheckStatus(env, status);
-  return (jboolean)ret;
+      static_cast<HAL_Bool>(realTime), static_cast<int32_t>(priority), &status);
+  CheckStatus(env, status, false);
+  return static_cast<jboolean>(ret);
 }
 
 }  // extern "C"

hal/src/main/native/include/hal/Errors.h

@@ -135,6 +135,10 @@
 #define HAL_USE_LAST_ERROR_MESSAGE \
   "HAL: Use HAL_GetLastError(status) to get last error"
 
+#define HAL_SETUID_ERROR -1157
+#define HAL_SETUID_ERROR_MESSAGE \
+  "HAL: Setting the effective user ID has failed";
+
 #define HAL_CAN_BUFFER_OVERRUN -35007
 #define HAL_CAN_BUFFER_OVERRUN_MESSAGE \
   "HAL: CAN Output Buffer Full. Ensure a device is attached"

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -20,6 +20,7 @@
 #include "WPILibVersion.h"
 #include "frc/DriverStation.h"
 #include "frc/Errors.h"
+#include "frc/Notifier.h"
 #include "frc/RobotState.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -35,6 +36,12 @@ int frc::RunHALInitialization() {
   }
   HAL_Report(HALUsageReporting::kResourceType_Language,
              HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
+
+  if (!frc::Notifier::SetHALThreadPriority(true, 40)) {
+    FRC_ReportError(warn::Warning, "{}",
+                    "Setting HAL Notifier RT priority to 40 failed\n");
+  }
+
   std::puts("\n********** Robot program starting **********");
   return 0;
 }

wpilibc/src/main/native/include/frc/Threads.h

@@ -20,7 +20,7 @@ namespace frc {
 int GetThreadPriority(std::thread& thread, bool* isRealTime);
 
 /**
- * Get the thread priority for the current thread
+ * Get the thread priority for the current thread.
  *
  * @param isRealTime Set to true if thread is real-time, otherwise false.
  * @return           The current thread priority. For real-time, this is 1-99
@@ -30,7 +30,7 @@ int GetThreadPriority(std::thread& thread, bool* isRealTime);
 int GetCurrentThreadPriority(bool* isRealTime);
 
 /**
- * Sets the thread priority for the specified thread
+ * Sets the thread priority for the specified thread.
  *
  * @param thread   Reference to the thread to set the priority of.
  * @param realTime Set to true to set a real-time priority, false for standard
@@ -43,7 +43,7 @@ int GetCurrentThreadPriority(bool* isRealTime);
 bool SetThreadPriority(std::thread& thread, bool realTime, int priority);
 
 /**
- * Sets the thread priority for the current thread
+ * Sets the thread priority for the current thread.
  *
  * @param realTime Set to true to set a real-time priority, false for standard
  *                 priority.

wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+
+#include "frc/Threads.h"
+#include "gtest/gtest.h"
+
+TEST(PriorityTest, SetThreadPriority) {
+  bool exited = false;
+  wpi::condition_variable cond;
+  wpi::mutex mutex;
+  std::thread thr{[&] {
+    std::unique_lock lock{mutex};
+    cond.wait(lock, [&] { return exited; });
+  }};
+
+  // Non-RT thread has priority of zero
+  bool isRealTime = false;
+  EXPECT_EQ(0, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  // Set thread priority to 15
+  EXPECT_TRUE(frc::SetThreadPriority(thr, true, 15));
+
+  // Verify thread was given priority 15
+  EXPECT_EQ(15, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_TRUE(isRealTime);
+
+  // Set thread back to non-RT
+  EXPECT_TRUE(frc::SetThreadPriority(thr, false, 0));
+
+  // Verify thread is now non-RT
+  EXPECT_EQ(0, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  {
+    std::scoped_lock lock{mutex};
+    exited = true;
+  }
+  cond.notify_one();
+  thr.join();
+}
+
+TEST(PriorityTest, SetCurrentThreadPriority) {
+  // Non-RT thread has priority of zero
+  bool isRealTime = true;
+  EXPECT_EQ(0, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  // Set thread priority to 15
+  EXPECT_TRUE(frc::SetCurrentThreadPriority(true, 15));
+
+  // Verify thread was given priority 15
+  EXPECT_EQ(15, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_TRUE(isRealTime);
+
+  // Set thread back to non-RT
+  EXPECT_TRUE(frc::SetCurrentThreadPriority(false, 0));
+
+  // Verify thread is now non-RT
+  EXPECT_EQ(0, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_FALSE(isRealTime);
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -422,6 +422,10 @@ public abstract class RobotBase implements AutoCloseable {
     HAL.report(
         tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0, WPILibVersion.Version);
 
+    if (!Notifier.setHALThreadPriority(true, 40)) {
+      DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
+    }
+
     if (HAL.hasMain()) {
       Thread thread =
           new Thread(

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
+
+/** Tests to see if the thread and process priority functions work. */
+public class PriorityTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(PriorityTest.class.getName());
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  @Test
+  public void testSetCurrentThreadPriority() {
+    // Non-RT thread has priority of zero
+    assertEquals(0, Threads.getCurrentThreadPriority());
+    assertFalse(Threads.getCurrentThreadIsRealTime());
+
+    // Set thread priority to 15
+    assertTrue(Threads.setCurrentThreadPriority(true, 15));
+
+    // Verify thread was given priority 15
+    assertEquals(15, Threads.getCurrentThreadPriority());
+    assertTrue(Threads.getCurrentThreadIsRealTime());
+
+    // Set thread back to non-RT
+    assertTrue(Threads.setCurrentThreadPriority(false, 0));
+
+    // Verify thread is now non-RT
+    assertEquals(0, Threads.getCurrentThreadPriority());
+    assertFalse(Threads.getCurrentThreadIsRealTime());
+  }
+}