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