Branch data Line data Source code
1 : : // src/discovery_manager.cpp
2 : : #include <cstring>
3 : :
4 : : #define LOG_LOCAL_LEVEL ESP_LOG_INFO
5 : : #include "esp_log.h"
6 : :
7 : : #include "discovery_manager.hpp"
8 : : #include "protocol_types.hpp"
9 : :
10 : : static const char* TAG = "DiscoveryMgr";
11 : :
12 : 37 : DiscoveryManager::DiscoveryManager(
13 : : IWiFiHAL& wifi_hal,
14 : : IEspNowHAL& espnow_hal,
15 : : IMessageCodec& message_codec,
16 : 37 : IFreeRTOSHAL& freertos_hal)
17 : 37 : : hal_wifi_(wifi_hal)
18 : 37 : , hal_espnow_(espnow_hal)
19 : 37 : , message_codec_(message_codec)
20 : 37 : , hal_freertos_(freertos_hal)
21 : : {
22 : 37 : }
23 : :
24 : : // TODO: replace params with EspNowConfig?
25 : : esp_err_t
26 : 36 : DiscoveryManager::init(NodeId id, NodeType type, TaskHandle_t rx_task_handle, UBaseType_t priority, uint32_t stack_size)
27 : : {
28 [ + + ]: 36 : if (rx_task_handle == nullptr) {
29 : 1 : ESP_LOGE(TAG, "RX task handle is required for non-Hub type.");
30 : 1 : return ESP_ERR_INVALID_ARG;
31 : : }
32 : 35 : rx_task_handle_ = rx_task_handle;
33 : :
34 : 35 : BaseType_t ret;
35 : 35 : ret = hal_freertos_.task_create(
36 : : discovery_task_func, "discovery_task", stack_size, this, priority, &discovery_task_handle_);
37 : :
38 [ + + ]: 35 : if (ret != pdPASS) {
39 : : return ESP_ERR_NO_MEM;
40 : : }
41 : :
42 : 34 : my_node_id_ = id;
43 : 34 : my_node_type_ = type;
44 : :
45 [ + + ]: 34 : if (type == ReservedTypes::HUB) {
46 : 8 : hub_ready_ = true;
47 : : }
48 : : else {
49 : 26 : node_ready_ = true;
50 : : }
51 : :
52 : : return ESP_OK;
53 : : }
54 : :
55 : 9 : void DiscoveryManager::deinit()
56 : : {
57 : : // Wake task from any blocking state
58 [ + + ]: 9 : if (discovery_task_handle_ != nullptr) {
59 : 7 : hal_freertos_.task_notify(discovery_task_handle_, NOTIFY_TASK_TO_STOP | NOTIFY_STOP_SCAN, eSetBits);
60 : : }
61 : :
62 : : // Wait for task to exit (worst case: full scan cycle)
63 : : const constexpr uint16_t timeout_ms = SCAN_CHANNEL_TIMEOUT_MS * SCAN_CHANNEL_ATTEMPTS * 13 + 100;
64 : : const constexpr uint8_t delay_ms = 50;
65 [ + + ]: 54 : for (int elapsed_ms = 0; elapsed_ms < timeout_ms; elapsed_ms += delay_ms) {
66 [ + + ]: 51 : if (discovery_task_handle_ == nullptr) {
67 : : break;
68 : : }
69 : 45 : hal_freertos_.task_delay(pdMS_TO_TICKS(delay_ms));
70 : : }
71 : :
72 : : // Force cleanup if task didn't exit gracefully
73 [ + + ]: 9 : if (discovery_task_handle_ != nullptr) {
74 : 3 : hal_freertos_.task_suspend(discovery_task_handle_);
75 : 3 : hal_freertos_.task_delete(discovery_task_handle_);
76 : 3 : discovery_task_handle_ = nullptr;
77 : : }
78 : 9 : }
79 : :
80 : 6 : void DiscoveryManager::start_scan()
81 : : {
82 [ + + + + ]: 6 : if (!node_ready_ || discovery_task_handle_ == nullptr) {
83 : 2 : ESP_LOGE(TAG, "DiscoveryManager not initialized properly. Call init() before start_scan().");
84 : 2 : return;
85 : : }
86 : 4 : hal_freertos_.task_notify(discovery_task_handle_, NOTIFY_START_SCAN, eSetBits);
87 : : }
88 : :
89 : 4 : void DiscoveryManager::stop_scan()
90 : : {
91 [ + + + + ]: 4 : if (!node_ready_ || discovery_task_handle_ == nullptr) {
92 : 2 : ESP_LOGE(TAG, "DiscoveryManager not initialized properly. Call init() before stop_scan().");
93 : 2 : return;
94 : : }
95 : 2 : hal_freertos_.task_notify(discovery_task_handle_, NOTIFY_STOP_SCAN, eSetBits);
96 : : }
97 : :
98 : 6 : void DiscoveryManager::handle_scan_probe(const DecodedRxPacket& decoded)
99 : : {
100 [ + + + + ]: 6 : if (!hub_ready_ || discovery_task_handle_ == nullptr) {
101 : 2 : ESP_LOGE(TAG, "DiscoveryManager not initialized properly. Call init() before handle_probe().");
102 : 2 : return;
103 : : }
104 : :
105 : 4 : destination_node_id_ = decoded.header.sender_node_id;
106 : 4 : hal_freertos_.task_notify(discovery_task_handle_, NOTIFY_SCAN_RESPONSE, eSetBits);
107 : : }
108 : :
109 : 4 : void DiscoveryManager::handle_scan_response(const DecodedRxPacket& decoded)
110 : : {
111 [ + + + + ]: 4 : if (!node_ready_ || discovery_task_handle_ == nullptr) {
112 : : return;
113 : : }
114 : 2 : ESP_LOGI(TAG, "Scan response received, notifying discovery task");
115 : 2 : hal_freertos_.task_notify(discovery_task_handle_, NOTIFY_LINK_ALIVE, eSetBits);
116 : : }
117 : :
118 : 12 : void DiscoveryManager::set_channel(uint8_t channel)
119 : : {
120 [ + + ]: 12 : if (channel >= 1 && channel <= 13) {
121 : 10 : current_channel_.store(channel);
122 : : }
123 : : else {
124 : 2 : ESP_LOGW(TAG, "Invalid channel %d, must be 1-13", channel);
125 : : }
126 : 12 : }
127 : :
128 : 4 : void DiscoveryManager::discovery_task_func(void* arg)
129 : : {
130 : 4 : DiscoveryManager* self = static_cast<DiscoveryManager*>(arg);
131 : 4 : self->discovery_task();
132 : 0 : }
133 : :
134 : 4 : void DiscoveryManager::discovery_task()
135 : : {
136 : 4 : bool should_stop = false;
137 : 4 : uint32_t notifications = 0;
138 : :
139 [ + + ]: 12 : while (!should_stop) {
140 : : // Wait blocked for any notification
141 : 8 : hal_freertos_.task_notify_wait(0, NOTIFY_ALL, ¬ifications, portMAX_DELAY);
142 : :
143 [ + + ]: 8 : if ((notifications & NOTIFY_TASK_TO_STOP) == NOTIFY_TASK_TO_STOP) {
144 : 4 : should_stop = true;
145 : : }
146 [ + + ]: 8 : if ((notifications & NOTIFY_START_SCAN) == NOTIFY_START_SCAN) {
147 [ + + ]: 3 : if (scan_channel() == ESP_OK) {
148 : 1 : notify_rx_task(NOTIFY_CHANNEL_FOUND);
149 : : }
150 : : else {
151 : 2 : notify_rx_task(NOTIFY_SCAN_FAILED);
152 : : }
153 : : }
154 [ + + ]: 8 : if ((notifications & NOTIFY_SCAN_RESPONSE) == NOTIFY_SCAN_RESPONSE) {
155 : 1 : send_scan_response();
156 : : }
157 : : }
158 : :
159 : 4 : ESP_LOGI(TAG, "Discovery Manager task exiting.");
160 : 4 : discovery_task_handle_ = nullptr;
161 : 4 : hal_freertos_.task_suspend(nullptr); // NULL / nullptr == current task
162 : 0 : hal_freertos_.task_delete(nullptr); // NULL / nullptr == current task
163 : 0 : }
164 : :
165 : : // ==========================================================================
166 : : // Private methods
167 : : // ==========================================================================
168 : :
169 : 8 : esp_err_t DiscoveryManager::scan_channel()
170 : : {
171 : 8 : ESP_LOGI(TAG, "Starting channel scan to find Hub.");
172 : 8 : is_scanning_.store(true);
173 : 8 : esp_err_t ret = ESP_FAIL;
174 : :
175 : : // We will start from actual channel (most likely to be the correct one)
176 [ + + ]: 41 : for (uint8_t offset = 0; offset < 13 && ret != ESP_OK; ++offset) {
177 : 35 : uint8_t channel = ((current_channel_.load() - 1 + offset) % 13) + 1;
178 : 35 : ESP_LOGD(TAG, "Scanning channel %d", channel);
179 : :
180 : : // Set wifi channel to make probes attempts on this channel
181 [ + + ]: 35 : if (hal_wifi_.wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE) != ESP_OK) {
182 : 1 : ESP_LOGE(TAG, "Failed to set WiFi channel to %d", channel);
183 : 1 : continue;
184 : 1 : }
185 : :
186 : : // We made SCAN_CHANNEL_ATTEMPTS per channel
187 [ + + ]: 62 : for (uint8_t attempt = 0; attempt < SCAN_CHANNEL_ATTEMPTS && ret != ESP_OK; attempt++) {
188 : : // Send probe on channel
189 [ + + ]: 34 : if (send_scan_probe() != ESP_OK) {
190 : 14 : continue;
191 : : }
192 : :
193 : : // Verify if scan should stop
194 [ + + ]: 20 : if (should_stop_scan()) {
195 : 2 : is_scanning_.store(false);
196 : 2 : return ESP_FAIL;
197 : : }
198 : :
199 : : // Wait for hub to respond
200 [ + + ]: 18 : if (hub_was_found()) {
201 : 4 : current_channel_.store(channel);
202 : 4 : ESP_LOGI(TAG, "Hub found on channel %d", channel);
203 : 4 : ret = ESP_OK;
204 : 4 : is_scanning_.store(false);
205 : 4 : break;
206 : : }
207 : : }
208 : : }
209 : 6 : is_scanning_.store(false);
210 : 6 : return ret;
211 : : }
212 : :
213 : 36 : esp_err_t DiscoveryManager::send_scan_probe()
214 : : {
215 : 36 : MessageHeader probe_header = make_probe_header();
216 : 36 : uint8_t buffer[ESP_NOW_MAX_DATA_LEN];
217 : 36 : size_t encoded_len = message_codec_.encode(probe_header, nullptr, 0, buffer, sizeof(buffer));
218 [ + + ]: 36 : if (encoded_len == 0) {
219 : : return ESP_FAIL;
220 : : }
221 : :
222 : 35 : return hal_espnow_.hal_esp_now_send(BROADCAST_MAC, buffer, encoded_len);
223 : : }
224 : :
225 : 36 : MessageHeader DiscoveryManager::make_probe_header()
226 : : {
227 : : // empty initializer to avoid memory garbage on unused fields
228 : 36 : MessageHeader resp = {};
229 : 36 : resp.msg_type = MessageType::CHANNEL_SCAN_PROBE;
230 : 36 : resp.sender_node_id = my_node_id_;
231 : 36 : resp.sender_type = my_node_type_;
232 : 36 : resp.dest_node_id = ReservedIds::HUB;
233 : 36 : resp.timestamp_ms = 0;
234 : :
235 : 36 : return resp;
236 : : }
237 : :
238 : 18 : bool DiscoveryManager::hub_was_found()
239 : : {
240 : 18 : uint32_t notifications = 0;
241 : 18 : hal_freertos_.task_notify_wait(0, NOTIFY_LINK_ALIVE, ¬ifications, pdMS_TO_TICKS(SCAN_CHANNEL_TIMEOUT_MS));
242 : 18 : return (notifications & NOTIFY_LINK_ALIVE) == NOTIFY_LINK_ALIVE;
243 : : }
244 : :
245 : 20 : bool DiscoveryManager::should_stop_scan()
246 : : {
247 : 20 : uint32_t notifications = 0;
248 : 20 : hal_freertos_.task_notify_wait(0, NOTIFY_STOP_SCAN, ¬ifications, 0);
249 : 20 : return (notifications & NOTIFY_STOP_SCAN) == NOTIFY_STOP_SCAN;
250 : : }
251 : :
252 : 3 : void DiscoveryManager::notify_rx_task(uint32_t notification)
253 : : {
254 [ + - ]: 3 : if (rx_task_handle_ != nullptr) {
255 : 3 : hal_freertos_.task_notify(rx_task_handle_, notification, eSetBits);
256 : : }
257 : 3 : }
258 : :
259 : 3 : esp_err_t DiscoveryManager::send_scan_response()
260 : : {
261 : 3 : ESP_LOGD(TAG, "Sending channel scan response to node_id=%d", destination_node_id_);
262 : :
263 : 3 : MessageHeader response_header = make_response_header();
264 : :
265 : 3 : uint8_t buffer[ESP_NOW_MAX_DATA_LEN];
266 : 3 : size_t encoded_len = message_codec_.encode(response_header, nullptr, 0, buffer, sizeof(buffer));
267 [ + + ]: 3 : if (encoded_len == 0) {
268 : : return ESP_FAIL;
269 : : }
270 : :
271 : : // Send via broadcast — probing node maybe is not yet a registered ESP-NOW peer
272 : 2 : return hal_espnow_.hal_esp_now_send(BROADCAST_MAC, buffer, encoded_len);
273 : : }
274 : :
275 : 3 : MessageHeader DiscoveryManager::make_response_header()
276 : : {
277 : 3 : MessageHeader resp = {};
278 : 3 : resp.msg_type = MessageType::CHANNEL_SCAN_RESPONSE;
279 : 3 : resp.sender_node_id = my_node_id_;
280 : 3 : resp.sender_type = my_node_type_;
281 : 3 : resp.dest_node_id = destination_node_id_;
282 : 3 : resp.timestamp_ms = 0;
283 : :
284 : 3 : return resp;
285 : : }
|
