From 5d9ae3cdb4a4595704efce41c4f3477367673580 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 14 Aug 2021 11:42:35 -0700 Subject: [PATCH] [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 | 53 +++++++++++++-- hal/src/main/native/cpp/jni/ThreadsJNI.cpp | 10 +-- hal/src/main/native/include/hal/Errors.h | 4 ++ wpilibc/src/main/native/cppcs/RobotBase.cpp | 7 ++ wpilibc/src/main/native/include/frc/Threads.h | 6 +- .../src/main/native/cpp/PriorityTest.cpp | 68 +++++++++++++++++++ .../java/edu/wpi/first/wpilibj/RobotBase.java | 4 ++ .../edu/wpi/first/wpilibj/PriorityTest.java | 44 ++++++++++++ 8 files changed, 182 insertions(+), 14 deletions(-) create mode 100644 wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp create mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java diff --git a/hal/src/main/native/athena/Threads.cpp b/hal/src/main/native/athena/Threads.cpp index fb52c208a6..d60b2800a7 100644 --- a/hal/src/main/native/athena/Threads.cpp +++ b/hal/src/main/native/athena/Threads.cpp @@ -6,9 +6,42 @@ #include #include +#include + +#include +#include +#include + +#include #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(handle), - scheduler, &sch)) { - *status = HAL_THREAD_PRIORITY_ERROR; + + try { + UidSetter uidSetter{0}; + + if (pthread_setschedparam(*reinterpret_cast(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; } } diff --git a/hal/src/main/native/cpp/jni/ThreadsJNI.cpp b/hal/src/main/native/cpp/jni/ThreadsJNI.cpp index b25a15d7ed..94f00c5b68 100644 --- a/hal/src/main/native/cpp/jni/ThreadsJNI.cpp +++ b/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(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(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(priority), &status); - CheckStatus(env, status); - return (jboolean)ret; + static_cast(realTime), static_cast(priority), &status); + CheckStatus(env, status, false); + return static_cast(ret); } } // extern "C" diff --git a/hal/src/main/native/include/hal/Errors.h b/hal/src/main/native/include/hal/Errors.h index ab54576945..316c4b8fbd 100644 --- a/hal/src/main/native/include/hal/Errors.h +++ b/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" diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 5471277c3a..ccb1d02634 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/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; } diff --git a/wpilibc/src/main/native/include/frc/Threads.h b/wpilibc/src/main/native/include/frc/Threads.h index 8c8c087152..5822670733 100644 --- a/wpilibc/src/main/native/include/frc/Threads.h +++ b/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. diff --git a/wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp new file mode 100644 index 0000000000..3085bed3f4 --- /dev/null +++ b/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 + +#include +#include + +#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); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 6c4b39815d..96c39b51ab 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/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( diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java new file mode 100644 index 0000000000..7c3455d3b2 --- /dev/null +++ b/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()); + } +}