From c6c4eeb12243cd464510b210d9c8bf3e0e9d3aa0 Mon Sep 17 00:00:00 2001
From: Graham Sanderson <graham.sanderson@raspberrypi.com>
Date: Wed, 12 May 2021 16:32:45 -0500
Subject: [PATCH] add timeout_us/until to mutex/sem blocking methods (#402)

---
 src/common/pico_sync/include/pico/mutex.h | 14 +++++++++++
 src/common/pico_sync/include/pico/sem.h   | 29 ++++++++++++++++++++++-
 src/common/pico_sync/mutex.c              |  4 ++++
 src/common/pico_sync/sem.c                | 14 +++++++----
 4 files changed, 55 insertions(+), 6 deletions(-)

diff --git a/src/common/pico_sync/include/pico/mutex.h b/src/common/pico_sync/include/pico/mutex.h
index a421718..22dd19d 100644
--- a/src/common/pico_sync/include/pico/mutex.h
+++ b/src/common/pico_sync/include/pico/mutex.h
@@ -91,6 +91,20 @@ bool mutex_try_enter(mutex_t *mtx, uint32_t *owner_out);
  */
 bool mutex_enter_timeout_ms(mutex_t *mtx, uint32_t timeout_ms);
 
+/*! \brief Wait for mutex with timeout
+ *  \ingroup mutex
+ *
+ * Wait for up to the specific time to take ownership of the mutex. If the calling
+ * core can take ownership of the mutex before the timeout expires, then true will be returned
+ * and the calling core will own the mutex, otherwise false will be returned and the calling
+ * core will *NOT* own the mutex.
+ *
+ * \param mtx Pointer to mutex structure
+ * \param timeout_us The timeout in microseconds.
+ * \return true if mutex now owned, false if timeout occurred before mutex became available
+ */
+bool mutex_enter_timeout_us(mutex_t *mtx, uint32_t timeout_us);
+
 /*! \brief Wait for mutex until a specific time
  *  \ingroup mutex
  *
diff --git a/src/common/pico_sync/include/pico/sem.h b/src/common/pico_sync/include/pico/sem.h
index 19ac292..ecd3cf5 100644
--- a/src/common/pico_sync/include/pico/sem.h
+++ b/src/common/pico_sync/include/pico/sem.h
@@ -90,11 +90,38 @@ void sem_acquire_blocking(semaphore_t *sem);
  * return false, otherwise it will return true.
  *
  * \param sem Pointer to semaphore structure
- * \param timeout_ms Time to wait to acquire the semaphore, in ms.
+ * \param timeout_ms Time to wait to acquire the semaphore, in milliseconds.
  * \return false if timeout reached, true if permit was acquired.
  */
 bool sem_acquire_timeout_ms(semaphore_t *sem, uint32_t timeout_ms);
 
+/*! \brief  Acquire a permit from a semaphore, with timeout
+ *  \ingroup sem
+ *
+ * This function will block and wait if no permits are available, until the
+ * defined timeout has been reached. If the timeout is reached the function will
+ * return false, otherwise it will return true.
+ *
+ * \param sem Pointer to semaphore structure
+ * \param timeout_us Time to wait to acquire the semaphore, in microseconds.
+ * \return false if timeout reached, true if permit was acquired.
+ */
+bool sem_acquire_timeout_us(semaphore_t *sem, uint32_t timeout_us);
+
+/*! \brief Wait to acquire a permit from a semaphore until a specific time
+ *  \ingroup sem
+ *
+ * This function will block and wait if no permits are available, until the
+ * specified timeout time. If the timeout is reached the function will
+ * return false, otherwise it will return true.
+ *
+ * \param sem Pointer to semaphore structure
+ * \param until The time after which to return if the sem is not available.
+ * \return true if permit was acquired, false if the until time was reached before
+ * acquiring.
+ */
+bool sem_acquire_until(semaphore_t *sem, absolute_time_t until);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/common/pico_sync/mutex.c b/src/common/pico_sync/mutex.c
index a758abf..45ede4d 100644
--- a/src/common/pico_sync/mutex.c
+++ b/src/common/pico_sync/mutex.c
@@ -67,6 +67,10 @@ bool __time_critical_func(mutex_enter_timeout_ms)(mutex_t *mtx, uint32_t timeout
     return mutex_enter_block_until(mtx, make_timeout_time_ms(timeout_ms));
 }
 
+bool __time_critical_func(mutex_enter_timeout_us)(mutex_t *mtx, uint32_t timeout_us) {
+    return mutex_enter_block_until(mtx, make_timeout_time_us(timeout_us));
+}
+
 bool __time_critical_func(mutex_enter_block_until)(mutex_t *mtx, absolute_time_t until) {
     assert(mtx->core.spin_lock);
     do {
diff --git a/src/common/pico_sync/sem.c b/src/common/pico_sync/sem.c
index 06bcac5..4f3dc32 100644
--- a/src/common/pico_sync/sem.c
+++ b/src/common/pico_sync/sem.c
@@ -31,7 +31,14 @@ void __time_critical_func(sem_acquire_blocking)(semaphore_t *sem) {
 }
 
 bool __time_critical_func(sem_acquire_timeout_ms)(semaphore_t *sem, uint32_t timeout_ms) {
-    absolute_time_t target = nil_time;
+    return sem_acquire_until(sem, make_timeout_time_ms(timeout_ms));
+}
+
+bool __time_critical_func(sem_acquire_timeout_us)(semaphore_t *sem, uint32_t timeout_us) {
+    return sem_acquire_until(sem, make_timeout_time_us(timeout_us));
+}
+
+bool __time_critical_func(sem_acquire_until)(semaphore_t *sem, absolute_time_t until) {
     do {
         uint32_t save = spin_lock_blocking(sem->core.spin_lock);
         if (sem->permits > 0) {
@@ -39,10 +46,7 @@ bool __time_critical_func(sem_acquire_timeout_ms)(semaphore_t *sem, uint32_t tim
             lock_internal_spin_unlock_with_notify(&sem->core, save);
             return true;
         }
-        if (is_nil_time(target)) {
-            target = make_timeout_time_ms(timeout_ms);
-        }
-        if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&sem->core, save, target)) {
+        if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&sem->core, save, until)) {
             return false;
         }
     } while (true);