Branch data Line data Source code
1 : : #include <cstring>
2 : :
3 : : #include "esp_log.h"
4 : : #include "esp_attr.h"
5 : :
6 : : #include "discovery_manager.hpp"
7 : : #include "heartbeat_manager.hpp"
8 : : #include "message_codec.hpp"
9 : : #include "message_router.hpp"
10 : : #include "node_state_machine.hpp"
11 : : #include "pairing_manager.hpp"
12 : : #include "peer_manager.hpp"
13 : : #include "protocol_messages.hpp"
14 : : #include "hal_timer.hpp"
15 : : #include "tx_manager.hpp"
16 : : #include "tx_state_machine.hpp"
17 : : #include "hal_wifi.hpp"
18 : : #include "hal_espnow.hpp"
19 : : #include "espnow_driver.hpp"
20 : : #include "hal_freertos.hpp"
21 : : #include "hal_nvs.hpp"
22 : : #include "persistence_backend.hpp"
23 : : #include "storage_manager.hpp"
24 : : #include "channel_monitor.hpp"
25 : : #include "statistics_manager.hpp"
26 : :
27 : : #include "espnow_manager.hpp"
28 : :
29 : : static const char* TAG = "EspNowManager";
30 : :
31 : : // RTC storage for peer list persistence must stay in global scope
32 : : static RTC_DATA_ATTR PersistentPeers g_rtc_peers;
33 : : static RTC_DATA_ATTR PersistentChannel g_rtc_channel;
34 : : static RTC_DATA_ATTR PersistentStats g_rtc_stats;
35 : :
36 : : // Pointer to the currently active (initialized) instance.
37 : : // ESP-NOW only supports a single active context at a time, so a single
38 : : // static pointer is sufficient. It is set in init() and cleared in deinit()
39 : : // so that the ISR callbacks never reach a stale or uninitialized object.
40 : : static EspNowManager* s_active_instance_ = nullptr;
41 : :
42 : : // Singleton Factory Constructor ---- (not used in host based tests) LCOV_EXCL_START
43 : : EspNowManager& EspNowManager::instance()
44 : : {
45 : : static NvsHAL nvs_hal;
46 : : static auto rtc_peers_backend = std::make_unique<RtcBackend>(&g_rtc_peers, sizeof(g_rtc_peers));
47 : : static auto rtc_channel_backend = std::make_unique<RtcBackend>(&g_rtc_channel, sizeof(g_rtc_channel));
48 : : static auto rtc_stats_backend = std::make_unique<RtcBackend>(&g_rtc_stats, sizeof(g_rtc_stats));
49 : : static auto nvs_peers_backend = std::make_unique<NvsBackend>(nvs_hal, "peers_data");
50 : : static auto nvs_channel_backend = std::make_unique<NvsBackend>(nvs_hal, "channel_data");
51 : : static auto nvs_stats_backend = std::make_unique<NvsBackend>(nvs_hal, "stats_data");
52 : : static auto storage = std::make_unique<StorageManager>(
53 : : std::move(rtc_peers_backend),
54 : : std::move(rtc_channel_backend),
55 : : std::move(rtc_stats_backend),
56 : : std::move(nvs_peers_backend),
57 : : std::move(nvs_channel_backend),
58 : : std::move(nvs_stats_backend));
59 : :
60 : : static auto hal_wifi = std::make_unique<WiFiHAL>();
61 : : static auto hal_espnow = std::make_unique<EspNowHAL>();
62 : : static auto hal_timer = std::make_unique<TimerHAL>();
63 : : static auto hal_freertos = std::make_unique<FreeRTOSHAL>();
64 : : static auto espnow_driver = std::make_unique<EspNowDriver>(*hal_wifi, *hal_espnow);
65 : : static auto peer_manager = std::make_unique<PeerManager>(*storage, *hal_espnow, *hal_freertos);
66 : : static auto message_codec = std::make_unique<MessageCodec>();
67 : : static auto channel_monitor = std::make_unique<ChannelMonitor>(*hal_wifi, *hal_freertos);
68 : : static auto scanner = std::make_unique<DiscoveryManager>(*hal_wifi, *hal_espnow, *message_codec, *hal_freertos);
69 : : static auto stats_mgr = std::make_unique<StatisticsManager>(*storage, *hal_freertos);
70 : : static auto tx_fsm = std::make_unique<TxStateMachine>();
71 : : static auto tx_manager =
72 : : std::make_unique<TxManager>(*tx_fsm, *hal_espnow, *hal_freertos, *message_codec, *stats_mgr, *peer_manager);
73 : : static auto heartbeat_mgr = std::make_unique<HeartbeatManager>(*tx_manager, *peer_manager, *hal_timer);
74 : : static auto pairing_mgr = std::make_unique<PairingManager>(*tx_manager, *peer_manager, *hal_freertos, *hal_timer);
75 : : static auto message_router = std::make_unique<MessageRouter>(*scanner, *tx_manager, *heartbeat_mgr, *pairing_mgr);
76 : :
77 : : static EspNowManager instance(
78 : : std::move(storage),
79 : : std::move(hal_wifi),
80 : : std::move(hal_timer),
81 : : std::move(hal_freertos),
82 : : std::move(hal_espnow),
83 : : std::move(espnow_driver),
84 : : std::move(peer_manager),
85 : : std::move(message_codec),
86 : : std::move(channel_monitor),
87 : : std::move(scanner),
88 : : std::move(tx_fsm),
89 : : std::move(tx_manager),
90 : : std::move(heartbeat_mgr),
91 : : std::move(pairing_mgr),
92 : : std::move(message_router),
93 : : std::move(stats_mgr),
94 : : std::make_unique<NodeStateMachine>());
95 : : return instance;
96 : : }
97 : : // LCOV_EXCL_STOP
98 : :
99 : 129 : EspNowManager::EspNowManager(
100 : : std::unique_ptr<IStorageManager> storage,
101 : : std::unique_ptr<IWiFiHAL> hal_wifi,
102 : : std::unique_ptr<ITimerHAL> hal_timer,
103 : : std::unique_ptr<IFreeRTOSHAL> hal_freertos,
104 : : std::unique_ptr<IEspNowHAL> hal_espnow,
105 : : std::unique_ptr<IEspNowDriver> espnow_driver,
106 : : std::unique_ptr<IPeerManager> peer_manager,
107 : : std::unique_ptr<IMessageCodec> message_codec,
108 : : std::unique_ptr<IChannelMonitor> channel_monitor,
109 : : std::unique_ptr<IDiscoveryManager> scanner,
110 : : std::unique_ptr<ITxStateMachine> tx_fsm,
111 : : std::unique_ptr<ITxManager> tx_manager,
112 : : std::unique_ptr<IHeartbeatManager> heartbeat_manager,
113 : : std::unique_ptr<IPairingManager> pairing_manager,
114 : : std::unique_ptr<IMessageRouter> message_router,
115 : : std::unique_ptr<IStatisticsManager> stats_mgr,
116 : 129 : std::unique_ptr<INodeStateMachine> node_fsm)
117 : 129 : : storage_(std::move(storage))
118 : 129 : , hal_wifi_(std::move(hal_wifi))
119 : 129 : , hal_timer_(std::move(hal_timer))
120 : 129 : , hal_freertos_(std::move(hal_freertos))
121 : 129 : , hal_espnow_(std::move(hal_espnow))
122 : 129 : , espnow_driver_(std::move(espnow_driver))
123 : 129 : , peer_manager_(std::move(peer_manager))
124 : 129 : , message_codec_(std::move(message_codec))
125 : 129 : , channel_monitor_(std::move(channel_monitor))
126 : 129 : , scanner_(std::move(scanner))
127 : 129 : , tx_fsm_(std::move(tx_fsm))
128 : 129 : , tx_manager_(std::move(tx_manager))
129 : 129 : , heartbeat_manager_(std::move(heartbeat_manager))
130 : 129 : , pairing_manager_(std::move(pairing_manager))
131 : 129 : , message_router_(std::move(message_router))
132 : 129 : , stats_mgr_(std::move(stats_mgr))
133 : 129 : , node_fsm_(std::move(node_fsm))
134 : : {
135 : 129 : }
136 : :
137 : 129 : EspNowManager::~EspNowManager()
138 : : {
139 : : // Caller is responsible for calling deinit() before destruction.
140 : : // Resources allocated in init() must be explicitly released.
141 : 129 : }
142 : :
143 : : // =========================================================================================
144 : : // Public API
145 : : // =========================================================================================
146 : :
147 : 115 : esp_err_t EspNowManager::init(const EspNowConfig& config)
148 : : {
149 [ + + ]: 115 : if (node_fsm_->get_state() != NodeState::UNINITIALIZED) {
150 : : return ESP_ERR_INVALID_STATE;
151 : : }
152 [ + + ]: 114 : if (config.app_rx_queue == nullptr) {
153 : : return ESP_ERR_INVALID_ARG;
154 : : }
155 : :
156 : 112 : config_ = config;
157 : 112 : esp_err_t ret = ESP_OK;
158 : :
159 : : // Storage needs to be initialized to load channel from storage before EspNowDriver
160 [ + - ]: 112 : if (storage_ != nullptr) {
161 : 112 : uint8_t stored_channel;
162 [ + - ]: 112 : if (storage_->load_channel(stored_channel) == ESP_OK) {
163 : 112 : config_.wifi_channel = stored_channel;
164 : : }
165 : : }
166 : :
167 : : // Register this instance as the active one before enabling the ESP-NOW
168 : : // callbacks. The callbacks run in ISR context and must reach this object.
169 : 112 : s_active_instance_ = this;
170 : :
171 : : // EspNowDriver initializes ESPNOW
172 : 112 : ret = espnow_driver_->init(config_, esp_now_recv_cb, esp_now_send_cb);
173 [ + + ]: 112 : if (ret != ESP_OK) {
174 : 1 : s_active_instance_ = nullptr;
175 : 1 : return init_fail(ret, "EspNow Driver");
176 : : }
177 : 111 : esp_now_initialized_ = true;
178 : :
179 : 111 : ret = create_queue();
180 [ + + ]: 111 : if (ret != ESP_OK) {
181 : 1 : return init_fail(ret, "Queues");
182 : : }
183 : :
184 : 110 : ret = create_task();
185 [ + + ]: 110 : if (ret != ESP_OK) {
186 : 1 : return init_fail(ret, "Tasks");
187 : : }
188 : :
189 : 109 : ret = init_tx_manager();
190 [ + + ]: 109 : if (ret != ESP_OK) {
191 : 1 : return init_fail(ret, "Tx Manager");
192 : : }
193 : :
194 : 108 : ret = init_discovery_manager();
195 [ + + ]: 108 : if (ret != ESP_OK) {
196 : 1 : return init_fail(ret, "Discovery Manager");
197 : : }
198 : :
199 : 107 : ret = init_heartbeat_manager();
200 [ - + ]: 107 : if (ret != ESP_OK) {
201 : 0 : return init_fail(ret, "Heartbeat Manager");
202 : : }
203 : :
204 : 107 : ret = init_pairing_manager();
205 [ + + ]: 107 : if (ret != ESP_OK) {
206 : 1 : return init_fail(ret, "Pairing Manager");
207 : : }
208 : :
209 : 106 : ret = init_channel_monitor();
210 [ + + ]: 106 : if (ret != ESP_OK) {
211 : 1 : return init_fail(ret, "Channel Monitor");
212 : : }
213 : :
214 [ + - ]: 105 : if (stats_mgr_ != nullptr) {
215 : 105 : ret = stats_mgr_->init();
216 [ + + ]: 105 : if (ret != ESP_OK) {
217 : 1 : return init_fail(ret, "Stats Manager");
218 : : }
219 : : }
220 : :
221 : : // Load peers from storage and add them to ESP-NOW
222 : 104 : peer_manager_->load_peers_from_storage();
223 : 104 : etl::vector<PeerInfo, MAX_PEERS> peers = peer_manager_->get_all();
224 : :
225 : : // Ensure statistics entries exist for all loaded peers. This covers the case where
226 : : // peer storage survived but stats storage was corrupted or never flushed.
227 [ + + ]: 106 : for (const auto& peer : peers) {
228 : 2 : stats_mgr_->on_peer_added(peer.node_id, peer.heartbeat_interval_ms);
229 : : }
230 : : // Remove any stats entry that has no matching peer (NVS inconsistency)
231 : 104 : stats_mgr_->sync_peers(peers);
232 : :
233 : : // NodeStateMachine decides the initial state
234 : 104 : NodeState old_state = node_fsm_->get_state();
235 : :
236 : 104 : bool is_hub = (config_.node_type == ReservedTypes::HUB);
237 : 104 : bool has_peers = !peers.empty();
238 : 104 : node_fsm_->on_init(is_hub, has_peers);
239 : :
240 : 104 : NodeState new_state = node_fsm_->get_state();
241 : :
242 [ + + ]: 104 : if (new_state == NodeState::OPERATIONAL) {
243 : 2 : add_peers_to_espnow(peers);
244 : : }
245 : :
246 : : // React to the initial state transition
247 : 104 : handle_state_transition(old_state, new_state);
248 : :
249 : : // Update scanner and storage with current channel
250 : 104 : scanner_->set_channel(config_.wifi_channel);
251 : 104 : storage_->store_channel(config_.wifi_channel);
252 : :
253 : 104 : ESP_LOGI(TAG, "EspNow component initialized successfully.");
254 : 104 : return ret;
255 : 104 : }
256 : :
257 : 54 : void EspNowManager::deinit()
258 : : {
259 : 54 : ESP_LOGI(TAG, "Deinitializing EspNowManager...");
260 : :
261 [ + - ]: 54 : if (stats_mgr_ != nullptr) {
262 : 54 : stats_mgr_->deinit();
263 : : }
264 : :
265 [ + - ]: 54 : if (tx_manager_ != nullptr) {
266 : 54 : tx_manager_->deinit();
267 : : }
268 : :
269 [ + - ]: 54 : if (scanner_ != nullptr) {
270 : 54 : scanner_->deinit();
271 : : }
272 : :
273 [ + - ]: 54 : if (heartbeat_manager_ != nullptr) {
274 : 54 : heartbeat_manager_->deinit();
275 : : }
276 : :
277 [ + - ]: 54 : if (pairing_manager_ != nullptr) {
278 : 54 : pairing_manager_->deinit();
279 : : }
280 : :
281 [ + - ]: 54 : if (channel_monitor_ != nullptr) {
282 : 54 : channel_monitor_->deinit();
283 : : }
284 : :
285 [ + + ]: 54 : if (rx_task_handle_ != nullptr) {
286 : : // Signal rx task to stop
287 : 48 : signal_task_to_stop();
288 : : // Delete tasks if not terminated gracefully
289 : 48 : delete_task();
290 : : }
291 : :
292 : : // Call cleanup_resources to delete queues and mutex
293 [ + + ]: 54 : if (rx_queue_handle_ != nullptr) {
294 : 51 : cleanup_resources();
295 : : }
296 : :
297 : : // Delete peers
298 [ + + + - ]: 54 : if (esp_now_initialized_ && peer_manager_) {
299 : 52 : etl::vector<PeerInfo, MAX_PEERS> peers = peer_manager_->get_all();
300 [ + + ]: 57 : for (const auto& peer : peers) {
301 : 5 : hal_espnow_->hal_esp_now_del_peer(peer.mac);
302 : : }
303 : 52 : }
304 : :
305 : : // Call EspNowDriver to deinit ESP-NOW
306 [ + - ]: 54 : if (espnow_driver_ != nullptr) {
307 : 54 : espnow_driver_->deinit();
308 : : }
309 : :
310 : : // Reset state
311 : 54 : esp_now_initialized_ = false;
312 : 54 : config_ = EspNowConfig();
313 : 54 : node_fsm_->on_deinit();
314 : :
315 : : // Clear the active-instance pointer so that any late ISR callback
316 : : // arriving after deinit() drops the packet safely.
317 : 54 : s_active_instance_ = nullptr;
318 : :
319 : 54 : ESP_LOGI(TAG, "EspNow component deinitialized.");
320 : 54 : }
321 : :
322 : 4 : esp_err_t EspNowManager::reconnect()
323 : : {
324 [ + + ]: 4 : if (node_fsm_->get_state() != NodeState::IDLE) {
325 : : return ESP_ERR_INVALID_STATE;
326 : : }
327 [ + + ]: 3 : if (peer_manager_->get_all().empty()) {
328 : : return ESP_ERR_INVALID_ARG; // Cannot reconnect if there are no peers
329 : : }
330 : :
331 : 2 : scan_retry_.reset(); // Reset counter - full attempts restored
332 : 2 : NodeState old_state = node_fsm_->get_state();
333 : 2 : esp_err_t ret = node_fsm_->on_scan_requested(); // IDLE -> RECOVERY_SCAN
334 [ + - ]: 2 : if (ret == ESP_OK) {
335 : 2 : handle_state_transition(old_state, node_fsm_->get_state());
336 : : }
337 : : return ret;
338 : : }
339 : :
340 : 5 : esp_err_t EspNowManager::start_pairing(uint32_t timeout_ms)
341 : : {
342 [ + + ]: 5 : if (node_fsm_->get_state() == NodeState::UNINITIALIZED) {
343 : : return ESP_ERR_INVALID_STATE;
344 : : }
345 : :
346 : : // Store timeout for use when pairing start or scan completes
347 : 4 : pairing_timeout_ms_ = timeout_ms;
348 : :
349 : 4 : NodeState old_state = node_fsm_->get_state();
350 : :
351 : 4 : bool is_hub = (config_.node_type == ReservedTypes::HUB);
352 : 4 : bool has_peers = !peer_manager_->get_all().empty();
353 : 4 : esp_err_t ret = node_fsm_->on_pairing_requested(is_hub, has_peers);
354 : :
355 : 4 : NodeState new_state = node_fsm_->get_state();
356 : :
357 : 4 : handle_state_transition(old_state, new_state);
358 : :
359 : 4 : return ret;
360 : : }
361 : :
362 : 7 : esp_err_t EspNowManager::send_data(
363 : : NodeId dest_node_id,
364 : : PayloadType payload_type,
365 : : const void* payload,
366 : : size_t len,
367 : : bool require_ack)
368 : : {
369 : 7 : return send_packet(dest_node_id, MessageType::DATA, payload_type, payload, len, require_ack);
370 : : }
371 : :
372 : 4 : esp_err_t EspNowManager::send_command(
373 : : NodeId dest_node_id,
374 : : CommandType command_type,
375 : : const void* payload,
376 : : size_t len,
377 : : bool require_ack)
378 : : {
379 : 4 : return send_packet(
380 : 4 : dest_node_id, MessageType::COMMAND, static_cast<PayloadType>(command_type), payload, len, require_ack);
381 : : }
382 : :
383 : 5 : esp_err_t EspNowManager::confirm_reception(NodeId sender_id, uint16_t sequence_number, AckStatus status)
384 : : {
385 : 5 : auto current_state = node_fsm_->get_state();
386 [ + + ]: 5 : if (current_state != NodeState::OPERATIONAL && current_state != NodeState::PAIRING)
387 : : return ESP_ERR_INVALID_STATE;
388 : :
389 : 4 : DecodedTxPacket tx_packet{};
390 [ + + ]: 4 : if (!peer_manager_->find_mac(sender_id, tx_packet.dest_mac)) {
391 : : return ESP_ERR_NOT_FOUND;
392 : : }
393 : :
394 : : // Make a header for the ack message
395 : 2 : MessageHeader ack_header{};
396 : 2 : ack_header.msg_type = MessageType::ACK;
397 : 2 : ack_header.sequence_number = sequence_number;
398 : 2 : ack_header.sender_type = config_.node_type;
399 : 2 : ack_header.sender_node_id = config_.node_id;
400 : 2 : ack_header.dest_node_id = sender_id;
401 : 2 : ack_header.requires_ack = false;
402 : 2 : ack_header.ack_status = status;
403 : 2 : ack_header.timestamp_ms = get_time_ms();
404 : :
405 : 2 : tx_packet.header = ack_header;
406 : :
407 : : // Payload for ACK matches AckMessage structure fields after header
408 : 2 : AckMessage ack_message{};
409 : 2 : ack_message.processing_time_us = 0; // Not in use yet
410 : :
411 : : // Copy only the payload portion of AckMessage, skipping MessageHeader.
412 : 2 : tx_packet.payload_len = sizeof(AckMessage) - sizeof(MessageHeader);
413 : 2 : memcpy(tx_packet.payload, &ack_message.processing_time_us, tx_packet.payload_len);
414 : :
415 : 2 : return tx_manager_->queue_packet(tx_packet);
416 : : }
417 : :
418 : : // Peer management
419 : 4 : esp_err_t EspNowManager::add_peer(NodeId node_id, const uint8_t* mac, NodeType type, uint32_t heartbeat_interval_ms)
420 : : {
421 : 4 : esp_err_t ret = peer_manager_->add(node_id, mac, type, heartbeat_interval_ms);
422 [ + + ]: 4 : if (ret == ESP_OK) {
423 : 2 : stats_mgr_->on_peer_added(node_id, heartbeat_interval_ms);
424 : : // Sync in case PeerManager silently evicted or reassigned a peer
425 : 2 : stats_mgr_->sync_peers(peer_manager_->get_all());
426 : : }
427 : 4 : return ret;
428 : : }
429 : :
430 : 5 : esp_err_t EspNowManager::remove_peer(NodeId node_id)
431 : : {
432 : 5 : esp_err_t ret = peer_manager_->remove(node_id);
433 [ + + ]: 5 : if (ret == ESP_OK) {
434 : 3 : stats_mgr_->on_peer_removed(node_id);
435 : : }
436 : 5 : return ret;
437 : : }
438 : :
439 : 2 : etl::vector<NodeId, MAX_PEERS> EspNowManager::get_offline_peers() const
440 : : {
441 : 2 : NodeState state = node_fsm_->get_state();
442 [ + + ]: 2 : if (state != NodeState::OPERATIONAL && state != NodeState::PAIRING) {
443 : 1 : return {};
444 : : }
445 : 1 : return peer_manager_->get_offline(get_time_ms());
446 : : }
447 : :
448 : : // Getters
449 : 35 : NodeState EspNowManager::get_node_state() const
450 : : {
451 : 35 : return node_fsm_->get_state();
452 : : }
453 : :
454 : 4 : bool EspNowManager::is_initialized() const
455 : : {
456 : 4 : return node_fsm_->get_state() != NodeState::UNINITIALIZED;
457 : : }
458 : :
459 : 1 : etl::vector<PeerInfo, MAX_PEERS> EspNowManager::get_peers()
460 : : {
461 : 1 : return peer_manager_->get_all();
462 : : }
463 : :
464 : 3 : bool EspNowManager::get_peer_stats(NodeId node_id, PeerStatistics& out) const
465 : : {
466 [ + - ]: 3 : if (stats_mgr_ == nullptr) {
467 : : return false;
468 : : }
469 : 3 : return stats_mgr_->get(node_id, out);
470 : : }
471 : :
472 : 2 : etl::vector<PeerStatistics, MAX_PEERS> EspNowManager::get_all_peer_stats() const
473 : : {
474 [ - + ]: 2 : if (stats_mgr_ == nullptr) {
475 : 0 : return {};
476 : : }
477 : 2 : return stats_mgr_->get_all();
478 : : }
479 : :
480 : : // =========================================================================================
481 : : // ESP-NOW callbacks - called by ESP-NOW driver in ISR context --- LCOV_EXCL_START
482 : : // =========================================================================================
483 : : void EspNowManager::esp_now_recv_cb(const esp_now_recv_info_t* info, const uint8_t* data, int len)
484 : : {
485 : : if (!info || !data || len <= 0 || len > ESP_NOW_MAX_DATA_LEN)
486 : : return;
487 : : EspNowManager* self = s_active_instance_;
488 : : if (self == nullptr || self->rx_queue_handle_ == nullptr)
489 : : return;
490 : : RxPacket packet;
491 : : memcpy(packet.src_mac, info->src_addr, 6);
492 : : memcpy(packet.data, data, len);
493 : : packet.len = len;
494 : : packet.rssi = static_cast<int8_t>(info->rx_ctrl->rssi);
495 : : // Raw timestamp in us, will be converted to ms in rx_task
496 : : packet.timestamp_ms = self->hal_timer_->get_time_us();
497 : : self->hal_freertos_->queue_send_fromISR(self->rx_queue_handle_, &packet, 0);
498 : : }
499 : :
500 : : void EspNowManager::esp_now_send_cb(const esp_now_send_info_t* info, esp_now_send_status_t status)
501 : : {
502 : : EspNowManager* self = s_active_instance_;
503 : : if (self == nullptr || info == nullptr || info->des_addr == nullptr) {
504 : : return;
505 : : }
506 : :
507 : : // Broadcast sends have no logical peer counterpart. Delivery tracking is meaningless
508 : : // for them. Driver-level errors are already handled synchronously via
509 : : // handle_esp_now_send_errors() right after hal_esp_now_send() returns.
510 : : // This covers: pairing requests/responses, channel scan probes/responses.
511 : : if (memcmp(info->des_addr, BROADCAST_MAC, 6) == 0) {
512 : : return;
513 : : }
514 : :
515 : : self->tx_manager_->notify_delivery(status, info->des_addr);
516 : : }
517 : : // LCOV_EXCL_STOP
518 : :
519 : : // =========================================================================================
520 : : // Task implementations
521 : : // =========================================================================================
522 : 37 : void EspNowManager::rx_task(void* arg)
523 : : {
524 : 37 : EspNowManager* self = static_cast<EspNowManager*>(arg);
525 : :
526 : 37 : RxPacket packet{};
527 : 37 : DecodedRxPacket decoded{};
528 : 37 : uint32_t notifications = 0;
529 : 37 : bool should_stop = false;
530 : :
531 : 271 : while (true) {
532 : : // Check for pending notifications without blocking (timeout = 0). We prioritize packet reception below
533 : : // notifications are checked every ~100 ms as a side effect of the queue_receive timeout.
534 [ + + ]: 154 : if (self->hal_freertos_->task_notify_wait(0, 0xFFFFFFFF, ¬ifications, 0) == pdTRUE) {
535 : : // Check for notifications and handle them
536 : 45 : self->handle_notifications(notifications, should_stop);
537 : : // If stop is requested, break the loop
538 [ + + ]: 45 : if (should_stop) {
539 : : break;
540 : : }
541 : : }
542 : :
543 : : // Wait for incoming packets with a timeout to periodically check for notifications and tick managers.
544 [ + + ]: 117 : if (self->hal_freertos_->queue_receive(self->rx_queue_handle_, &packet, pdMS_TO_TICKS(100)) == pdTRUE) {
545 : : // Convert raw timestamp from us to ms
546 : 45 : packet.timestamp_ms /= 1000;
547 : :
548 : : // Validate CRC
549 [ + + ]: 45 : if (self->message_codec_->validate_crc(packet.data, packet.len)) {
550 : : // Decode header
551 : 43 : auto header_opt = self->message_codec_->decode_header(packet.data, packet.len);
552 [ + + ]: 43 : if (header_opt) {
553 : : // Any valid packet is proof the link is alive notify tx_manager about it
554 : 41 : self->tx_manager_->notify_link_alive();
555 : :
556 : : // Report packet reception to stats manager
557 : 41 : self->stats_mgr_->on_packet_received(header_opt->sender_node_id, packet.rssi);
558 : :
559 [ + + ]: 41 : decoded = {packet, header_opt.value()};
560 : :
561 : : // Application-level packets — deliver directly to app queue
562 [ + + ]: 41 : if (decoded.header.msg_type == MessageType::DATA ||
563 : : decoded.header.msg_type == MessageType::COMMAND) {
564 [ + - ]: 9 : if (self->config_.app_rx_queue != nullptr) {
565 : 9 : AppMessage msg = self->build_app_message(decoded);
566 : 9 : if (self->hal_freertos_->queue_send(self->config_.app_rx_queue, &msg, 0) != pdTRUE) {
567 : 9 : ESP_LOGW(TAG, "App queue full, dropping packet");
568 : : }
569 : : }
570 : : }
571 : : else {
572 : : // Protocol-internal packets — handle immediately via router
573 : 32 : self->message_router_->handle_packet(decoded);
574 : : }
575 : : // After processing (Routing or App delivery), update peer last_seen.
576 : : // If it was a new pairing, the peer is now in the list.
577 : 41 : self->peer_manager_->update_last_seen(decoded.header.sender_node_id, self->get_time_ms());
578 : : }
579 : : }
580 : : }
581 : : // HUBs go to pairing inactive only by timeout, not when sucessfull pair a node.
582 : : // Non_HUB cannot be handled here because the scanning process is not instantaneous and if
583 : : // on_pairing_timeout is called before the scan is completed, the node will go to IDLE instead of
584 : : // OPERATIONAL.
585 : : // TODO: remove this after full refactor, pairing done is notified to rx_task
586 : : // if (self->config_.node_type == ReservedTypes::HUB && !self->pairing_manager_->is_active()) {
587 : : // bool has_peers = !self->peer_manager_->get_all().empty();
588 : : // self->node_fsm_->on_pairing_timeout(has_peers);
589 : : // }
590 : :
591 : : // Tick submodules to handle timers
592 : 117 : int64_t now_ms = self->get_time_ms();
593 : 117 : NodeState current_state = self->node_fsm_->get_state();
594 [ + + ]: 117 : if (current_state == NodeState::PAIRING) {
595 : 8 : self->channel_monitor_->tick(now_ms);
596 : 8 : self->pairing_manager_->tick(now_ms);
597 : : }
598 [ + + ]: 109 : else if (current_state == NodeState::OPERATIONAL) {
599 : 17 : self->channel_monitor_->tick(now_ms);
600 : 17 : self->heartbeat_manager_->tick(now_ms);
601 : : }
602 : 117 : self->tick_scan_retry(now_ms);
603 : 117 : }
604 : :
605 : : // Task cleanup on exit
606 : 37 : self->rx_task_handle_ = nullptr;
607 : 37 : self->hal_freertos_->task_suspend(NULL);
608 : 0 : self->hal_freertos_->task_delete(NULL);
609 : 0 : }
610 : :
611 : : // =========================================================================================
612 : : // Internal methods
613 : : // =========================================================================================
614 : :
615 : : // Helper method used by send_data and send_command
616 : 11 : esp_err_t EspNowManager::send_packet(
617 : : NodeId dest_node_id,
618 : : MessageType msg_type,
619 : : PayloadType payload_type,
620 : : const void* payload,
621 : : size_t len,
622 : : bool require_ack)
623 : : {
624 [ + + ]: 11 : if (node_fsm_->get_state() != NodeState::OPERATIONAL)
625 : : return ESP_ERR_INVALID_STATE;
626 : :
627 : 9 : DecodedTxPacket tx_packet;
628 [ + + ]: 9 : if (!peer_manager_->find_mac(dest_node_id, tx_packet.dest_mac))
629 : : return ESP_ERR_NOT_FOUND;
630 : :
631 : 7 : tx_packet.header.msg_type = msg_type;
632 : 7 : tx_packet.header.sequence_number = 0;
633 : 7 : tx_packet.header.sender_type = config_.node_type;
634 : 7 : tx_packet.header.sender_node_id = config_.node_id;
635 : 7 : tx_packet.header.payload_type = payload_type;
636 : 7 : tx_packet.header.requires_ack = require_ack;
637 : 7 : tx_packet.header.dest_node_id = dest_node_id;
638 : 7 : tx_packet.header.timestamp_ms = get_time_ms();
639 : :
640 [ + + ]: 7 : if (len > MAX_PAYLOAD_SIZE) {
641 : : return ESP_ERR_INVALID_ARG;
642 : : }
643 : :
644 : 6 : tx_packet.payload_len = len;
645 [ + + ]: 6 : if (payload != nullptr && len > 0) {
646 : 1 : memcpy(tx_packet.payload, payload, len);
647 : : }
648 : :
649 : 6 : return tx_manager_->queue_packet(tx_packet);
650 : : }
651 : :
652 : 178 : int64_t EspNowManager::get_time_ms() const
653 : : {
654 : 178 : return hal_timer_->get_time_us() / 1000;
655 : : }
656 : :
657 : : // Helper to build AppMessage from DecodedRxPacket
658 : 10 : AppMessage EspNowManager::build_app_message(const DecodedRxPacket& decoded)
659 : : {
660 : 10 : AppMessage msg{};
661 : 10 : msg.sender_id = decoded.header.sender_node_id;
662 : 10 : msg.sender_type = decoded.header.sender_type;
663 : 10 : msg.msg_type = decoded.header.msg_type;
664 : 10 : msg.payload_type = decoded.header.payload_type;
665 : 10 : msg.sequence_number = decoded.header.sequence_number;
666 : 10 : msg.requires_ack = decoded.header.requires_ack;
667 : 10 : memcpy(msg.src_mac, decoded.raw.src_mac, 6);
668 : 10 : msg.rssi = decoded.raw.rssi;
669 : :
670 : 10 : const size_t payload_offset = sizeof(MessageHeader);
671 : 10 : msg.payload_len = decoded.raw.len - payload_offset - CRC_SIZE;
672 : 10 : memcpy(msg.payload, decoded.raw.data + payload_offset, msg.payload_len);
673 : :
674 : 10 : return msg;
675 : : }
676 : :
677 : 62 : void EspNowManager::handle_notifications(uint32_t notifications, bool& should_stop)
678 : : {
679 : : // If we receive NOTIFY_MAX_FAILURES from TxManager::tx_task()
680 [ + + ]: 62 : if ((notifications & NOTIFY_MAX_FAILURES) == NOTIFY_MAX_FAILURES) {
681 : 3 : NodeState old_state = node_fsm_->get_state();
682 : 3 : node_fsm_->on_scan_requested();
683 : 3 : handle_state_transition(old_state, node_fsm_->get_state());
684 : : }
685 : :
686 : : // NOTIFY_CHANNEL_FOUND is set by DiscoveryManager::discovery_task()
687 [ + + ]: 62 : if ((notifications & NOTIFY_CHANNEL_FOUND) == NOTIFY_CHANNEL_FOUND) {
688 : : // Calls get_channel() to get the channel where the HUB was found
689 : 7 : config_.wifi_channel = scanner_->get_channel();
690 : :
691 : 7 : NodeState old_state = node_fsm_->get_state();
692 : 7 : node_fsm_->on_channel_found();
693 : 7 : handle_state_transition(old_state, node_fsm_->get_state());
694 : : }
695 : :
696 : : // NOTIFY_PEER_ADDED is set by PairingManager::notify_rx_task_peer_add()
697 [ + + ]: 62 : if ((notifications & NOTIFY_PEER_ADDED) == NOTIFY_PEER_ADDED) {
698 : 2 : auto peers = peer_manager_->get_all();
699 [ + + ]: 4 : for (const auto& p : peers) {
700 : 2 : stats_mgr_->on_peer_added(p.node_id, p.heartbeat_interval_ms);
701 : : }
702 : : // Sync in case PairingManager's add() triggered a silent eviction/reassign
703 : 2 : stats_mgr_->sync_peers(peers);
704 : 2 : }
705 : :
706 : : // NOTIFY_PAIRING_DONE is set by PairingManager::notify_rx_task_pairing_done()
707 [ + + ]: 62 : if ((notifications & NOTIFY_PAIRING_DONE) == NOTIFY_PAIRING_DONE) {
708 : 4 : bool has_peers = !peer_manager_->get_all().empty();
709 : :
710 : 4 : NodeState old_state = node_fsm_->get_state();
711 : 4 : node_fsm_->on_pairing_timeout(has_peers);
712 : 4 : handle_state_transition(old_state, node_fsm_->get_state());
713 : : }
714 : :
715 : : // NOTIFY_SCAN_FAILED is set by DiscoveryManager::discovery_task()
716 [ + + ]: 62 : if ((notifications & NOTIFY_SCAN_FAILED) == NOTIFY_SCAN_FAILED) {
717 : 5 : bool has_peers = !peer_manager_->get_all().empty();
718 : 5 : NodeState old_state = node_fsm_->get_state();
719 : 5 : node_fsm_->on_scan_failed();
720 : 5 : handle_state_transition(old_state, node_fsm_->get_state());
721 : 5 : handle_scan_retries(has_peers);
722 : : }
723 : :
724 : : // NOTIFY_CHANNEL_CHANGED is set by ChannelMonitor via direct to task notification
725 [ + + ]: 62 : if ((notifications & NOTIFY_CHANNEL_CHANGED) == NOTIFY_CHANNEL_CHANGED) {
726 : 3 : uint8_t channel = channel_monitor_->get_wifi_channel();
727 : 3 : config_.wifi_channel = channel;
728 : 3 : scanner_->set_channel(channel);
729 : 3 : storage_->store_channel(channel);
730 : 3 : ESP_LOGI(TAG, "Channel changed to %d", channel);
731 : : }
732 : :
733 : : // If NOTIFY_TASK_TO_STOP is set, we break the loop and exit the task.
734 [ + + ]: 62 : if (notifications & NOTIFY_TASK_TO_STOP) {
735 : 38 : should_stop = true;
736 : : }
737 : 62 : }
738 : :
739 : 131 : void EspNowManager::handle_state_transition(NodeState old_state, NodeState new_state)
740 : : {
741 [ + - ]: 131 : if (old_state == new_state) {
742 : : return;
743 : : }
744 : :
745 : : // ESP_LOGI(TAG, "Reacting to state change: %d -> %d", static_cast<int>(old_state), static_cast<int>(new_state));
746 : :
747 [ + + + + : 131 : switch (new_state) {
+ - ]
748 : 101 : case NodeState::PAIRING_SCAN:
749 : 101 : scan_retry_.reset(); // Do not interfere with pairing which has separate logic
750 : 101 : scanner_->start_scan();
751 : 101 : break;
752 : :
753 : 7 : case NodeState::RECOVERY_SCAN:
754 : 7 : scanner_->start_scan();
755 : 7 : break;
756 : :
757 : 9 : case NodeState::PAIRING:
758 [ + + ]: 9 : if (scanner_->is_scanning()) {
759 : 1 : scanner_->stop_scan();
760 : : }
761 : 9 : pairing_manager_->start(pairing_timeout_ms_, get_time_ms());
762 : 9 : break;
763 : :
764 : 6 : case NodeState::OPERATIONAL:
765 [ + + ]: 6 : if (scanner_->is_scanning()) {
766 : 1 : scanner_->stop_scan();
767 : : }
768 [ + + ]: 6 : scan_retry_.reset(); // Success: cancel any pending retry and reset counter
769 : : // If we just rediscovered the channel, store it
770 [ + + ]: 6 : if (old_state == NodeState::RECOVERY_SCAN || old_state == NodeState::PAIRING_SCAN) {
771 : 3 : storage_->store_channel(config_.wifi_channel);
772 : : }
773 : : break;
774 : :
775 : 8 : case NodeState::IDLE:
776 [ + + ]: 8 : if (scanner_->is_scanning()) {
777 : 1 : scanner_->stop_scan();
778 : : }
779 : : break;
780 : :
781 : : default:
782 : : break;
783 : : }
784 : : }
785 : :
786 : 122 : void EspNowManager::tick_scan_retry(int64_t now_ms)
787 : : {
788 [ + + + + ]: 122 : if (!scan_retry_.active || now_ms < scan_retry_.next_attempt_ms) {
789 : : return;
790 : : }
791 [ + + ]: 3 : if (node_fsm_->get_state() != NodeState::IDLE) {
792 : 1 : scan_retry_.reset();
793 : 1 : return;
794 : : }
795 : :
796 : 2 : scan_retry_.active = false;
797 : 2 : NodeState old_state = node_fsm_->get_state();
798 : :
799 : : // Always RECOVERY_SCAN because we only schedule if we have peers
800 : 2 : node_fsm_->on_scan_requested();
801 : 2 : handle_state_transition(old_state, node_fsm_->get_state());
802 : : }
803 : :
804 : 5 : void EspNowManager::handle_scan_retries(bool has_peers)
805 : : {
806 : : // Only retry scan if we have peers (RECOVERY_SCAN)
807 [ + + ]: 5 : if (!has_peers) {
808 : : return;
809 : : }
810 [ + - ]: 1 : if (scan_retry_.count < config_.scan_max_retries) {
811 : 1 : uint32_t delay = SCAN_BACKOFF_BASE_MS << scan_retry_.count;
812 : 1 : scan_retry_.active = true;
813 : 1 : scan_retry_.next_attempt_ms = get_time_ms() + delay;
814 : 1 : scan_retry_.count++;
815 : 1 : ESP_LOGI(
816 : : TAG,
817 : : "Recovery scan failed. Retry %d/%d in %lu ms",
818 : : scan_retry_.count,
819 : : config_.scan_max_retries,
820 : : (unsigned long)delay);
821 : : }
822 : : else {
823 : 5 : ESP_LOGW(TAG, "Recovery scan max retries (%d) exhausted. Node is IDLE.", config_.scan_max_retries);
824 : : }
825 : : }
826 : :
827 : : // ==================================================================
828 : : // Init helpers
829 : : // ==================================================================
830 : :
831 : 111 : esp_err_t EspNowManager::create_queue()
832 : : {
833 : 111 : rx_queue_handle_ = hal_freertos_->queue_create(config_.rx_queue_length, sizeof(RxPacket));
834 [ + + ]: 111 : if (rx_queue_handle_ == nullptr) {
835 : 1 : return ESP_FAIL;
836 : : }
837 : :
838 : : return ESP_OK;
839 : : }
840 : :
841 : 110 : esp_err_t EspNowManager::create_task()
842 : : {
843 : 110 : BaseType_t ret;
844 : 110 : ret = hal_freertos_->task_create(
845 : : rx_task, "rx_task", config_.stack_size_rx_task, this, config_.priority_rx_task, &rx_task_handle_);
846 : :
847 [ + + ]: 110 : if (ret != pdPASS) {
848 : 1 : return ESP_FAIL;
849 : : }
850 : :
851 : : return ESP_OK;
852 : : }
853 : :
854 : 109 : esp_err_t EspNowManager::init_tx_manager()
855 : : {
856 : : // TODO: remove nullptr guards, since the objects are not optional, they are passed as reference
857 : : // to the constructor, the compiler should complain if they are not initialized.
858 [ + - ]: 109 : if (tx_manager_ == nullptr) {
859 : : return ESP_FAIL;
860 : : }
861 : 109 : return tx_manager_->init(
862 : 109 : config_.stack_size_tx_task, config_.priority_tx_task, rx_task_handle_, config_.ack_timeout_ms);
863 : : }
864 : :
865 : 108 : esp_err_t EspNowManager::init_discovery_manager()
866 : : {
867 [ + - ]: 108 : if (scanner_ == nullptr) {
868 : : return ESP_FAIL;
869 : : }
870 : 108 : return scanner_->init(
871 : 108 : config_.node_id, config_.node_type, rx_task_handle_, config_.priority_rx_task, config_.stack_size_rx_task);
872 : : }
873 : :
874 : 107 : esp_err_t EspNowManager::init_heartbeat_manager()
875 : : {
876 [ + - ]: 107 : if (heartbeat_manager_ == nullptr) {
877 : : return ESP_FAIL;
878 : : }
879 : 107 : heartbeat_manager_->init(config_.node_id, config_.node_type, config_.heartbeat_interval_ms);
880 : 107 : return ESP_OK;
881 : : }
882 : :
883 : 107 : esp_err_t EspNowManager::init_pairing_manager()
884 : : {
885 [ + - ]: 107 : if (pairing_manager_ == nullptr) {
886 : : return ESP_FAIL;
887 : : }
888 : 107 : return pairing_manager_->init(config_.node_id, config_.node_type, rx_task_handle_, config_.heartbeat_interval_ms);
889 : : }
890 : :
891 : 106 : esp_err_t EspNowManager::init_channel_monitor()
892 : : {
893 [ + - ]: 106 : if (channel_monitor_ == nullptr) {
894 : : return ESP_FAIL;
895 : : }
896 : 106 : return channel_monitor_->init(config_.channel_monitor_interval_ms, rx_task_handle_);
897 : : }
898 : :
899 : 8 : esp_err_t EspNowManager::init_fail(esp_err_t ret, const char* step)
900 : : {
901 : 8 : ESP_LOGE(TAG, "init failed at %s: %s", step, esp_err_to_name(ret));
902 : 8 : deinit();
903 : 8 : return ret;
904 : : }
905 : :
906 : 2 : void EspNowManager::add_peers_to_espnow(etl::ivector<PeerInfo>& peers)
907 : : {
908 [ + + ]: 4 : for (auto& peer : peers) {
909 : 2 : esp_now_peer_info_t peer_info = {};
910 : 2 : memcpy(peer_info.peer_addr, peer.mac, 6);
911 : 2 : peer_info.channel = 0;
912 : 2 : peer_info.ifidx = WIFI_IF_STA;
913 : 2 : peer_info.encrypt = false;
914 : 2 : hal_espnow_->hal_esp_now_add_peer(&peer_info);
915 : : }
916 : 2 : }
917 : :
918 : 48 : void EspNowManager::signal_task_to_stop()
919 : : {
920 : : // Signal tasks to stop
921 [ + - ]: 48 : if (rx_task_handle_ != nullptr) {
922 : 48 : hal_freertos_->task_notify(rx_task_handle_, NOTIFY_TASK_TO_STOP, eSetBits);
923 : : }
924 : :
925 : : // Wait for tasks to exit (up to 1s).
926 : : uint16_t timeout_ms = 1000;
927 : : uint8_t delay_ms = 10;
928 [ + + ]: 1582 : while (timeout_ms > 0) {
929 [ + + ]: 1570 : if (rx_task_handle_ == nullptr) {
930 : : break;
931 : : }
932 : 1534 : hal_freertos_->task_delay(pdMS_TO_TICKS(delay_ms));
933 : 1534 : timeout_ms -= delay_ms;
934 : : }
935 : :
936 : 48 : if (rx_task_handle_ != nullptr) {
937 : 48 : ESP_LOGW(TAG, "Tasks did not terminate gracefully within timeout");
938 : : }
939 : 48 : }
940 : :
941 : 48 : void EspNowManager::delete_task()
942 : : {
943 [ + + ]: 48 : if (rx_task_handle_ != nullptr) {
944 : 12 : hal_freertos_->task_suspend(rx_task_handle_);
945 : 12 : hal_freertos_->task_delete(rx_task_handle_);
946 : 12 : rx_task_handle_ = nullptr;
947 : : }
948 : 48 : }
949 : :
950 : 51 : void EspNowManager::cleanup_resources()
951 : : {
952 [ + - ]: 51 : if (rx_queue_handle_ != nullptr) {
953 : 51 : hal_freertos_->queue_delete(rx_queue_handle_);
954 : 51 : rx_queue_handle_ = nullptr;
955 : : }
956 : 51 : }
|
