Add sem_try_acquire(). Fixes #846 (#856)

Co-authored-by: Geoff Simmons <geoff@uplex.de>

src/common/pico_sync/include/pico/sem.h

@@ -122,6 +122,17 @@ bool sem_acquire_timeout_us(semaphore_t *sem, uint32_t timeout_us);
  */
 bool sem_acquire_block_until(semaphore_t *sem, absolute_time_t until);
 
+/*! \brief Attempt to acquire a permit from a semaphore without blocking
+ *  \ingroup sem
+ *
+ * This function will return false without blocking if no permits are
+ * available, otherwise it will acquire a permit and return true.
+ *
+ * \param sem Pointer to semaphore structure
+ * \return true if permit was acquired.
+ */
+bool sem_try_acquire(semaphore_t *sem);
+
 #ifdef __cplusplus
 }
 #endif

src/common/pico_sync/sem.c

@@ -52,6 +52,17 @@ bool __time_critical_func(sem_acquire_block_until)(semaphore_t *sem, absolute_ti
     } while (true);
 }
 
+bool __time_critical_func(sem_try_acquire)(semaphore_t *sem) {
+    uint32_t save = spin_lock_blocking(sem->core.spin_lock);
+    if (sem->permits > 0) {
+        sem->permits--;
+        lock_internal_spin_unlock_with_notify(&sem->core, save);
+        return true;
+    }
+    spin_unlock(sem->core.spin_lock, save);
+    return false;
+}
+
 // todo this should really have a blocking variant for when permits are maxed out
 bool __time_critical_func(sem_release)(semaphore_t *sem) {
     uint32_t save = spin_lock_blocking(sem->core.spin_lock);