1  #ifndef _UTIL_QUEUE_U32_INLINE_H
 2  #define _UTIL_QUEUE_U32_INLINE_H
 3  
 4  // Faster versions of the functions found in pico/util/queue.h, for the common
 5  // case of 32-bit-sized elements. Can be used on the same queue data
 6  // structure, and mixed freely with the generic access methods, as long as
 7  // element_size == 4.
 8  
 9  #include "pico/util/queue.h"
10  #include "hardware/sync.h"
11  
12  static inline uint16_t _queue_inc_index_u32(queue_t *q, uint16_t index) {
13      if (++index > q->element_count) { // > because we have element_count + 1 elements
14          index = 0;
15      }
16      return index;
17  }
18  
19  static inline bool queue_try_add_u32(queue_t *q, void *data) {
20      bool success = false;
21      uint32_t flags = spin_lock_blocking(q->core.spin_lock);
22      if (queue_get_level_unsafe(q) != q->element_count) {
23          ((uint32_t*)q->data)[q->wptr] = *(uint32_t*)data;
24          q->wptr = _queue_inc_index_u32(q, q->wptr);
25          success = true;
26      }
27      spin_unlock(q->core.spin_lock, flags);
28      if (success) __sev();
29      return success;
30  }
31  
32  static inline bool queue_try_remove_u32(queue_t *q, void *data) {
33      bool success = false;
34      uint32_t flags = spin_lock_blocking(q->core.spin_lock);
35      if (queue_get_level_unsafe(q) != 0) {
36          *(uint32_t*)data = ((uint32_t*)q->data)[q->rptr];
37          q->rptr = _queue_inc_index_u32(q, q->rptr);
38          success = true;
39      }
40      spin_unlock(q->core.spin_lock, flags);
41      if (success) __sev();
42      return success;
43  }
44  
45  static inline bool queue_try_peek_u32(queue_t *q, void *data) {
46      bool success = false;
47      uint32_t flags = spin_lock_blocking(q->core.spin_lock);
48      if (queue_get_level_unsafe(q) != 0) {
49          *(uint32_t*)data = ((uint32_t*)q->data)[q->rptr];
50          success = true;
51      }
52      spin_unlock(q->core.spin_lock, flags);
53      return success;
54  }
55  
56  static inline void queue_add_blocking_u32(queue_t *q, void *data) {
57      bool done;
58      do {
59          done = queue_try_add_u32(q, data);
60          if (done) break;
61          __wfe();
62      } while (true);
63  }
64  
65  static inline void queue_remove_blocking_u32(queue_t *q, void *data) {
66      bool done;
67      do {
68          done = queue_try_remove_u32(q, data);
69          if (done) break;
70          __wfe();
71      } while (true);
72  }
73  
74  static inline void queue_peek_blocking_u32(queue_t *q, void *data) {
75      bool done;
76      do {
77          done = queue_try_peek_u32(q, data);
78          if (done) break;
79          __wfe();
80      } while (true);
81  }
82  
83  #endif