8000 esp32/esp_espnow.c: Add ring buffer implementation to be used by esp_… · micropython/micropython@1b372b2 · GitHub
[go: up one dir, main page]
Skip to content

Commit 1b372b2

Browse files
glenn20glenn20
committed
esp32/esp_espnow.c: Add ring buffer implementation to be used by esp_espnow.
esp32/ringbuffer.[ch]: Add ring buffer for espnow send and recv. Use the ring buffer for the espnow send and recv callbacks. Merge recv_cb_wrapper() and send_cb_wrapper() into a single callback_wrapper() function which process multiple received packets on each invocation. These remove the buffer overwrites and reduce dropped packets.
1 parent bbea04b commit 1b372b2

File tree

4 files changed

+336
-44
lines changed

4 files changed

+336
-44
lines changed

‎ports/esp32/Makefile

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -347,6 +347,7 @@ SRC_C = \
347347
esp32_partition.c \
348348
esp32_rmt.c \
349349
esp_espnow.c \
350+
ring_buffer.c \
350351
esp32_ulp.c \
351352
modesp32.c \
352353
espneopixel.c \

‎ports/esp32/esp_espnow.c

Lines changed: 110 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
*
66
* Copyright (c) 2017-2020 Nick Moore
77
* Copyright (c) 2018 shawwwn <shawwwn1@gmail.com>
8+
* Copyright (c) 2020 Glenn Moloney
89
*
910
* Permission is hereby granted, free of charge, to any person obtaining a copy
1011
* of this software and associated documentation files (the "Software"), to deal
@@ -45,16 +46,27 @@
4546

4647
#include "modnetwork.h"
4748

48-
// XXX There's exactly one buffer and so sending or receiving
49-
// frames too quickly can therefore cause data to be lost *and* repeated.
50-
// TODO make this a ring buffer or something.
49+
#include "ring_buffer.h"
5150

52-
uint8_t send_mac[ESP_NOW_ETH_ALEN];
53-
esp_now_send_status_t send_status;
51+
// The receive buffer
52+
static buffer_t recv_buffer = NULL;
53+
static size_t recv_buffer_size = (
54+
2 * (ESP_NOW_ETH_ALEN + sizeof(uint8_t) + ESP_NOW_MAX_DATA_LEN)
55+
); // Enough for 2 full-size packets: 2 * (6 + 1 + 255) = 534 bytes
5456

55-
uint8_t recv_mac[ESP_NOW_ETH_ALEN];
56-
int recv_len;
57-
uint8_t recv_dat[ESP_NOW_MAX_DATA_LEN];
57+
// We also need a small send buffer as send_cb() may be called out of
58+
// order with the send() calls.
59+
static buffer_t send_buffer = NULL;
60+
static size_t send_buffer_size = (
61+
20 * (ESP_NOW_ETH_ALEN + sizeof(esp_now_send_status_t))
62+
); // Up to 20 callbacks = 140 bytes
63+
64+
static size_t espnow_sent_packets = 0;
65+
static size_t espnow_recv_packets = 0;
66+
static size_t espnow_dropped_packets = 0;
67+
68+
// The maximum number of packets to process in each recv_cb_wrapper()
69+
static int callback_max_packets = 20;
5870

5971
typedef struct _esp_espnow_obj_t {
6072
mp_obj_base_t base;
@@ -73,7 +85,7 @@ STATIC mp_obj_t get_esp_espnow(size_t n_args, const mp_obj_t *args) {
7385
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(get_esp_espnow_obj, 0, 1, get_esp_espnow);
7486

7587
NORETURN void _esp_espnow_exceptions(esp_err_t e) {
76-
switch (e) {
88+
switch (e) {
7789
case ESP_ERR_ESPNOW_NOT_INIT:
7890
mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT("ESP-Now Not Initialized"));
7991
case ESP_ERR_ESPNOW_ARG:
@@ -92,7 +104,7 @@ NORETURN void _esp_espnow_exceptions(esp_err_t e) {
92104
nlr_raise(mp_obj_new_exception_msg_varg(
93105
&mp_type_RuntimeError, MP_ERROR_TEXT("ESP-Now Unknown Error 0x%04x"), e
94106
));
95-
}
107+
}
96108
}
97109

98110
static inline void esp_espnow_exceptions(esp_err_t e) {
@@ -108,67 +120,111 @@ static inline void _get_bytes(mp_obj_t str, size_t len, uint8_t *dst) {
108120
memcpy(dst, data, len);
109121
}
110122

111-
// send_cb_wrapper and recv_cb_wrapper exist as MicroPython function
112-
// objects so that they can be added to the mp_sched_schedule.
113-
// send_cb and recv_cb are called from the ESP-Now library but might
114-
// be in an interrupt, so can't allocate memory.
115-
116-
STATIC mp_obj_t send_cb_wrapper(mp_obj_t dummy) {
117-
const esp_espnow_obj_t *self = &esp_espnow_obj;
118-
mp_obj_tuple_t *msg = mp_obj_new_tuple(2, NULL 8000 );
119-
msg->items[0] = mp_obj_new_bytes(send_mac, ESP_NOW_ETH_ALEN);
120-
msg->items[1] = (send_status == ESP_NOW_SEND_SUCCESS) ? mp_const_true : mp_const_false;
121-
mp_call_function_1(self->send_cb_obj, msg);
122-
return mp_const_none;
123-
}
124-
STATIC MP_DEFINE_CONST_FUN_OBJ_1(send_cb_wrapper_obj, send_cb_wrapper);
123+
// callback_wrapper exists as MicroPython function object so that it
124+
// can be added to the mp_sched_schedule. send_cb and recv_cb are called
125+
// from the ESP-Now library but might be in an interrupt, so can't
126+
// allocate memory.
127+
// The mp_sched_schedule stack is quite shallow (8), so it is safest and most
128+
// efficient to process all buffered send and receive in the same callback
129+
// to minimise packet loss.
125130

126-
STATIC mp_obj_t recv_cb_wrapper(mp_obj_t dummy) {
131+
STATIC mp_obj_t callback_wrapper(mp_obj_t dummy) {
127132
const esp_espnow_obj_t *self = &esp_espnow_obj;
128-
mp_obj_tuple_t *msg = mp_obj_new_tuple(2, NULL);
129-
msg->items[0] = mp_obj_new_bytes(recv_mac, ESP_NOW_ETH_ALEN);
130-
msg->items[1] = mp_obj_new_bytes(recv_dat, recv_len);
131-
mp_call_function_1(self->recv_cb_obj, msg);
133+
static size_t last_dropped_packets = 0;
134+
uint8_t mac_addr[ESP_NOW_ETH_ALEN];
135+
uint8_t data[ESP_NOW_MAX_DATA_LEN];
136+
uint8_t data_len;
137+
// Process multiple packets as a work around for the finite
138+
// size (8) of the mp_sched_schedule stack. This means some of our
139+
// calls might have no packets to process.
140+
// This reduces the number of dropped packets from bursts.
141+
int i = 0;
142+
while (!buffer_empty(recv_buffer) && i++ < callback_max_packets &&
143+
buffer_get(recv_buffer, mac_addr, ESP_NOW_ETH_ALEN) &&
144+
buffer_get(recv_buffer, &data_len, sizeof(data_len)) &&
145+
buffer_get(recv_buffer, &data, data_len)) {
146+
mp_obj_tuple_t *msg = mp_obj_new_tuple(3, NULL);
147+
msg->items[0] = mp_obj_new_bytes(mac_addr, ESP_NOW_ETH_ALEN);
148+
msg->items[1] = mp_obj_new_bytes(data, data_len);
149+
// Add the number of packets dropped since the last call to the tuple
150+
size_t dropped_packets = espnow_dropped_packets;
151+
msg->items[2] = mp_obj_new_int(dropped_packets - last_dropped_packets);
152+
last_dropped_packets = dropped_packets;
153+
mp_call_function_1(self->recv_cb_obj, msg);
154+
}
155+
esp_now_send_status_t send_status;
156+
i = 0;
157+
while (!buffer_empty(send_buffer) && i++ < callback_max_packets &&
158+
buffer_get(send_buffer, mac_addr, ESP_NOW_ETH_ALEN) &&
159+
buffer_get(send_buffer, &send_status, sizeof(send_status))) {
160+
mp_obj_tuple_t *msg = mp_obj_new_tuple(2, NULL);
161+
msg->items[0] = mp_obj_new_bytes(mac_addr, ESP_NOW_ETH_ALEN);
162+
msg->items[1] = (send_status == ESP_NOW_SEND_SUCCESS) ? mp_const_true : mp_const_false;
163+
mp_call_function_1(self->send_cb_obj, msg);
164+
}
132165
return mp_const_none;
133166
}
134-
STATIC MP_DEFINE_CONST_FUN_OBJ_1(recv_cb_wrapper_obj, recv_cb_wrapper);
167+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(callback_wrapper_obj, callback_wrapper);
135168

136-
STATIC void IRAM_ATTR send_cb(const uint8_t *macaddr, esp_now_send_status_t status)
169+
STATIC void IRAM_ATTR send_cb(const uint8_t *mac_addr, esp_now_send_status_t status)
137170
{
138-
if (esp_espnow_obj.send_cb_obj != mp_const_none) {
139-
memcpy(send_mac, macaddr, ESP_NOW_ETH_ALEN);
140-
send_status = status;
141-
mp_sched_schedule((const mp_obj_t)&send_cb_wrapper_obj, mp_const_none);
171+
if (esp_espnow_obj.send_cb_obj == mp_const_none ||
172+
ESP_NOW_ETH_ALEN + sizeof(status) >= buffer_free(send_buffer)) {
173+
return;
142174
}
175+
buffer_put(send_buffer, &mac_addr, ESP_NOW_ETH_ALEN);
176+
buffer_put(send_buffer, &status, sizeof(status));
177+
espnow_sent_packets++;
178+
mp_sched_schedule((const mp_obj_t)&callback_wrapper_obj, mp_const_none);
143179
}
144180

145-
STATIC void IRAM_ATTR recv_cb(const uint8_t *macaddr, const uint8_t *data, int len)
181+
STATIC void IRAM_ATTR recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len)
146182
{
147-
if (esp_espnow_obj.recv_cb_obj != mp_const_none) {
148-
memcpy(recv_mac, macaddr, ESP_NOW_ETH_ALEN);
149-
recv_len = len;
150-
memcpy(recv_dat, data, len);
151-
mp_sched_schedule((const mp_obj_t)&recv_cb_wrapper_obj, mp_const_none);
183+
if (esp_espnow_obj.recv_cb_obj == mp_const_none ||
184+
ESP_NOW_ETH_ALEN + sizeof(uint8_t) + len >= buffer_free(recv_buffer)) {
185+
espnow_dropped_packets++;
186+
return;
152187
}
153-
}
188+
uint8_t data_len = len;
189+
buffer_put(recv_buffer, mac_addr, ESP_NOW_ETH_ALEN);
190+
buffer_put(recv_buffer, &data_len, sizeof(data_len));
191+
buffer_put(recv_buffer, data, len);
192+
espnow_recv_packets++;
193+
mp_sched_schedule((const mp_obj_t)&callback_wrapper_obj, mp_const_none);
194+
}
154195

155196
static int initialized = 0;
156197

157-
STATIC mp_obj_t espnow_init(mp_obj_t self) {
198+
STATIC mp_obj_t espnow_init(size_t n_args, const mp_obj_t *args) {
158199
if (!initialized) {
200+
if (n_args > 1 && args[1] != mp_const_none) {
201+
recv_buffer_size = mp_obj_get_int(args[1]);
202+
}
203+
if (n_args > 2 && args[2] != mp_const_none) {
204+
send_buffer_size = mp_obj_get_int(args[2]);
205+
}
206+
if (n_args > 3 && args[3] != mp_const_none) {
207+
callback_max_packets = mp_obj_get_int(args[3]);
208+
}
209+
recv_buffer = buffer_init(recv_buffer_size);
210+
send_buffer = buffer_init(send_buffer_size);
159211
ESPNOW_EXCEPTIONS(esp_now_init());
160212
initialized = 1;
161213

162214
ESPNOW_EXCEPTIONS(esp_now_register_recv_cb(recv_cb));
163215
ESPNOW_EXCEPTIONS(esp_now_register_send_cb(send_cb));
216+
164217
}
165218
return mp_const_none;
166219
}
167-
STATIC MP_DEFINE_CONST_FUN_OBJ_1(espnow_init_obj, espnow_init);
220+
STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(espnow_init_obj, 1, 4, espnow_init);
168221

169222
STATIC mp_obj_t espnow_deinit(mp_obj_t self) {
170223
if (initialized) {
171224
ESPNOW_EXCEPTIONS(esp_now_deinit());
225+
buffer_release(recv_buffer);
226+
buffer_release(send_buffer);
227+
buffer_release(send_buffer);
172228
initialized = 0;
173229
}
174230
return mp_const_none;
@@ -334,6 +390,15 @@ STATIC mp_obj_t espnow_peer_count(mp_obj_t _) {
334390
}
335391
STATIC MP_DEFINE_CONST_FUN_OBJ_1(espnow_peer_count_obj, espnow_peer_count);
336392

393+
STATIC mp_obj_t espnow_stats(mp_obj_t _) {
394+
mp_obj_t tuple[3];
395+
tuple[0] = mp_obj_new_int(espnow_sent_packets);
396+
tuple[1] = mp_obj_new_int(espnow_recv_packets);
397+
tuple[2] = mp_obj_new_int(espnow_dropped_packets);
398+
return mp_obj_new_tuple(3, tuple);
399+
}
400+
STATIC MP_DEFINE_CONST_FUN_OBJ_1(espnow_stats_obj, espnow_stats);
401+
337402
STATIC mp_obj_t espnow_version(mp_obj_t _) {
338403
uint32_t version;
339404
ESPNOW_EXCEPTIONS(esp_now_get_version(&version));
@@ -353,6 +418,7 @@ STATIC const mp_rom_map_elem_t esp_espnow_locals_dict_table[] = {
353418
{ MP_ROM_QSTR(MP_QSTR_on_send), MP_ROM_PTR(&espnow_on_send_obj) },
354419
{ MP_ROM_QSTR(MP_QSTR_on_recv), MP_ROM_PTR(&espnow_on_recv_obj) },
355420
{ MP_ROM_QSTR(MP_QSTR_peer_count), MP_ROM_PTR(&espnow_peer_count_obj) },
421+
{ MP_ROM_QSTR(MP_QSTR_stats), MP_ROM_PTR(&espnow_stats_obj) },
356422
};
357423
STATIC MP_DEFINE_CONST_DICT(esp_espnow_locals_dict, esp_espnow_locals_dict_table);
358424

‎ports/esp32/ring_buffer.c

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
/*
2+
* This file is part of the MicroPython project, http://micropython.org/
3+
*
4+
* The MIT License (MIT)
5+
*
6+
* Copyright (c) 2020 Glenn Moloney
7+
*
8+
* Permission is hereby granted, free of charge, to any person obtaining a copy
9+
* of this software and associated documentation files (the "Software"), to deal
10+
* in the Software without restriction, including without limitation the rights
11+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12+
* copies of the Software, and to permit persons to whom the Software is
13+
* furnished to do so, subject to the following conditions:
14+
*
15+
* The above copyright notice and this permission notice shall be included in
16+
* all copies or substantial portions of the Software.
17+
*
18+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
24+
* THE SOFTWARE.
25+
*/
26+
27+
#include <assert.h>
28+
#include <stdio.h>
29+
#include <string.h>
30+
#include <stdint.h>
31+
#include <stdlib.h>
32+
33+
#include "ring_buffer.h"
34+
35+
// A simple ring buffer to memcpy data blocks in and out of buffer.
36+
// This implementation:
37+
// - maintains one free byte to ensure lock-less thread safety
38+
// (for a single producer and a single consumer).
39+
// - prioritises efficient memory usage at the expense of additional
40+
// memcpy()s (eg. on buffer wrap).
41+
42+
// Initialise a buffer of the requested size
43+
buffer_t buffer_init(size_t size) {
44+
assert(size);
45+
46+
buffer_t buffer = malloc(sizeof(buffer_real_t));
47+
assert(buffer);
48+
49+
// Allocate one extra byte to ensure threadsafety
50+
buffer->memory = malloc(size + 1);
51+
assert(buffer->memory);
52+
53+
buffer->size = size + 1;
54+
buffer_reset(buffer);
55+
56+
assert(buffer_empty(buffer));
57+
58+
return buffer;
59+
}
60+
61+
// Release and free the memory buffer
62+
void buffer_release(buffer_t buffer) {
63+
assert(buffer);
64+
buffer->size = buffer->head = buffer->tail = 0;
65+
if (buffer->memory != NULL) {
66+
free(buffer->memory);
67+
buffer->memory = NULL;
68+
}
69+
free(buffer);
70+
buffer = NULL;
71+
}
72+
73+
// Reset the buffer pointers discarding any data in the buffer
74+
void buffer_reset(buffer_t buffer) {
75+
assert(buffer);
76+
77+
buffer->head = buffer->tail = 0;
78+
}
79+
80+
// Copy some data to the buffer - reject if buffer is full
81+
bool buffer_put(buffer_t buffer, const void *data, size_t len) {
82+
assert(buffer && buffer->memory && data);
83+
84+
if (buffer_free(buffer) < len) {
85+
return false;
86+
}
87+
88+
size_t end = (buffer->head + len) % buffer->size;
89+
if (end < buffer->head) {
90+
memcpy(buffer->memory + buffer->head, data, buffer->size - buffer->head);
91+
data += (buffer->size - buffer->head);
92+
buffer->head = 0;
93+
}
94+
memcpy(buffer->memory + buffer->head, data, end - buffer->head);
95+
buffer->head = end;
96+
97+
return true;
98+
}
99+
100+
// Copy data from the buffer - return false if buffer is empty
101+
bool buffer_get(buffer_t buffer, void *data, size_t len) {
102+
assert(buffer && buffer->memory && data);
103+
104+
if (buffer_used(buffer) < len) {
105+
return false;
106+
}
107+
108+
size_t end = (buffer->tail + len) % buffer->size;
109+
if (end < buffer->tail) {
110+
memcpy(data, buffer->memory + buffer->tail, buffer->size - buffer->tail);
111+
data += (buffer->size - buffer->tail);
112+
buffer->tail = 0;
113+
}
114+
memcpy(data, buffer->memory + buffer->tail, end - buffer->tail);
115+
buffer->tail = end;
116+
117+
return true;
118+
}
119+
120+
// Print the current buffer state
121+
void buffer_print(buffer_t buffer) {
122+
printf("Buffer: size=%3d head=%3d, tail=%3d, used=%3d, free 4AF3 =%3d\n",
123+
(int)buffer->size, (int)buffer->head, (int)buffer->tail,
124+
(int)buffer_used(buffer), (int)buffer_free(buffer)
125+
);
126+
}

0 commit comments

Comments
 (0)
0