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 return *(volatile typeof(sem->permits) *) &sem->permits;
19 }
20
__time_critical_func(sem_acquire_blocking)21 void __time_critical_func(sem_acquire_blocking)(semaphore_t *sem) {
22 bool block = true;
23 do {
24 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
25 if (sem->permits > 0) {
26 sem->permits--;
27 __sev();
28 block = false;
29 }
30 spin_unlock(sem->core.spin_lock, save);
31 if (block) {
32 __wfe();
33 }
34 } while (block);
35 }
36
__time_critical_func(sem_acquire_timeout_ms)37 bool __time_critical_func(sem_acquire_timeout_ms)(semaphore_t *sem, uint32_t timeout_ms) {
38 bool block = true;
39 absolute_time_t target = nil_time;
40 do {
41 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
42 if (sem->permits > 0) {
43 sem->permits--;
44 __sev();
45 block = false;
46 }
47 spin_unlock(sem->core.spin_lock, save);
48 if (block) {
49 if (is_nil_time(target)) {
50 target = make_timeout_time_ms(timeout_ms);
51 }
52 if (best_effort_wfe_or_timeout(target)) {
53 return false;
54 }
55 }
56 } while (block);
57 return true;
58 }
59
60 // todo this should really have a blocking variant for when permits are maxed out
__time_critical_func(sem_release)61 bool __time_critical_func(sem_release)(semaphore_t *sem) {
62 bool rc;
63 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
64 int32_t count = sem->permits;
65 if (count < sem->max_permits) {
66 sem->permits = count + 1;
67 __sev();
68 rc = true;
69 } else {
70 rc = false;
71 }
72 spin_unlock(sem->core.spin_lock, save);
73 return rc;
74 }
75
__time_critical_func(sem_reset)76 void __time_critical_func(sem_reset)(semaphore_t *sem, int16_t permits) {
77 assert(permits >= 0 && permits <= sem->max_permits);
78 uint32_t save = spin_lock_blocking(sem->core.spin_lock);
79 if (permits > sem->permits) __sev();
80 sem->permits = permits;
81 spin_unlock(sem->core.spin_lock, save);
82 }
83