Add priority_mutex and priority_condition_variable. (#50)

Also provide wpi::mutex and wpi::condition_variable as wrappers for these
on Linux (where they're available), and for std::mutex and
std::condition_variable on other platforms.

src/main/native/include/support/condition_variable.h

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <condition_variable>
+
+#include "priority_condition_variable.h"
+
+namespace wpi {
+
+#ifdef WPI_HAVE_PRIORITY_CONDITION_VARIABLE
+using condition_variable = priority_condition_variable;
+#else
+using condition_variable = ::std::condition_variable;
+#endif
+
+}  // namespace wpi

src/main/native/include/support/mutex.h

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <mutex>
+
+#include "priority_mutex.h"
+
+namespace wpi {
+
+#ifdef WPI_HAVE_PRIORITY_MUTEX
+using mutex = priority_mutex;
+using recursive_mutex = priority_recursive_mutex;
+#else
+using mutex = ::std::mutex;
+using recursive_mutex = ::std::recursive_mutex;
+#endif
+
+}  // namespace wpi

src/main/native/include/support/priority_condition_variable.h

@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <condition_variable>
+#include <memory>
+#include <utility>
+
+#ifdef __linux__
+#include <pthread.h>
+#endif
+
+#include "priority_mutex.h"
+
+namespace wpi {
+
+#if defined(__linux__) && defined(WPI_HAVE_PRIORITY_MUTEX)
+
+#define WPI_HAVE_PRIORITY_CONDITION_VARIABLE 1
+
+class priority_condition_variable {
+  typedef std::chrono::system_clock clock;
+
+ public:
+  typedef pthread_cond_t* native_handle_type;
+
+  priority_condition_variable() noexcept = default;
+  ~priority_condition_variable() noexcept { pthread_cond_destroy(&m_cond); }
+
+  priority_condition_variable(const priority_condition_variable&) = delete;
+  priority_condition_variable& operator=(const priority_condition_variable&) =
+      delete;
+
+  void notify_one() noexcept {
+    pthread_cond_signal(&m_cond);
+  }
+
+  void notify_all() noexcept {
+    pthread_cond_broadcast(&m_cond);
+  }
+
+  void wait(std::unique_lock<priority_mutex>& lock) noexcept {
+    int e = pthread_cond_wait(&m_cond, lock.mutex()->native_handle());
+    if (e) std::terminate();
+  }
+
+  template <typename Predicate>
+  void wait(std::unique_lock<priority_mutex>& lock, Predicate p) {
+    while (!p()) {
+      wait(lock);
+    }
+  }
+
+  template <typename Duration>
+  std::cv_status wait_until(
+      std::unique_lock<priority_mutex>& lock,
+      const std::chrono::time_point<clock, Duration>& atime) {
+    return wait_until_impl(lock, atime);
+  }
+
+  template <typename Clock, typename Duration>
+  std::cv_status wait_until(
+      std::unique_ptr<priority_mutex>& lock,
+      const std::chrono::time_point<Clock, Duration>& atime) {
+    const typename Clock::time_point c_entry = Clock::now();
+    const clock::time_point s_entry = clock::now();
+    const auto delta = atime - c_entry;
+    const auto s_atime = s_entry + delta;
+
+    return wait_until_impl(lock, s_atime);
+  }
+
+  template <typename Clock, typename Duration, typename Predicate>
+  bool wait_until(std::unique_lock<priority_mutex>& lock,
+                  const std::chrono::time_point<Clock, Duration>& atime,
+                  Predicate p) {
+    while (!p()) {
+      if (wait_until(lock, atime) == std::cv_status::timeout) {
+        return p();
+      }
+    }
+    return true;
+  }
+
+  template <typename Rep, typename Period>
+  std::cv_status wait_for(std::unique_lock<priority_mutex>& lock,
+                          const std::chrono::duration<Rep, Period>& rtime) {
+    return wait_until(lock, clock::now() + rtime);
+  }
+
+  template <typename Rep, typename Period, typename Predicate>
+  bool wait_for(std::unique_lock<priority_mutex>& lock,
+                const std::chrono::duration<Rep, Period>& rtime, Predicate p) {
+    return wait_until(lock, clock::now() + rtime, std::move(p));
+  }
+
+  native_handle_type native_handle() { return &m_cond; }
+
+ private:
+  pthread_cond_t m_cond = PTHREAD_COND_INITIALIZER;
+
+  template <typename Dur>
+  std::cv_status wait_until_impl(
+      std::unique_lock<priority_mutex>& lock,
+      const std::chrono::time_point<clock, Dur>& atime) {
+    auto s = std::chrono::time_point_cast<std::chrono::seconds>(atime);
+    auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(atime - s);
+
+    struct timespec ts = {
+        static_cast<std::time_t>(s.time_since_epoch().count()),
+        static_cast<long>(ns.count())};
+
+    pthread_cond_timedwait(&m_cond, lock.mutex()->native_handle(), &ts);
+
+    return (clock::now() < atime ? std::cv_status::no_timeout
+                                 : std::cv_status::timeout);
+  }
+};
+#endif
+
+}  // namespace wpi

src/main/native/include/support/priority_mutex.h

@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+// Allows usage with std::lock_guard without including <mutex> separately
+#include <mutex>
+
+#ifdef __linux__
+#include <pthread.h>
+#endif
+
+namespace wpi {
+
+#ifdef __linux__
+
+#define WPI_HAVE_PRIORITY_MUTEX 1
+
+class priority_recursive_mutex {
+ public:
+  typedef pthread_mutex_t* native_handle_type;
+
+  constexpr priority_recursive_mutex() noexcept = default;
+  priority_recursive_mutex(const priority_recursive_mutex&) = delete;
+  priority_recursive_mutex& operator=(const priority_recursive_mutex&) = delete;
+
+  // Lock the mutex, blocking until it's available.
+  void lock() { pthread_mutex_lock(&m_mutex); }
+
+  // Unlock the mutex.
+  void unlock() { pthread_mutex_unlock(&m_mutex); }
+
+  // Tries to lock the mutex.
+  bool try_lock() noexcept { return !pthread_mutex_trylock(&m_mutex); }
+
+  pthread_mutex_t* native_handle() { return &m_mutex; }
+
+ private:
+// Do the equivalent of setting PTHREAD_PRIO_INHERIT and
+// PTHREAD_MUTEX_RECURSIVE_NP.
+#ifdef __PTHREAD_MUTEX_HAVE_PREV
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, __PTHREAD_SPINS, {0, 0}}};
+#else
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, {__PTHREAD_SPINS}}};
+#endif
+};
+
+class priority_mutex {
+ public:
+  typedef pthread_mutex_t* native_handle_type;
+
+  constexpr priority_mutex() noexcept = default;
+  priority_mutex(const priority_mutex&) = delete;
+  priority_mutex& operator=(const priority_mutex&) = delete;
+
+  // Lock the mutex, blocking until it's available.
+  void lock() { pthread_mutex_lock(&m_mutex); }
+
+  // Unlock the mutex.
+  void unlock() { pthread_mutex_unlock(&m_mutex); }
+
+  // Tries to lock the mutex.
+  bool try_lock() noexcept { return !pthread_mutex_trylock(&m_mutex); }
+
+  pthread_mutex_t* native_handle() { return &m_mutex; }
+
+ private:
+// Do the equivalent of setting PTHREAD_PRIO_INHERIT.
+#ifdef __PTHREAD_MUTEX_HAVE_PREV
+  pthread_mutex_t m_mutex = {{0, 0, 0, 0, 0x20, __PTHREAD_SPINS, {0, 0}}};
+#else
+  pthread_mutex_t m_mutex = {{0, 0, 0, 0x20, 0, {__PTHREAD_SPINS}}};
+#endif
+};
+
+#endif  // __linux__
+
+}  // namespace wpi

src/test/native/cpp/priority_condition_variable_test.cpp

@@ -0,0 +1,300 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
+#include <support/priority_condition_variable.h>
+#include <support/priority_mutex.h>
+
+#include "gtest/gtest.h"
+
+namespace wpi {
+
+#ifdef WPI_HAVE_PRIORITY_CONDITION_VARIABLE
+
+// Tests that the condition variable class which we wrote ourselves actually
+// does work.
+class ConditionVariableTest : public ::testing::Test {
+ protected:
+  typedef std::unique_lock<priority_mutex> priority_lock;
+
+  // Condition variable to test.
+  priority_condition_variable m_cond;
+
+  // Mutex to pass to condition variable when waiting.
+  priority_mutex m_mutex;
+
+  // flags for testing when threads are completed.
+  std::atomic<bool> m_done1{false}, m_done2{false};
+  // Threads to use for testing. We want multiple threads to ensure that it
+  // behaves correctly when multiple processes are waiting on a signal.
+  std::thread m_watcher1, m_watcher2;
+
+  // Information for when running with predicates.
+  std::atomic<bool> m_pred_var{false};
+
+  void ShortSleep(uint32_t time = 10) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(time));
+  }
+
+  // Start up the given threads with a wait function. The wait function should
+  // call some version of m_cond.wait and should take as an argument a reference
+  // to an std::atomic<bool> which it will set to true when it is ready to have
+  // join called on it.
+  template <class Function>
+  void StartThreads(Function wait) {
+    m_watcher1 = std::thread(wait, std::ref(m_done1));
+    m_watcher2 = std::thread(wait, std::ref(m_done2));
+
+    // Wait briefly to let the lock be unlocked.
+    ShortSleep();
+    bool locked = m_mutex.try_lock();
+    if (locked) m_mutex.unlock();
+    EXPECT_TRUE(locked) << "The condition variable failed to unlock the lock.";
+  }
+
+  void NotifyAll() { m_cond.notify_all(); }
+  void NotifyOne() { m_cond.notify_one(); }
+
+  // Test that all the threads are notified by a notify_all() call.
+  void NotifyAllTest() {
+    NotifyAll();
+    // Wait briefly to let the lock be re-locked.
+    ShortSleep();
+    EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+    EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+  }
+
+  // For use when testing predicates. First tries signalling the threads with
+  // the predicate set to false (and ensures that they do not activate) and then
+  // tests with the predicate set to true.
+  void PredicateTest() {
+    m_pred_var = false;
+    NotifyAll();
+    ShortSleep();
+    EXPECT_FALSE(m_done1) << "watcher1 didn't pay attention to its predicate.";
+    EXPECT_FALSE(m_done2) << "watcher2 didn't pay attention to its predicate.";
+    m_pred_var = true;
+    NotifyAllTest();
+  }
+
+  // Used by the WaitFor and WaitUntil tests to test that, without a predicate,
+  // the timeout works properly.
+  void WaitTimeTest(bool wait_for) {
+    std::atomic<bool> timed_out{true};
+    auto wait_until = [this, &timed_out, wait_for](std::atomic<bool>& done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        timed_out = m_cond.wait_for(lock, wait_time) == std::cv_status::timeout;
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        timed_out =
+            m_cond.wait_until(lock, wait_time) == std::cv_status::timeout;
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // First, test without timing out.
+    timed_out = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_FALSE(timed_out) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Next, test and time out.
+    timed_out = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have timed out.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have timed out.";
+    EXPECT_TRUE(timed_out) << "The watcher should have timed out.";
+  }
+
+  // For use with tests that have a timeout and a predicate.
+  void WaitTimePredicateTest(bool wait_for) {
+    // The condition_variable return value from the wait_for or wait_until
+    // function should in the case of having a predicate, by a boolean. If the
+    // predicate is true, then the return value will always be true. If the
+    // condition times out and, at the time of the timeout, the predicate is
+    // false, the return value will be false.
+    std::atomic<bool> retval{true};
+    auto predicate = [this]() -> bool { return m_pred_var; };
+    auto wait_until = [this, &retval, predicate,
+                       wait_for](std::atomic<bool>& done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        retval = m_cond.wait_for(lock, wait_time, predicate);
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        retval = m_cond.wait_until(lock, wait_time, predicate);
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // Test without timing out and with the predicate set to true.
+    retval = true;
+    m_pred_var = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_TRUE(retval) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Test with timing out and with the predicate set to true.
+    retval = false;
+    m_pred_var = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have finished.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have finished.";
+    EXPECT_FALSE(retval) << "The watcher should have timed out.";
+
+    TearDown();
+
+    // Test without timing out and run the PredicateTest().
+    retval = false;
+    StartThreads(wait_until);
+
+    PredicateTest();
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+
+    TearDown();
+
+    // Test with timing out and the predicate set to true while we are waiting
+    // for the condition variable to time out.
+    retval = true;
+    StartThreads(wait_until);
+    ShortSleep();
+    m_pred_var = true;
+    ShortSleep(110);
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+  }
+
+  virtual void TearDown() {
+    // If a thread has not completed, then continuing will cause the tests to
+    // hang forever and could cause issues. If we don't call detach, then
+    // std::terminate is called and all threads are terminated.
+    // Detaching is non-optimal, but should allow the rest of the tests to run
+    // before anything drastic occurs.
+    if (m_done1)
+      m_watcher1.join();
+    else
+      m_watcher1.detach();
+    if (m_done2)
+      m_watcher2.join();
+    else
+      m_watcher2.detach();
+  }
+};
+
+TEST_F(ConditionVariableTest, NotifyAll) {
+  auto wait = [this](std::atomic<bool>& done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyAllTest();
+}
+
+TEST_F(ConditionVariableTest, NotifyOne) {
+  auto wait = [this](std::atomic<bool>& done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyOne();
+  // Wait briefly to let things settle.
+  ShortSleep();
+  EXPECT_TRUE(m_done1 ^ m_done2) << "Only one thread should've been notified.";
+  NotifyOne();
+  ShortSleep();
+  EXPECT_TRUE(m_done2 && m_done2) << "Both threads should've been notified.";
+}
+
+TEST_F(ConditionVariableTest, WaitWithPredicate) {
+  auto predicate = [this]() -> bool { return m_pred_var; };
+  auto wait_predicate = [this, predicate](std::atomic<bool>& done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock, predicate);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait_predicate);
+
+  PredicateTest();
+}
+
+TEST_F(ConditionVariableTest, WaitUntil) { WaitTimeTest(false); }
+
+TEST_F(ConditionVariableTest, WaitUntilWithPredicate) {
+  WaitTimePredicateTest(false);
+}
+
+TEST_F(ConditionVariableTest, WaitFor) { WaitTimeTest(true); }
+
+TEST_F(ConditionVariableTest, WaitForWithPredicate) {
+  WaitTimePredicateTest(true);
+}
+
+TEST_F(ConditionVariableTest, NativeHandle) {
+  auto wait = [this](std::atomic<bool>& done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  pthread_cond_t* native_handle = m_cond.native_handle();
+  pthread_cond_broadcast(native_handle);
+  ShortSleep();
+  EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+  EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+}
+
+#endif  // WPI_HAVE_PRIORITY_CONDITION_VARIABLE
+
+}  // namespace wpi

src/test/native/cpp/priority_mutex_test.cpp

@@ -0,0 +1,271 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2017 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <support/priority_mutex.h>  // NOLINT(build/include_order)
+
+#include <atomic>
+#include <condition_variable>
+#include <thread>
+
+#include "gtest/gtest.h"
+
+namespace wpi {
+
+#ifdef WPI_HAVE_PRIORITY_MUTEX
+
+using std::chrono::system_clock;
+
+// Threading primitive used to notify many threads that a condition is now true.
+// The condition can not be cleared.
+class Notification {
+ public:
+  // Efficiently waits until the Notification has been notified once.
+  void Wait() {
+    std::unique_lock<priority_mutex> lock(m_mutex);
+    while (!m_set) {
+      m_condition.wait(lock);
+    }
+  }
+  // Sets the condition to true, and wakes all waiting threads.
+  void Notify() {
+    std::lock_guard<priority_mutex> lock(m_mutex);
+    m_set = true;
+    m_condition.notify_all();
+  }
+
+ private:
+  // priority_mutex used for the notification and to protect the bit.
+  priority_mutex m_mutex;
+  // Condition for threads to sleep on.
+  std::condition_variable_any m_condition;
+  // Bool which is true when the notification has been notified.
+  bool m_set = false;
+};
+
+void SetProcessorAffinity(int32_t core_id) {
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(core_id, &cpuset);
+
+  pthread_t current_thread = pthread_self();
+  ASSERT_EQ(pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset),
+            0);
+}
+
+void SetThreadRealtimePriorityOrDie(int32_t priority) {
+  struct sched_param param;
+  // Set realtime priority for this thread
+  param.sched_priority = priority + sched_get_priority_min(SCHED_RR);
+  ASSERT_EQ(pthread_setschedparam(pthread_self(), SCHED_RR, &param), 0)
+      << ": Failed to set scheduler priority.";
+}
+
+// This thread holds the mutex and spins until signaled to release it and stop.
+template <typename MutexType>
+class LowPriorityThread {
+ public:
+  explicit LowPriorityThread(MutexType* mutex)
+      : m_mutex(mutex), m_hold_mutex(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(20);
+    m_mutex->lock();
+    m_started.Notify();
+    while (m_hold_mutex.test_and_set()) {
+    }
+    m_mutex->unlock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  void release_mutex() { m_hold_mutex.clear(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // priority_mutex to grab and release.
+  MutexType* m_mutex;
+  Notification m_started;
+  // Atomic type used to signal when the thread should unlock the mutex.
+  // Using a mutex to protect something could cause other issues, and a delay
+  // between setting and reading isn't a problem as long as the set is atomic.
+  std::atomic_flag m_hold_mutex;
+  std::atomic<int> m_success;
+};
+
+// This thread spins forever until signaled to stop.
+class BusyWaitingThread {
+ public:
+  BusyWaitingThread() : m_run(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(21);
+    system_clock::time_point start_time = system_clock::now();
+    m_started.Notify();
+    while (m_run.test_and_set()) {
+      // Have the busy waiting thread time out after a while.  If it times out,
+      // the test failed.
+      if (system_clock::now() - start_time > std::chrono::milliseconds(50)) {
+        return;
+      }
+    }
+    m_success.store(1);
+  }
+
+  void quit() { m_run.clear(); }
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // Use an atomic type to signal if the thread should be running or not.  A
+  // mutex could affect the scheduler, which isn't worth it.  A delay between
+  // setting and reading the new value is fine.
+  std::atomic_flag m_run;
+
+  Notification m_started;
+
+  std::atomic<int> m_success;
+};
+
+// This thread starts up, grabs the mutex, and then exits.
+template <typename MutexType>
+class HighPriorityThread {
+ public:
+  explicit HighPriorityThread(MutexType* mutex) : m_mutex(mutex) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(22);
+    m_started.Notify();
+    m_mutex->lock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  Notification m_started;
+  MutexType* m_mutex;
+  std::atomic<int> m_success{0};
+};
+
+// Class to test a MutexType to see if it solves the priority inheritance
+// problem.
+//
+// To run the test, we need 3 threads, and then 1 thread to kick the test off.
+// The threads must all run on the same core, otherwise they wouldn't starve
+// eachother. The threads and their roles are as follows:
+//
+// Low priority thread:
+//   Holds a lock that the high priority thread needs, and releases it upon
+//   request.
+// Medium priority thread:
+//   Hogs the processor so that the low priority thread will never run if it's
+//   priority doesn't get bumped.
+// High priority thread:
+//   Starts up and then goes to grab the lock that the low priority thread has.
+//
+// Control thread:
+//   Sets the deadlock up so that it will happen 100% of the time by making sure
+//   that each thread in order gets to the point that it needs to be at to cause
+//   the deadlock.
+template <typename MutexType>
+class InversionTestRunner {
+ public:
+  void operator()() {
+    // This thread must run at the highest priority or it can't coordinate the
+    // inversion.  This means that it can't busy wait or everything could
+    // starve.
+    SetThreadRealtimePriorityOrDie(23);
+
+    MutexType m;
+
+    // Start the lowest priority thread and wait until it holds the lock.
+    LowPriorityThread<MutexType> low(&m);
+    std::thread low_thread(std::ref(low));
+    low.WaitForStartup();
+
+    // Start the busy waiting thread and let it get to the loop.
+    BusyWaitingThread busy;
+    std::thread busy_thread(std::ref(busy));
+    busy.WaitForStartup();
+
+    // Start the high priority thread and let it become blocked on the lock.
+    HighPriorityThread<MutexType> high(&m);
+    std::thread high_thread(std::ref(high));
+    high.WaitForStartup();
+    // Startup and locking the mutex in the high priority thread aren't atomic,
+    // but pretty close.  Wait a bit to let it happen.
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+    // Release the mutex in the low priority thread.  If done right, everything
+    // should finish now.
+    low.release_mutex();
+
+    // Wait for everything to finish and compute success.
+    high_thread.join();
+    busy.quit();
+    busy_thread.join();
+    low_thread.join();
+    m_success = low.success() && busy.success() && high.success();
+  }
+
+  bool success() { return m_success; }
+
+ private:
+  bool m_success = false;
+};
+
+// TODO: Fix roborio permissions to run as root.
+
+// Priority inversion test.
+TEST(MutexTest, DISABLED_PriorityInversionTest) {
+  InversionTestRunner<priority_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Verify that the non-priority inversion mutex doesn't pass the test.
+TEST(MutexTest, DISABLED_StdMutexPriorityInversionTest) {
+  InversionTestRunner<std::mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_FALSE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, TryLock) {
+  priority_mutex m;
+  m.lock();
+  EXPECT_FALSE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+// Priority inversion test.
+TEST(MutexTest, DISABLED_ReentrantPriorityInversionTest) {
+  InversionTestRunner<priority_recursive_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, ReentrantTryLock) {
+  priority_recursive_mutex m;
+  m.lock();
+  EXPECT_TRUE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+#endif  // WPI_HAVE_PRIORITY_MUTEX
+
+}  // namespace wpi