-
Notifications
You must be signed in to change notification settings - Fork 0
/
ring.c
179 lines (152 loc) · 5.37 KB
/
ring.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
// =============================================================================
// ring.c: Multi-core and IRQ safe ring buffer implementation. Data is an array
// of uint8_t, so it can be used for any data type.
//
// Author: Steve Shreeve <[email protected]>
// Date: February 29, 2024
// Legal: Same license as the Pico SDK
// Thanks: btstack_ring_buffer from the Raspberry Pi Pico SDK
// Thanks: rppicomidi has a nice ring buffer implementation
// =============================================================================
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <malloc.h>
#include "pico.h"
#include "pico/assert.h"
#include "pico/lock_core.h"
#include "ring.h"
// ==[ Init, Reset, Destroy ]===================================================
void ring_init_with_spin_lock(ring_t *r, uint size, uint spin_lock_num) {
assert(r);
assert(!r->data);
lock_init(&r->core, spin_lock_num);
r->data = (uint8_t *) calloc(size, 1);
r->size = (uint16_t) size;
r->wptr = 0;
r->rptr = 0;
}
ring_t *ring_new(uint size) {
ring_t *r = (ring_t *) calloc(sizeof(ring_t), 1);
ring_init_with_spin_lock(r, size, next_striped_spin_lock_num());
return r;
}
void ring_reset(ring_t *r) {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
r->wptr = 0;
r->rptr = 0;
spin_unlock(r->core.spin_lock, save);
}
void ring_destroy(ring_t *r) {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
free(r->data);
free(r);
spin_unlock(r->core.spin_lock, save);
}
// ==[ Used, Free, Empty, Full ]================================================
inline uint16_t ring_used_unsafe(ring_t *r) {
int32_t used = (int32_t) r->wptr - (int32_t) r->rptr;
if (used < 0) used += r->size + 1;
return (uint16_t) used;
}
inline uint16_t ring_free_unsafe(ring_t *r) {
return r->size - ring_used_unsafe(r);
}
inline uint16_t ring_used(ring_t *r) {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
uint16_t used = ring_used_unsafe(r);
spin_unlock(r->core.spin_lock, save);
return used;
}
inline uint16_t ring_free(ring_t *r) {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
uint16_t free = ring_free_unsafe(r);
spin_unlock(r->core.spin_lock, save);
return free;
}
inline bool ring_is_empty(ring_t *r) {
return ring_used(r) == 0;
}
inline bool ring_is_full(ring_t *r) {
return ring_free(r) == 0;
}
// ==[ Internal ]===============================================================
static uint16_t
ring_write_internal(ring_t *r, const void *ptr, uint16_t len, bool block) {
do {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
uint16_t cnt = ring_free_unsafe(r);
if (cnt) {
if (len = MIN(len, cnt)) {
uint16_t sip = MIN(r->size - r->wptr, len);
if (sip < len) {
memcpy(r->data + r->wptr, ptr, sip);
memcpy(r->data, ptr + sip, len - sip);
r->wptr = len - sip;
} else {
memcpy(r->data + r->wptr, ptr, len);
r->wptr += len;
}
}
lock_internal_spin_unlock_with_notify(&r->core, save);
return len;
}
if (block) {
lock_internal_spin_unlock_with_wait(&r->core, save);
} else {
spin_unlock(r->core.spin_lock, save);
return 0;
}
} while (true);
}
static uint16_t
ring_read_internal(ring_t *r, const void *ptr, uint16_t len, bool block) {
do {
uint32_t save = spin_lock_blocking(r->core.spin_lock);
uint16_t cnt = ring_used_unsafe(r);
if (cnt) {
if (len = MIN(len, cnt)) {
uint16_t sip = MIN(r->size - r->rptr, len);
if (sip < len) {
memcpy((void *) ptr, r->data + r->rptr, sip);
memcpy((void *) ptr + sip, r->data, len - sip);
r->rptr = len - sip;
} else {
memcpy((void *) ptr, r->data + r->rptr, len);
r->rptr += len;
}
}
lock_internal_spin_unlock_with_notify(&r->core, save);
return len;
}
if (block) {
lock_internal_spin_unlock_with_wait(&r->core, save);
} else {
spin_unlock(r->core.spin_lock, save);
return 0;
}
} while (true);
}
// ==[ Read and write ]=========================================================
inline uint16_t ring_try_read(ring_t *r, void *ptr, uint16_t len) {
return ring_read_internal(r, ptr, len, false);
}
inline uint16_t ring_try_write(ring_t *r, const void *ptr, uint16_t len) {
return ring_write_internal(r, ptr, len, false);
}
inline uint16_t ring_read_blocking(ring_t *r, void *ptr, uint16_t len) {
return ring_read_internal(r, ptr, len, true);
}
inline uint16_t ring_write_blocking(ring_t *r, const void *ptr, uint16_t len) {
return ring_write_internal(r, ptr, len, true);
}
// ==[ FIXME: We should remove or improve this ]================================
#include <stdarg.h>
char ring_buffer[RING_BUFFER_SIZE];
uint16_t ring_printf(ring_t *r, const char *fmt, ...) {
va_list args;
va_start(args, fmt);
uint16_t len = vsnprintf(ring_buffer, RING_BUFFER_SIZE, fmt, args);
va_end(args);
return ring_write_blocking(r, ring_buffer, len);
}