Branch data Line data Source code
1 : : #include <algorithm>
2 : : #include <cstring>
3 : :
4 : : #include "esp_log.h"
5 : : // #include "esp_now.h"
6 : : // #include "esp_wifi.h"
7 : : // #include "freertos/FreeRTOS.h"
8 : : // #include "freertos/queue.h"
9 : : // #include "freertos/semphr.h"
10 : : // #include "freertos/task.h"
11 : :
12 : : #include "i_storage_manager.hpp"
13 : : #include "i_hal_espnow.hpp"
14 : : #include "peer_manager.hpp"
15 : :
16 : : static const char* TAG = "PeerManager";
17 : :
18 : 37 : PeerManager::PeerManager(IStorageManager& storage, IEspNowHAL& hal_espnow, IFreeRTOSHAL& hal_freertos)
19 : : : storage_(storage)
20 : 37 : , hal_espnow_(hal_espnow)
21 : 37 : , hal_freertos_(hal_freertos)
22 : : {
23 : 37 : mutex_ = hal_freertos_.mutex_create();
24 : : // peers_ is etl::vector, capacity is fixed at compile-time.
25 : 37 : }
26 : :
27 : 74 : PeerManager::~PeerManager()
28 : : {
29 [ + - ]: 37 : if (mutex_ != nullptr) {
30 : 37 : hal_freertos_.semaphore_delete(mutex_);
31 : : }
32 : 74 : }
33 : :
34 : : // =====================================================================================
35 : : // Public methods
36 : : // =====================================================================================
37 : :
38 : 105 : esp_err_t PeerManager::add(NodeId id, const uint8_t* mac, NodeType type, uint32_t heartbeat_interval_ms)
39 : : {
40 [ + + ]: 105 : if (mac == nullptr) {
41 : : return ESP_ERR_INVALID_ARG;
42 : : }
43 : :
44 [ + + ]: 104 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
45 : : return ESP_ERR_TIMEOUT;
46 : : }
47 : :
48 : : // Find existing peers
49 : 103 : PeerInfo* existing_by_id = find_peer_by_id(id);
50 : 103 : PeerInfo* existing_by_mac = find_peer_by_mac(mac);
51 : :
52 : 103 : esp_err_t ret = ESP_OK;
53 : :
54 : : // Case 1: ID exists → update
55 [ + + ]: 103 : if (existing_by_id != nullptr) {
56 : 3 : ret = update_existing_peer_by_id(existing_by_id, mac, type, heartbeat_interval_ms);
57 : : }
58 : : // Case 2: MAC exists with different ID → reassign
59 [ + + ]: 100 : else if (existing_by_mac != nullptr) {
60 : 1 : reassign_mac_to_new_id(existing_by_mac, id, type, heartbeat_interval_ms);
61 : : }
62 : : // Case 3: New peer → add
63 : : else {
64 : 99 : ret = add_new_peer_to_empty_slot(id, mac, type, heartbeat_interval_ms);
65 : : }
66 : :
67 : : // Snapshot under mutex, then release before NVS write.
68 [ + + ]: 103 : etl::vector<PersistentPeer, MAX_PEERS> snapshot;
69 [ + + ]: 103 : if (ret == ESP_OK) {
70 [ + + ]: 903 : for (const auto& p : peers_)
71 : 803 : snapshot.push_back(info_to_persistent(p));
72 : : }
73 : 103 : hal_freertos_.semaphore_give(mutex_);
74 : :
75 [ + + ]: 103 : if (ret == ESP_OK)
76 : 100 : ret = storage_.store_peers(snapshot, true);
77 : :
78 : 103 : return ret;
79 : 103 : }
80 : :
81 : 5 : esp_err_t PeerManager::remove(NodeId id)
82 : : {
83 [ + + ]: 5 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
84 : : return ESP_ERR_TIMEOUT;
85 : : }
86 : :
87 [ + - + - : 14 : auto it = std::find_if(peers_.begin(), peers_.end(), [id](const PeerInfo& p) { return p.node_id == id; });
+ - + - +
- + + + +
+ + ]
88 : :
89 [ + + ]: 4 : if (it == peers_.end()) {
90 : 1 : hal_freertos_.semaphore_give(mutex_);
91 : 1 : return ESP_ERR_NOT_FOUND;
92 : : }
93 : :
94 : 3 : esp_err_t ret = hal_espnow_.hal_esp_now_del_peer(it->mac);
95 : :
96 [ + + ]: 3 : etl::vector<PersistentPeer, MAX_PEERS> snapshot;
97 [ + + ]: 3 : if (ret == ESP_OK) { // If peer is removed successfully from driver
98 : 2 : peers_.erase(it); // Remove from peer list
99 [ + + ]: 3 : for (const auto& p : peers_)
100 : 1 : snapshot.push_back(info_to_persistent(p));
101 : : }
102 : :
103 : 3 : hal_freertos_.semaphore_give(mutex_);
104 : :
105 [ + + ]: 3 : if (ret == ESP_OK)
106 : 2 : ret = storage_.store_peers(snapshot, true);
107 : :
108 : 3 : return ret;
109 : 3 : }
110 : :
111 : 14 : bool PeerManager::find_mac(NodeId id, uint8_t* mac)
112 : : {
113 [ + + ]: 14 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
114 : : return false;
115 : : }
116 : :
117 : 13 : bool found = false;
118 [ + + ]: 37 : for (const auto& p : peers_) {
119 [ + + ]: 33 : if (p.node_id == id) {
120 [ + + ]: 9 : if (mac != nullptr) {
121 : 8 : memcpy(mac, p.mac, 6);
122 : : }
123 : : found = true;
124 : : break;
125 : : }
126 : : }
127 : :
128 : 13 : hal_freertos_.semaphore_give(mutex_);
129 : 13 : return found;
130 : : }
131 : :
132 : 24 : etl::vector<PeerInfo, MAX_PEERS> PeerManager::get_all()
133 : : {
134 : 24 : etl::vector<PeerInfo, MAX_PEERS> copy;
135 : :
136 [ + + ]: 24 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
137 : : return copy;
138 : : }
139 : 23 : copy = peers_;
140 : 23 : hal_freertos_.semaphore_give(mutex_);
141 : 23 : return copy;
142 : : }
143 : :
144 : 6 : etl::vector<NodeId, MAX_PEERS> PeerManager::get_offline(int64_t now_ms)
145 : : {
146 : 6 : etl::vector<NodeId, MAX_PEERS> offline;
147 : :
148 [ + + ]: 6 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
149 : : return offline;
150 : : }
151 : :
152 [ + + ]: 9 : for (const auto& p : peers_) {
153 [ + + ]: 4 : if (p.heartbeat_interval_ms > 0) {
154 : 3 : uint32_t timeout = p.heartbeat_interval_ms * HEARTBEAT_OFFLINE_MULTIPLIER;
155 [ + + + + ]: 3 : if (p.last_seen_ms > 0 && (now_ms - p.last_seen_ms > timeout)) {
156 : 1 : offline.push_back(p.node_id);
157 : : }
158 : : }
159 : : }
160 : :
161 : 5 : hal_freertos_.semaphore_give(mutex_);
162 : 5 : return offline;
163 : : }
164 : :
165 : 24 : void PeerManager::update_last_seen(NodeId id, int64_t now_ms)
166 : : {
167 [ + + ]: 24 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) != pdTRUE) {
168 : : return;
169 : : }
170 [ + + ]: 195 : for (auto& p : peers_) {
171 [ + + ]: 194 : if (p.node_id == id) {
172 : 22 : p.last_seen_ms = now_ms;
173 : 22 : break;
174 : : }
175 : : }
176 : 23 : hal_freertos_.semaphore_give(mutex_);
177 : : }
178 : :
179 : 3 : esp_err_t PeerManager::find_node_id_by_mac(const uint8_t* mac, NodeId& out_id)
180 : : {
181 [ + + ]: 3 : if (hal_freertos_.semaphore_take(mutex_, pdMS_TO_TICKS(10)) != pdTRUE)
182 : : return ESP_ERR_TIMEOUT;
183 : :
184 : 2 : esp_err_t ret = ESP_ERR_NOT_FOUND;
185 [ + + ]: 2 : for (const auto& p : peers_) {
186 [ + - ]: 1 : if (memcmp(p.mac, mac, 6) == 0) {
187 : 1 : out_id = p.node_id;
188 : 1 : ret = ESP_OK;
189 : 1 : break;
190 : : }
191 : : }
192 : :
193 : 2 : hal_freertos_.semaphore_give(mutex_);
194 : 2 : return ret;
195 : : }
196 : :
197 : 4 : esp_err_t PeerManager::load_peers_from_storage()
198 : : {
199 : 4 : etl::vector<PersistentPeer, MAX_PEERS> stored_peers;
200 : :
201 : 4 : esp_err_t err = storage_.load_peers(stored_peers);
202 [ + + ]: 4 : if (err != ESP_OK) {
203 : : return err;
204 : : }
205 : :
206 [ + + ]: 3 : if (hal_freertos_.semaphore_take(mutex_, portMAX_DELAY) == pdTRUE) {
207 : 2 : peers_.clear();
208 [ + + ]: 5 : for (const auto& sp : stored_peers) {
209 [ + - ]: 3 : if (peers_.size() < MAX_PEERS) {
210 : 3 : peers_.push_back(persistent_to_info(sp));
211 : : }
212 : : }
213 : 2 : hal_freertos_.semaphore_give(mutex_);
214 : 2 : return ESP_OK;
215 : : }
216 : : else {
217 : : return ESP_ERR_TIMEOUT;
218 : : }
219 : 4 : }
220 : :
221 : : // =====================================================================================
222 : : // Private methods
223 : : // =====================================================================================
224 : :
225 : 804 : PersistentPeer PeerManager::info_to_persistent(const PeerInfo& info)
226 : : {
227 : 804 : PersistentPeer p;
228 : 804 : memcpy(p.mac, info.mac, 6);
229 : 804 : p.type = info.type;
230 : 804 : p.node_id = info.node_id;
231 : 804 : p.paired = info.paired;
232 : 804 : p.heartbeat_interval_ms = info.heartbeat_interval_ms;
233 : 804 : return p;
234 : : }
235 : :
236 : 3 : PeerInfo PeerManager::persistent_to_info(const PersistentPeer& persistent)
237 : : {
238 : 3 : PeerInfo info;
239 : 3 : memcpy(info.mac, persistent.mac, 6);
240 : 3 : info.type = persistent.type;
241 : 3 : info.node_id = persistent.node_id;
242 : 3 : info.last_seen_ms = 0;
243 : 3 : info.paired = persistent.paired;
244 : 3 : info.heartbeat_interval_ms = persistent.heartbeat_interval_ms;
245 : 3 : return info;
246 : : }
247 : :
248 : 99 : esp_now_peer_info_t PeerManager::make_espnow_peer_info(const uint8_t* mac)
249 : : {
250 : 99 : esp_now_peer_info_t peer_info = {};
251 : 99 : memcpy(peer_info.peer_addr, mac, 6);
252 : 99 : peer_info.channel = 0;
253 : 99 : peer_info.ifidx = WIFI_IF_STA;
254 : 99 : peer_info.encrypt = false;
255 : 99 : return peer_info;
256 : : }
257 : :
258 : : // =====================================================================================
259 : : // Private helper methods for add()
260 : : // =====================================================================================
261 : :
262 : 103 : PeerInfo* PeerManager::find_peer_by_id(NodeId id)
263 : : {
264 [ + + ]: 827 : for (auto& peer : peers_) {
265 [ + + ]: 727 : if (peer.node_id == id) {
266 : : return &peer;
267 : : }
268 : : }
269 : : return nullptr;
270 : : }
271 : :
272 : 103 : PeerInfo* PeerManager::find_peer_by_mac(const uint8_t* mac)
273 : : {
274 [ + + ]: 828 : for (auto& peer : peers_) {
275 [ + + ]: 727 : if (memcmp(peer.mac, mac, 6) == 0) {
276 : : return &peer;
277 : : }
278 : : }
279 : : return nullptr;
280 : : }
281 : :
282 : 3 : esp_err_t PeerManager::update_existing_peer_by_id(
283 : : PeerInfo* peer,
284 : : const uint8_t* new_mac,
285 : : NodeType type,
286 : : uint32_t heartbeat_interval_ms)
287 : : {
288 : 3 : ESP_LOGI(TAG, "Node ID %d already exists. Updating peer info.", (int)peer->node_id);
289 : :
290 : 3 : bool mac_changed = (memcmp(peer->mac, new_mac, 6) != 0);
291 : 3 : esp_err_t ret = ESP_OK;
292 : :
293 [ + + ]: 3 : if (mac_changed) {
294 : : // Delete old MAC from ESP-NOW driver
295 : 2 : ret = hal_espnow_.hal_esp_now_del_peer(peer->mac);
296 : : // Add peer with new MAC
297 [ + + ]: 2 : if (ret == ESP_OK) {
298 : 1 : auto peer_info = make_espnow_peer_info(new_mac);
299 : 1 : ret = hal_espnow_.hal_esp_now_add_peer(&peer_info);
300 : : }
301 : : }
302 : :
303 [ + - ]: 1 : if (ret == ESP_OK) {
304 : : // Update peer info in our list
305 [ + - ]: 2 : memcpy(peer->mac, new_mac, 6);
306 : 2 : peer->type = type;
307 : 2 : peer->heartbeat_interval_ms = heartbeat_interval_ms;
308 : :
309 : : // Move to front (LRU)
310 : 2 : PeerInfo updated = *peer;
311 [ - - - - : 4 : auto it = std::find_if(peers_.begin(), peers_.end(), [peer](const PeerInfo& p) { return &p == peer; });
- - - - -
- - - + -
+ - ]
312 [ + - ]: 2 : if (it != peers_.end()) {
313 : 2 : peers_.erase(it);
314 : 2 : peers_.insert(peers_.begin(), updated);
315 : : }
316 : : }
317 : :
318 : 3 : return ret;
319 : : }
320 : :
321 : 1 : void PeerManager::reassign_mac_to_new_id(PeerInfo* peer, NodeId new_id, NodeType type, uint32_t heartbeat_interval_ms)
322 : : {
323 : 1 : ESP_LOGI(TAG, "MAC address already exists with ID %d. Re-assigning to new ID %d.", (int)peer->node_id, (int)new_id);
324 : :
325 : : // Update the existing entry with the new ID and other info
326 : 1 : peer->node_id = new_id;
327 : 1 : peer->type = type;
328 : 1 : peer->heartbeat_interval_ms = heartbeat_interval_ms;
329 : 1 : peer->last_seen_ms = 0; // Reset as it's practically a new node identity
330 : :
331 : : // Move to front (LRU)
332 : 1 : PeerInfo updated = *peer;
333 [ - - - - : 2 : auto it = std::find_if(peers_.begin(), peers_.end(), [peer](const PeerInfo& p) { return &p == peer; });
- - - - -
- - - + -
+ - ]
334 [ + - ]: 1 : if (it != peers_.end()) {
335 : 1 : peers_.erase(it);
336 : 1 : peers_.insert(peers_.begin(), updated);
337 : : }
338 : 1 : }
339 : :
340 : : esp_err_t
341 : 99 : PeerManager::add_new_peer_to_empty_slot(NodeId id, const uint8_t* mac, NodeType type, uint32_t heartbeat_interval_ms)
342 : : {
343 : 99 : esp_err_t ret = ESP_OK;
344 : :
345 : : // Check if peer list is full
346 [ + + ]: 99 : if (peers_.size() >= MAX_PEERS) {
347 : 2 : ESP_LOGW(TAG, "Peer list is full. Removing the oldest seen peer.");
348 : :
349 : : // Evict based on LRU (least recently seen)
350 [ + - ]: 2 : auto oldest = std::min_element(peers_.begin(), peers_.end(), [](const PeerInfo& a, const PeerInfo& b) {
351 [ + + ]: 36 : return a.last_seen_ms < b.last_seen_ms;
352 : : });
353 : :
354 : 2 : ret = hal_espnow_.hal_esp_now_del_peer(oldest->mac);
355 [ + + ]: 2 : if (ret == ESP_OK) {
356 : 1 : peers_.erase(oldest);
357 : : }
358 : : }
359 : :
360 : : // Add new peer to ESP-NOW driver
361 : 1 : if (ret == ESP_OK) {
362 : 98 : auto peer_info = make_espnow_peer_info(mac);
363 : 98 : ret = hal_espnow_.hal_esp_now_add_peer(&peer_info);
364 : : }
365 : :
366 : : // Create and insert new peer info
367 [ + + ]: 98 : if (ret == ESP_OK) {
368 : 97 : PeerInfo new_peer;
369 : 97 : memcpy(new_peer.mac, mac, 6);
370 : 97 : new_peer.node_id = id;
371 : 97 : new_peer.type = type;
372 : 97 : new_peer.last_seen_ms = 0;
373 : 97 : new_peer.paired = true;
374 : 97 : new_peer.heartbeat_interval_ms = heartbeat_interval_ms;
375 : 97 : peers_.insert(peers_.begin(), new_peer);
376 : 97 : ESP_LOGI(TAG, "New peer added: ID %d", (int)id);
377 : : }
378 : :
379 : 99 : return ret;
380 : : }
|
