1 /*
2 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include "pico/sem.h"
8 #include "pico/time.h"
9
sem_init(semaphore_t * sem,int16_t initial_permits,int16_t max_permits)10 void sem_init(semaphore_t *sem, int16_t initial_permits, int16_t max_permits) {
11 lock_init(&sem->core, next_striped_spin_lock_num());
12 sem->permits = initial_permits;
13 sem->max_permits = max_permits;
14 __mem_fence_release();
15 }
16
__time_critical_func(sem_available)17 int __time_critical_func(sem_available)(semaphore_t *sem) {
18 #ifdef __GNUC__
19 return *(volatile typeof(sem->permits) *) &sem->permits;
20 #else
21 static_assert(sizeof(sem->permits) == 2, "");
22 return *(volatile int16_t *) &sem->permits;
23 #endif
24 }
25
__time_critical_func(sem_acquire_blocking)26 void __time_critical_func(sem_acquire_blocking)(semaphore_t *sem) {
27 do {
28 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
29 if (sem->permits > 0) {
30 sem->permits--;
31 spin_unlock(sem->core.spin_lock, save);
32 break;
33 }
34 lock_internal_spin_unlock_with_wait(&sem->core, save);
35 } while (true);
36 }
37
__time_critical_func(sem_acquire_timeout_ms)38 bool __time_critical_func(sem_acquire_timeout_ms)(semaphore_t *sem, uint32_t timeout_ms) {
39 return sem_acquire_block_until(sem, make_timeout_time_ms(timeout_ms));
40 }
41
__time_critical_func(sem_acquire_timeout_us)42 bool __time_critical_func(sem_acquire_timeout_us)(semaphore_t *sem, uint32_t timeout_us) {
43 return sem_acquire_block_until(sem, make_timeout_time_us(timeout_us));
44 }
45
__time_critical_func(sem_acquire_block_until)46 bool __time_critical_func(sem_acquire_block_until)(semaphore_t *sem, absolute_time_t until) {
47 do {
48 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
49 if (sem->permits > 0) {
50 sem->permits--;
51 spin_unlock(sem->core.spin_lock, save);
52 return true;
53 }
54 if (lock_internal_spin_unlock_with_best_effort_wait_or_timeout(&sem->core, save, until)) {
55 return false;
56 }
57 } while (true);
58 }
59
__time_critical_func(sem_try_acquire)60 bool __time_critical_func(sem_try_acquire)(semaphore_t *sem) {
61 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
62 if (sem->permits > 0) {
63 sem->permits--;
64 spin_unlock(sem->core.spin_lock, save);
65 return true;
66 }
67 spin_unlock(sem->core.spin_lock, save);
68 return false;
69 }
70
71 // todo this should really have a blocking variant for when permits are maxed out
__time_critical_func(sem_release)72 bool __time_critical_func(sem_release)(semaphore_t *sem) {
73 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
74 int32_t count = sem->permits;
75 if (count < sem->max_permits) {
76 sem->permits = (int16_t)(count + 1);
77 lock_internal_spin_unlock_with_notify(&sem->core, save);
78 return true;
79 } else {
80 spin_unlock(sem->core.spin_lock, save);
81 return false;
82 }
83 }
84
__time_critical_func(sem_reset)85 void __time_critical_func(sem_reset)(semaphore_t *sem, int16_t permits) {
86 assert(permits >= 0 && permits <= sem->max_permits);
87 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
88 if (permits > sem->permits) {
89 sem->permits = permits;
90 lock_internal_spin_unlock_with_notify(&sem->core, save);
91 } else {
92 sem->permits = permits;
93 spin_unlock(sem->core.spin_lock, save);
94 }
95 }
96
