Line data Source code
1 : /*
2 : class for networking mapped ports
3 : */
4 :
5 : #include "AP_Networking_Config.h"
6 :
7 : #if AP_NETWORKING_REGISTER_PORT_ENABLED
8 :
9 : #include "AP_Networking.h"
10 : #include <AP_HAL/utility/Socket.h>
11 : #include <GCS_MAVLink/GCS.h>
12 : #include <AP_BoardConfig/AP_BoardConfig.h>
13 : #include <AP_Math/AP_Math.h>
14 : #include <AP_SerialManager/AP_SerialManager.h>
15 : #include <AP_HAL/utility/packetise.h>
16 : #include <errno.h>
17 :
18 : extern const AP_HAL::HAL& hal;
19 :
20 : #ifndef AP_NETWORKING_PORT_MIN_TXSIZE
21 : #define AP_NETWORKING_PORT_MIN_TXSIZE 2048
22 : #endif
23 :
24 : #ifndef AP_NETWORKING_PORT_MIN_RXSIZE
25 : #define AP_NETWORKING_PORT_MIN_RXSIZE 2048
26 : #endif
27 :
28 : #ifndef AP_NETWORKING_PORT_STACK_SIZE
29 : #define AP_NETWORKING_PORT_STACK_SIZE 1300
30 : #endif
31 :
32 : const AP_Param::GroupInfo AP_Networking::Port::var_info[] = {
33 : // @Param: TYPE
34 : // @DisplayName: Port type
35 : // @Description: Port type for network serial port. For the two client types a valid destination IP address must be set. For the two server types either 0.0.0.0 or a local address can be used. The UDP client type will use broadcast if the IP is set to 255.255.255.255 and will use UDP multicast if the IP is in the multicast address range.
36 : // @Values: 0:Disabled, 1:UDP client, 2:UDP server, 3:TCP client, 4:TCP server
37 : // @RebootRequired: True
38 : // @User: Advanced
39 : AP_GROUPINFO_FLAGS("TYPE", 1, AP_Networking::Port, type_param, 0, AP_PARAM_FLAG_ENABLE),
40 :
41 : // @Param: PROTOCOL
42 : // @DisplayName: Protocol
43 : // @Description: Networked serial port protocol
44 : // @User: Advanced
45 : // @RebootRequired: True
46 : // @CopyFieldsFrom: SERIAL1_PROTOCOL
47 : AP_GROUPINFO("PROTOCOL", 2, AP_Networking::Port, state.protocol, 0),
48 :
49 : // @Group: IP
50 : // @Path: AP_Networking_address.cpp
51 : // @RebootRequired : True
52 : AP_SUBGROUPINFO(ip, "IP", 3, AP_Networking::Port, AP_Networking_IPV4),
53 :
54 : // @Param: PORT
55 : // @DisplayName: Port number
56 : // @Description: Port number
57 : // @Range: 0 65535
58 : // @RebootRequired: True
59 : // @User: Advanced
60 : AP_GROUPINFO("PORT", 4, AP_Networking::Port, port, 0),
61 :
62 : AP_GROUPEND
63 : };
64 :
65 : /*
66 : initialise mapped network ports
67 : */
68 8 : void AP_Networking::ports_init(void)
69 : {
70 40 : for (uint8_t i=0; i<ARRAY_SIZE(ports); i++) {
71 32 : auto &p = ports[i];
72 32 : p.type = (NetworkPortType)p.type_param;
73 32 : p.state.idx = AP_SERIALMANAGER_NET_PORT_1 + i;
74 32 : switch (p.type) {
75 19 : case NetworkPortType::NONE:
76 19 : break;
77 3 : case NetworkPortType::UDP_CLIENT:
78 3 : p.udp_client_init();
79 3 : break;
80 1 : case NetworkPortType::UDP_SERVER:
81 1 : p.udp_server_init();
82 1 : break;
83 5 : case NetworkPortType::TCP_SERVER:
84 5 : p.tcp_server_init();
85 5 : break;
86 2 : case NetworkPortType::TCP_CLIENT:
87 2 : p.tcp_client_init();
88 2 : break;
89 : }
90 32 : if (p.sock != nullptr || p.listen_sock != nullptr) {
91 11 : AP::serialmanager().register_port(&p);
92 : }
93 : }
94 8 : }
95 :
96 : /*
97 : wrapper for thread_create for port functions
98 : */
99 11 : void AP_Networking::Port::thread_create(AP_HAL::MemberProc proc)
100 : {
101 11 : const uint8_t idx = state.idx - AP_SERIALMANAGER_NET_PORT_1;
102 11 : hal.util->snprintf(thread_name, sizeof(thread_name), "NET_P%u", unsigned(idx));
103 :
104 11 : if (!init_buffers(AP_NETWORKING_PORT_MIN_RXSIZE, AP_NETWORKING_PORT_MIN_TXSIZE)) {
105 0 : AP_BoardConfig::allocation_error("Failed to allocate %s buffers", thread_name);
106 : return;
107 : }
108 :
109 11 : if (!hal.scheduler->thread_create(proc, thread_name, AP_NETWORKING_PORT_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_UART, 0)) {
110 0 : AP_BoardConfig::allocation_error("Failed to allocate %s client thread", thread_name);
111 : }
112 : }
113 :
114 : /*
115 : initialise a UDP client
116 : */
117 3 : void AP_Networking::Port::udp_client_init(void)
118 : {
119 3 : sock = NEW_NOTHROW SocketAPM(true);
120 3 : if (sock == nullptr) {
121 0 : return;
122 : }
123 3 : sock->set_blocking(false);
124 :
125 : // setup for packet boundaries if this is mavlink
126 6 : packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink ||
127 3 : state.protocol == AP_SerialManager::SerialProtocol_MAVLink2);
128 :
129 3 : thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void));
130 : }
131 :
132 : /*
133 : initialise a UDP server
134 : */
135 1 : void AP_Networking::Port::udp_server_init(void)
136 : {
137 1 : sock = NEW_NOTHROW SocketAPM(true);
138 1 : if (sock == nullptr) {
139 0 : return;
140 : }
141 1 : sock->set_blocking(false);
142 :
143 : // setup for packet boundaries if this is mavlink
144 2 : packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink ||
145 1 : state.protocol == AP_SerialManager::SerialProtocol_MAVLink2);
146 :
147 1 : thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_server_loop, void));
148 : }
149 :
150 : /*
151 : initialise a TCP server
152 : */
153 5 : void AP_Networking::Port::tcp_server_init(void)
154 : {
155 5 : listen_sock = NEW_NOTHROW SocketAPM(false);
156 5 : if (listen_sock == nullptr) {
157 0 : return;
158 : }
159 5 : listen_sock->reuseaddress();
160 :
161 5 : thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_server_loop, void));
162 : }
163 :
164 : /*
165 : initialise a TCP client
166 : */
167 2 : void AP_Networking::Port::tcp_client_init(void)
168 : {
169 2 : sock = NEW_NOTHROW SocketAPM(false);
170 2 : if (sock != nullptr) {
171 2 : sock->set_blocking(true);
172 2 : thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_client_loop, void));
173 : }
174 2 : }
175 :
176 : /*
177 : update a UDP client
178 : */
179 3 : void AP_Networking::Port::udp_client_loop(void)
180 : {
181 3 : AP::network().startup_wait();
182 :
183 3 : const char *dest = ip.get_str();
184 3 : if (!sock->connect(dest, port.get())) {
185 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest);
186 0 : delete sock;
187 0 : sock = nullptr;
188 0 : return;
189 : }
190 :
191 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get()));
192 :
193 3 : connected = true;
194 :
195 3 : bool active = false;
196 : while (true) {
197 81630 : if (!active) {
198 57423 : hal.scheduler->delay_microseconds(100);
199 : }
200 81630 : active = send_receive();
201 : }
202 : }
203 :
204 : /*
205 : update a UDP server
206 : */
207 1 : void AP_Networking::Port::udp_server_loop(void)
208 : {
209 1 : AP::network().startup_wait();
210 :
211 1 : const char *addr = ip.get_str();
212 1 : if (!sock->bind(addr, port.get())) {
213 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get()));
214 0 : delete sock;
215 0 : sock = nullptr;
216 0 : return;
217 : }
218 1 : sock->reuseaddress();
219 :
220 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get()));
221 :
222 1 : bool active = false;
223 : while (true) {
224 23639 : if (!active) {
225 18922 : hal.scheduler->delay_microseconds(100);
226 : }
227 23638 : active = send_receive();
228 : }
229 : }
230 :
231 : /*
232 : update a TCP server
233 : */
234 5 : void AP_Networking::Port::tcp_server_loop(void)
235 : {
236 5 : AP::network().startup_wait();
237 :
238 5 : const char *addr = ip.get_str();
239 5 : if (!listen_sock->bind(addr, port.get()) || !listen_sock->listen(1)) {
240 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get()));
241 0 : delete listen_sock;
242 0 : listen_sock = nullptr;
243 0 : return;
244 : }
245 :
246 5 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get()));
247 :
248 5 : close_on_recv_error = true;
249 :
250 5 : bool active = false;
251 : while (true) {
252 6742 : if (!active) {
253 5761 : hal.scheduler->delay_microseconds(100);
254 : }
255 6739 : if (sock == nullptr) {
256 186 : sock = listen_sock->accept(100);
257 185 : if (sock != nullptr) {
258 5 : sock->set_blocking(false);
259 : char buf[IP4_STR_LEN];
260 : uint16_t last_port;
261 5 : const char *last_addr = listen_sock->last_recv_address(buf, sizeof(buf), last_port);
262 5 : if (last_addr != nullptr) {
263 5 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connection from %s:%u", (unsigned)state.idx, last_addr, unsigned(last_port));
264 : }
265 5 : connected = true;
266 5 : sock->reuseaddress();
267 : }
268 : }
269 6738 : if (sock != nullptr) {
270 6555 : active = send_receive();
271 : }
272 6737 : }
273 : }
274 :
275 : /*
276 : update a TCP client
277 : */
278 2 : void AP_Networking::Port::tcp_client_loop(void)
279 : {
280 2 : AP::network().startup_wait();
281 :
282 2 : close_on_recv_error = true;
283 :
284 2 : bool active = false;
285 : while (true) {
286 3845 : if (!active) {
287 2932 : hal.scheduler->delay_microseconds(100);
288 : }
289 3844 : if (sock == nullptr) {
290 384 : sock = NEW_NOTHROW SocketAPM(false);
291 384 : if (sock == nullptr) {
292 0 : continue;
293 : }
294 384 : sock->set_blocking(true);
295 384 : connected = false;
296 : }
297 3844 : if (!connected) {
298 386 : const char *dest = ip.get_str();
299 386 : connected = sock->connect_timeout(dest, port.get(), 3000);
300 386 : if (connected) {
301 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connected to %s:%u", unsigned(state.idx), dest, unsigned(port.get()));
302 2 : sock->set_blocking(false);
303 : } else {
304 384 : delete sock;
305 384 : sock = nullptr;
306 : // don't try and connect too fast
307 384 : hal.scheduler->delay(100);
308 : }
309 : }
310 3843 : if (sock != nullptr && connected) {
311 3460 : active = send_receive();
312 : }
313 3843 : }
314 : }
315 :
316 : /*
317 : run one send/receive loop
318 : */
319 115271 : bool AP_Networking::Port::send_receive(void)
320 : {
321 :
322 115271 : bool active = false;
323 : uint32_t space;
324 :
325 :
326 : // handle incoming packets
327 : {
328 115271 : WITH_SEMAPHORE(sem);
329 115290 : space = readbuffer->space();
330 115263 : }
331 115292 : if (space > 0) {
332 230552 : const uint32_t n = MIN(300U, space);
333 115265 : uint8_t buf[n];
334 115265 : const auto ret = sock->recv(buf, n, 0);
335 115274 : if (close_on_recv_error && ret == 0) {
336 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: closed connection", unsigned(state.idx));
337 2 : delete sock;
338 2 : sock = nullptr;
339 2 : return false;
340 : }
341 115272 : if (ret > 0) {
342 402 : WITH_SEMAPHORE(sem);
343 402 : readbuffer->write(buf, ret);
344 :
345 : // Cant track dropped read packets because we only read in what there is space for
346 : // The socket buffer becomes full and data is lost there
347 402 : rx_stats_bytes += ret;
348 :
349 402 : active = true;
350 402 : have_received = true;
351 402 : }
352 115287 : }
353 :
354 115281 : if (type == NetworkPortType::UDP_SERVER && have_received) {
355 : // connect the socket to the last receive address if we have one
356 11813 : uint32_t last_addr = 0;
357 11813 : uint16_t last_port = 0;
358 11813 : if (sock->last_recv_address(last_addr, last_port)) {
359 : // we might be disconnected and want to reconnect to a different address/port
360 : // if we haven't received anything for a while
361 11814 : bool maybe_disconnected = (AP_HAL::millis() - last_udp_srv_recv_time_ms) > 3000 &&
362 1 : ((last_addr != last_udp_connect_address) || (last_port != last_udp_connect_port));
363 11813 : if (maybe_disconnected || !connected) {
364 : char last_addr_str[IP4_STR_LEN];
365 1 : sock->inet_addr_to_str(last_addr, last_addr_str, sizeof(last_addr_str));
366 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", unsigned(state.idx), last_addr_str, unsigned(last_port));
367 1 : connected = true;
368 1 : last_udp_connect_address = last_addr;
369 1 : last_udp_connect_port = last_port;
370 : }
371 : // if we received something from the same address, reset the timer
372 11813 : if (((last_addr == last_udp_connect_address) && (last_port == last_udp_connect_port))) {
373 11813 : last_udp_srv_recv_time_ms = AP_HAL::millis();
374 : }
375 : }
376 : }
377 :
378 115281 : if (connected) {
379 : // handle outgoing packets
380 : uint32_t available;
381 :
382 : {
383 103457 : WITH_SEMAPHORE(sem);
384 103464 : available = writebuffer->available();
385 103450 : available = MIN(300U, available);
386 : #if AP_MAVLINK_PACKETISE_ENABLED
387 103457 : if (packetise) {
388 93442 : available = mavlink_packetise(*writebuffer, available);
389 : }
390 : #endif
391 103453 : if (available == 0) {
392 73030 : return active;
393 : }
394 103453 : }
395 30430 : uint8_t buf[available];
396 : uint32_t n;
397 : {
398 30430 : WITH_SEMAPHORE(sem);
399 30428 : n = writebuffer->peekbytes(buf, available);
400 30422 : }
401 :
402 : // nothing to send return
403 30422 : if (n <= 0) {
404 0 : return active;
405 : }
406 :
407 30422 : ssize_t ret = -1;
408 30422 : if (type == NetworkPortType::UDP_SERVER) {
409 : // UDP Server uses sendto, allowing us to change the destination address port on the fly
410 4678 : if(last_udp_connect_address != 0 && last_udp_connect_port != 0) {
411 4678 : ret = sock->sendto(buf, n, last_udp_connect_address, last_udp_connect_port);
412 : }
413 : } else {
414 : // TCP Server and Client and UDP Client use send
415 25744 : ret = sock->send(buf, n);
416 : }
417 :
418 30433 : if (ret > 0) {
419 30433 : WITH_SEMAPHORE(sem);
420 30429 : writebuffer->advance(ret);
421 30424 : tx_stats_bytes += ret;
422 30424 : active = true;
423 30424 : } else if (errno == ENOTCONN &&
424 0 : (type == NetworkPortType::TCP_CLIENT || type == NetworkPortType::TCP_SERVER)) {
425 : // close socket and mark as disconnected, so we can reconnect with another client or when server comes back
426 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: disconnected", unsigned(state.idx));
427 0 : sock->close();
428 0 : delete sock;
429 0 : sock = nullptr;
430 0 : connected = false;
431 : }
432 103457 : }
433 :
434 42248 : return active;
435 : }
436 :
437 : /*
438 : available space in outgoing buffer
439 : */
440 332656 : uint32_t AP_Networking::Port::txspace(void)
441 : {
442 332656 : WITH_SEMAPHORE(sem);
443 332656 : return writebuffer->space();
444 332656 : }
445 :
446 20 : void AP_Networking::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
447 : {
448 20 : rxS = MAX(rxS, AP_NETWORKING_PORT_MIN_RXSIZE);
449 20 : txS = MAX(txS, AP_NETWORKING_PORT_MIN_TXSIZE);
450 20 : init_buffers(rxS, txS);
451 20 : }
452 :
453 101570 : size_t AP_Networking::Port::_write(const uint8_t *buffer, size_t size)
454 : {
455 101570 : WITH_SEMAPHORE(sem);
456 101570 : return writebuffer->write(buffer, size);
457 101570 : }
458 :
459 9485 : ssize_t AP_Networking::Port::_read(uint8_t *buffer, uint16_t count)
460 : {
461 9485 : WITH_SEMAPHORE(sem);
462 9485 : return readbuffer->read(buffer, count);
463 9485 : }
464 :
465 97013 : uint32_t AP_Networking::Port::_available()
466 : {
467 97013 : WITH_SEMAPHORE(sem);
468 97013 : return readbuffer->available();
469 97013 : }
470 :
471 :
472 0 : bool AP_Networking::Port::_discard_input()
473 : {
474 0 : WITH_SEMAPHORE(sem);
475 0 : readbuffer->clear();
476 0 : return true;
477 0 : }
478 :
479 : /*
480 : initialise read/write buffers
481 : */
482 31 : bool AP_Networking::Port::init_buffers(const uint32_t size_rx, const uint32_t size_tx)
483 : {
484 31 : if (size_tx == last_size_tx &&
485 20 : size_rx == last_size_rx) {
486 20 : return true;
487 : }
488 11 : WITH_SEMAPHORE(sem);
489 11 : if (readbuffer == nullptr) {
490 11 : readbuffer = NEW_NOTHROW ByteBuffer(size_rx);
491 : } else {
492 0 : readbuffer->set_size_best(size_rx);
493 : }
494 11 : if (writebuffer == nullptr) {
495 11 : writebuffer = NEW_NOTHROW ByteBuffer(size_tx);
496 : } else {
497 0 : writebuffer->set_size_best(size_tx);
498 : }
499 11 : last_size_rx = size_rx;
500 11 : last_size_tx = size_tx;
501 11 : return readbuffer != nullptr && writebuffer != nullptr;
502 11 : }
503 :
504 : /*
505 : return flow control state
506 : */
507 702 : enum AP_HAL::UARTDriver::flow_control AP_Networking::Port::get_flow_control(void)
508 : {
509 702 : const NetworkPortType ptype = (NetworkPortType)type;
510 702 : switch (ptype) {
511 6 : case NetworkPortType::TCP_CLIENT:
512 : case NetworkPortType::TCP_SERVER:
513 6 : return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE;
514 696 : case NetworkPortType::UDP_CLIENT:
515 : case NetworkPortType::UDP_SERVER:
516 : case NetworkPortType::NONE:
517 696 : break;
518 : }
519 696 : return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
520 : }
521 :
522 : #endif // AP_NETWORKING_REGISTER_PORT_ENABLED
523 :
524 :
|
