Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // Copyright (c) 2010 Michael Smith. All rights reserved.
18 : //
19 : #include <AP_HAL/AP_HAL.h>
20 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
21 :
22 : #include <limits.h>
23 : #include <stdlib.h>
24 : #include <stdio.h>
25 : #include <unistd.h>
26 : #include <fcntl.h>
27 : #include <stdarg.h>
28 : #include <AP_Math/AP_Math.h>
29 :
30 : #include <errno.h>
31 : #include <sys/ioctl.h>
32 : #include <sys/stat.h>
33 : #include <sys/types.h>
34 : #include <sys/socket.h>
35 : #include <sys/un.h>
36 : #include <netinet/in.h>
37 : #include <netinet/tcp.h>
38 : #include <sys/select.h>
39 : #include <termios.h>
40 : #include <sys/time.h>
41 : #include <arpa/inet.h>
42 :
43 : #include "UARTDriver.h"
44 : #include "SITL_State.h"
45 : #if HAL_GCS_ENABLED
46 : #include <AP_HAL/utility/packetise.h>
47 : #endif
48 :
49 : #include <AP_Vehicle/AP_Vehicle_Type.h>
50 : #include <AP_Filesystem/AP_Filesystem.h>
51 : #include <AP_Common/ExpandingString.h>
52 : #include <AP_HAL/utility/Socket_native.h>
53 :
54 : extern const AP_HAL::HAL& hal;
55 :
56 : using namespace HALSITL;
57 :
58 : bool UARTDriver::_console;
59 :
60 : /* UARTDriver method implementations */
61 :
62 542778 : void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
63 : {
64 542778 : if (baud == 0 && rxSpace == 0 && txSpace == 0) {
65 : // this is a claim of the uart for the current thread, which
66 : // is currently not implemented in SITL
67 506810 : return;
68 : }
69 :
70 35968 : if (_portNumber >= ARRAY_SIZE(_sitlState->_serial_path)) {
71 0 : AP_HAL::panic("port number out of range; you may need to extend _sitlState->_serial_path");
72 : }
73 :
74 35968 : const char *path = _sitlState->_serial_path[_portNumber];
75 :
76 35968 : if (baud != 0) {
77 35963 : _uart_baudrate = baud;
78 : }
79 :
80 35968 : if (strcmp(path, "GPS1") == 0) {
81 : /* gps */
82 12464 : _connected = true;
83 12464 : _sim_serial_device = _sitlState->create_serial_sim("gps", "1", _portNumber);
84 23504 : } else if (strcmp(path, "GPS2") == 0) {
85 : /* 2nd gps */
86 2066 : _connected = true;
87 2066 : _sim_serial_device = _sitlState->create_serial_sim("gps", "2", _portNumber);
88 : } else {
89 : /* parse type:args:flags string for path.
90 : For example:
91 : tcp:5760:wait // tcp listen on port 5760
92 : tcp:0:wait // tcp listen on use base_port + 0
93 : uds:APM-UDS-serial0:wait
94 : tcpclient:192.168.2.15:5762
95 : udpclient:127.0.0.1
96 : udpclient:127.0.0.1:14550
97 : mcast:
98 : mcast:239.255.145.50:14550
99 : uart:/dev/ttyUSB0:57600
100 : sim:ParticleSensor_SDS021:
101 : file:/tmp/my-device-capture.BIN
102 : logic_async_csv:/tmp/logic_async.csv:
103 : */
104 21438 : char *saveptr = nullptr;
105 21438 : char *s = strdup(path);
106 21438 : char *devtype = strtok_r(s, ":", &saveptr);
107 21438 : char *args1 = strtok_r(nullptr, ":", &saveptr);
108 21438 : char *args2 = strtok_r(nullptr, ":", &saveptr);
109 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
110 17388 : if (_portNumber == 1 && AP::sitl()->adsb_plane_count >= 0) {
111 : // this is ordinarily port 5762. The ADSB simulation assumed
112 : // this port, so if enabled we assume we'll be doing ADSB...
113 : // add sanity check here that we're doing mavlink on this port?
114 6 : ::printf("SIM-ADSB connection on SERIAL%u\n", _portNumber);
115 6 : _connected = true;
116 6 : _sim_serial_device = _sitlState->create_serial_sim("adsb", nullptr, _portNumber);
117 : } else
118 : #endif
119 21432 : if (strcmp(devtype, "tcp") == 0) {
120 21028 : uint16_t port = atoi(args1);
121 21028 : bool wait = (args2 && strcmp(args2, "wait") == 0);
122 21028 : _tcp_start_connection(port, wait);
123 404 : } else if (strcmp(devtype, "uds") == 0) {
124 0 : if (args1 == nullptr || args1[0] == '\0') {
125 0 : AP_HAL::panic("Invalid Unix domain socket path: %s", path);
126 : }
127 0 : bool wait = (args2 && strcmp(args2, "wait") == 0);
128 0 : _unix_start_connection(args1, wait);
129 404 : } else if (strcmp(devtype, "tcpclient") == 0) {
130 1 : if (args2 == nullptr) {
131 0 : AP_HAL::panic("Invalid tcp client path: %s", path);
132 : }
133 1 : uint16_t port = atoi(args2);
134 1 : _tcp_start_client(args1, port);
135 403 : } else if (strcmp(devtype, "uart") == 0) {
136 0 : uint32_t baudrate = args2? atoi(args2) : baud;
137 0 : if (_uart_path == nullptr) {
138 0 : ::printf("UART connection %s:%u\n", args1, baudrate);
139 0 : _uart_path = strdup(args1);
140 : }
141 0 : _uart_baudrate = baudrate;
142 0 : _uart_start_connection();
143 403 : } else if (strcmp(devtype, "sim") == 0) {
144 397 : if (!_connected) {
145 232 : ::printf("SIM connection %s:%s on SERIAL%u\n", args1, args2, _portNumber);
146 232 : _connected = true;
147 232 : _sim_serial_device = _sitlState->create_serial_sim(args1, args2, _portNumber);
148 : }
149 6 : } else if (strcmp(devtype, "udpclient") == 0) {
150 : // udp client connection
151 0 : const char *ip = args1;
152 0 : uint16_t port = args2?atoi(args2):14550;
153 0 : if (!_connected) {
154 0 : ::printf("UDP connection %s:%u\n", ip, port);
155 0 : _udp_start_client(ip, port);
156 : }
157 6 : } else if (strcmp(devtype, "mcast") == 0) {
158 : // udp multicast connection
159 6 : const char *ip = args1 && *args1?args1:mcast_ip_default;
160 6 : uint16_t port = args2?atoi(args2):mcast_port_default;
161 6 : if (!_connected) {
162 2 : ::printf("UDP multicast connection %s:%u\n", ip, port);
163 2 : _udp_start_multicast(ip, port);
164 : }
165 0 : } else if (strcmp(devtype,"none") == 0) {
166 : // skipping port
167 0 : ::printf("Skipping port %s\n", args1);
168 0 : } else if (strcmp(devtype, "file") == 0) {
169 0 : if (_connected) {
170 0 : AP::FS().close(_fd);
171 : }
172 0 : ::printf("FILE connection %s\n", args1);
173 0 : _fd = AP::FS().open(args1, O_RDONLY);
174 0 : if (_fd == -1) {
175 0 : AP_HAL::panic("Failed to open (%s): %m", args1);
176 : }
177 0 : _connected = true;
178 0 : } else if (strcmp(devtype, "outfile") == 0) {
179 0 : if (_connected) {
180 0 : AP::FS().close(_fd);
181 : }
182 0 : ::printf("FILE output connection %s\n", args1);
183 0 : _fd = AP::FS().open(args1, O_WRONLY|O_CREAT|O_TRUNC, 0644);
184 0 : if (_fd == -1) {
185 0 : AP_HAL::panic("Failed to open (%s): %m", args1);
186 : }
187 0 : _connected = true;
188 0 : } else if (strcmp(devtype, "logic_async_csv") == 0) {
189 0 : if (_connected) {
190 0 : AP::FS().close(_fd);
191 : }
192 0 : ::printf("logic_async_csv connection %s\n", args1);
193 0 : _fd = AP::FS().open(args1, O_RDONLY);
194 0 : if (_fd == -1) {
195 0 : AP_HAL::panic("Failed to open (%s): %m", args1);
196 : }
197 0 : _connected = true;
198 0 : logic_async_csv.active = true;
199 : } else {
200 0 : AP_HAL::panic("Invalid device path: %s", path);
201 : }
202 21438 : free(s);
203 : }
204 :
205 35968 : if (_sim_serial_device != nullptr) {
206 14933 : _sim_serial_device->set_autopilot_baud(baud);
207 : }
208 :
209 35968 : if (hal.console != this) { // don't clear USB buffers (allows early startup messages to escape)
210 27592 : _readbuffer.clear();
211 27592 : _writebuffer.clear();
212 : }
213 : }
214 :
215 0 : void UARTDriver::_end()
216 : {
217 0 : }
218 :
219 136181823 : uint32_t UARTDriver::_available(void)
220 : {
221 136181823 : _check_connection();
222 :
223 136181828 : if (!_connected) {
224 82343345 : return 0;
225 : }
226 :
227 53838483 : return _readbuffer.available();
228 : }
229 :
230 430642998 : uint32_t UARTDriver::txspace(void)
231 : {
232 430642998 : _check_connection();
233 430642989 : if (!_connected) {
234 168060453 : return 0;
235 : }
236 262582536 : return _writebuffer.space();
237 : }
238 :
239 39949 : void UARTDriver::set_flow_control(enum flow_control flow_control_setting)
240 : {
241 39949 : _flow_control = flow_control_setting;
242 39949 : if (_flow_control == FLOW_CONTROL_AUTO) {
243 : // reset auto-detection state
244 0 : _auto_flow_start_ms = 0;
245 0 : _auto_flow_detected = false;
246 : }
247 : // If already connected on a real UART path, apply immediately
248 39949 : if (_uart_path && _connected && _fd != -1) {
249 0 : struct termios t {};
250 0 : if (tcgetattr(_fd, &t) == 0) {
251 0 : if (_flow_control != FLOW_CONTROL_DISABLE) {
252 0 : t.c_cflag |= CRTSCTS;
253 : } else {
254 0 : t.c_cflag &= ~CRTSCTS;
255 : }
256 0 : tcsetattr(_fd, TCSANOW, &t);
257 : }
258 : }
259 39949 : }
260 :
261 13351 : enum AP_HAL::UARTDriver::flow_control UARTDriver::get_flow_control(void)
262 : {
263 13351 : if (_uart_path) {
264 0 : return _flow_control;
265 : }
266 13351 : return FLOW_CONTROL_ENABLE;
267 : }
268 :
269 444364572 : ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t count)
270 : {
271 444364572 : const ssize_t ret = _readbuffer.read(buffer, count);
272 444363251 : _rx_stats_bytes += ret;
273 444363251 : return ret;
274 : }
275 :
276 191581 : bool UARTDriver::_discard_input(void)
277 : {
278 191581 : _readbuffer.clear();
279 191581 : return true;
280 : }
281 :
282 1639 : void UARTDriver::_flush(void)
283 : {
284 : // flush the write buffer - but don't fail and don't
285 : // infinitely-loop. This is not a good definition of "flush", but
286 : // it was judged that we had to return from this function even if
287 : // we hadn't actually done our job.
288 1639 : uint32_t start_ms = AP_HAL::millis();
289 3278 : while (AP_HAL::millis() - start_ms < 1000) {
290 3278 : if (_writebuffer.available() == 0) {
291 1639 : break;
292 : }
293 1639 : _timer_tick();
294 : }
295 :
296 : // ensure that the outbound TCP queue is also empty...
297 1639 : start_ms = AP_HAL::millis();
298 3270 : while (AP_HAL::millis() - start_ms < 1000) {
299 3270 : if (((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length() == 0) {
300 1639 : break;
301 : }
302 1631 : usleep(1000);
303 : }
304 1639 : }
305 :
306 117372606 : size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
307 : {
308 117372606 : const auto _txspace = txspace();
309 117372603 : if (_txspace < size) {
310 29170 : size = _txspace;
311 : }
312 117372603 : if (size <= 0) {
313 1345947 : return 0;
314 : }
315 :
316 : /*
317 : simulate byte loss at the link layer
318 : */
319 116026656 : uint8_t lost_byte = 0;
320 : #if !defined(HAL_BUILD_AP_PERIPH)
321 116026656 : SITL::SIM *_sitl = AP::sitl();
322 :
323 116026655 : if (_sitl && _sitl->uart_byte_loss_pct > 0) {
324 0 : if (fabsf(rand_float()) < _sitl->uart_byte_loss_pct.get() * 0.01 * size) {
325 0 : lost_byte = 1;
326 : }
327 : }
328 : #endif // HAL_BUILD_AP_PERIPH
329 :
330 : // Include lost byte in tx count, we think we sent it even though it was never added to the write buffer
331 116026654 : _tx_stats_bytes += lost_byte;
332 :
333 116026654 : const size_t ret = _writebuffer.write(buffer, size - lost_byte) + lost_byte;
334 116026654 : if (_unbuffered_writes) {
335 0 : handle_writing_from_writebuffer_to_device();
336 : }
337 116026654 : return ret;
338 : }
339 :
340 :
341 : /*
342 : start a TCP connection for the serial port. If wait_for_connection
343 : is true then block until a client connects
344 : */
345 21028 : void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection)
346 : {
347 21028 : int one=1;
348 : int ret;
349 21028 : struct sockaddr_in _listen_sockaddr {};
350 :
351 21028 : if (_connected) {
352 6183 : return;
353 : }
354 :
355 14845 : _use_send_recv = true;
356 :
357 14845 : if (_console) {
358 : // hack for console access
359 0 : _connected = true;
360 0 : _use_send_recv = false;
361 0 : _listen_fd = -1;
362 0 : _fd = 1;
363 0 : return;
364 : }
365 :
366 14845 : if (_fd != -1) {
367 0 : close(_fd);
368 : }
369 :
370 14845 : if (_listen_fd == -1) {
371 6320 : memset(&_listen_sockaddr,0,sizeof(_listen_sockaddr));
372 :
373 : #ifdef HAVE_SOCK_SIN_LEN
374 : _listen_sockaddr.sin_len = sizeof(_listen_sockaddr);
375 : #endif
376 6320 : if (port > 1000) {
377 15 : _listen_sockaddr.sin_port = htons(port);
378 : } else {
379 6305 : _listen_sockaddr.sin_port = htons(_sitlState->base_port() + port);
380 : }
381 6320 : _listen_sockaddr.sin_family = AF_INET;
382 :
383 6320 : _listen_fd = socket(AF_INET, SOCK_STREAM, 0);
384 6320 : if (_listen_fd == -1) {
385 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
386 0 : exit(1);
387 : }
388 6320 : ret = fcntl(_listen_fd, F_SETFD, FD_CLOEXEC);
389 6320 : if (ret == -1) {
390 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
391 0 : exit(1);
392 : }
393 :
394 : /* we want to be able to re-use ports quickly */
395 6320 : if (setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
396 0 : fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
397 0 : exit(1);
398 : }
399 :
400 6320 : fprintf(stderr, "bind port %u for SERIAL%u\n",
401 6320 : (unsigned)ntohs(_listen_sockaddr.sin_port),
402 6320 : (unsigned)_portNumber);
403 :
404 6320 : ret = bind(_listen_fd, (struct sockaddr *)&_listen_sockaddr, sizeof(_listen_sockaddr));
405 6320 : if (ret == -1) {
406 0 : fprintf(stderr, "bind failed on port %u - %s\n",
407 0 : (unsigned)ntohs(_listen_sockaddr.sin_port),
408 0 : strerror(errno));
409 0 : exit(1);
410 : }
411 :
412 6320 : ret = listen(_listen_fd, 5);
413 6320 : if (ret == -1) {
414 0 : fprintf(stderr, "listen failed - %s\n", strerror(errno));
415 0 : exit(1);
416 : }
417 :
418 6320 : fprintf(stderr, "SERIAL%u on TCP port %u\n", _portNumber,
419 6320 : (unsigned)ntohs(_listen_sockaddr.sin_port));
420 6320 : fflush(stdout);
421 : }
422 :
423 14845 : if (wait_for_connection) {
424 2199 : fprintf(stdout, "Waiting for connection ....\n");
425 2199 : fflush(stdout);
426 2199 : _fd = accept(_listen_fd, nullptr, nullptr);
427 2199 : if (_fd == -1) {
428 0 : fprintf(stderr, "accept() error - %s", strerror(errno));
429 0 : exit(1);
430 : }
431 2199 : setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
432 2199 : setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
433 2199 : fcntl(_fd, F_SETFD, FD_CLOEXEC);
434 2199 : _connected = true;
435 2199 : fprintf(stdout, "Connection on serial port %u\n", (unsigned)ntohs(_listen_sockaddr.sin_port));
436 : }
437 : }
438 :
439 :
440 : /*
441 : start a Unix domain socket connection for the serial port. If
442 : wait_for_connection is true then block until a client connects
443 : */
444 0 : void UARTDriver::_unix_start_connection(const char *path, bool wait_for_connection)
445 : {
446 0 : struct sockaddr_un listen_sockaddr {};
447 :
448 0 : if (_connected) {
449 0 : return;
450 : }
451 :
452 0 : _use_send_recv = true;
453 0 : _is_unix_socket = true;
454 :
455 0 : if (_console) {
456 0 : _connected = true;
457 0 : _use_send_recv = false;
458 0 : _listen_fd = -1;
459 0 : _fd = 1;
460 0 : return;
461 : }
462 :
463 0 : if (_fd != -1) {
464 0 : close(_fd);
465 : }
466 :
467 0 : if (_listen_fd == -1) {
468 0 : if (strlen(path) >= sizeof(listen_sockaddr.sun_path)) {
469 0 : fprintf(stderr, "Unix domain socket path is too long: %s\n", path);
470 0 : exit(1);
471 : }
472 :
473 0 : listen_sockaddr.sun_family = AF_UNIX;
474 0 : strncpy(listen_sockaddr.sun_path, path, sizeof(listen_sockaddr.sun_path) - 1);
475 :
476 0 : _listen_fd = socket(AF_UNIX, SOCK_STREAM, 0);
477 0 : if (_listen_fd == -1) {
478 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
479 0 : exit(1);
480 : }
481 0 : if (fcntl(_listen_fd, F_SETFD, FD_CLOEXEC) == -1) {
482 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
483 0 : exit(1);
484 : }
485 :
486 0 : int ret = bind(_listen_fd, (struct sockaddr *)&listen_sockaddr, sizeof(listen_sockaddr));
487 0 : if (ret == -1 && errno == EADDRINUSE) {
488 : struct stat path_stat;
489 0 : if (lstat(path, &path_stat) == 0 && S_ISSOCK(path_stat.st_mode)) {
490 0 : int probe_fd = socket(AF_UNIX, SOCK_STREAM, 0);
491 0 : int connect_ret = probe_fd == -1 ? -1 : connect(
492 : probe_fd,
493 : (struct sockaddr *)&listen_sockaddr,
494 0 : sizeof(listen_sockaddr));
495 0 : int connect_errno = errno;
496 0 : if (probe_fd != -1) {
497 0 : close(probe_fd);
498 : }
499 0 : if (connect_ret == -1 && connect_errno == ECONNREFUSED) {
500 : // A newly bound stream socket also refuses connections in
501 : // the brief interval before listen(). Give it time to
502 : // become live before treating the path as stale.
503 0 : usleep(100000);
504 0 : probe_fd = socket(AF_UNIX, SOCK_STREAM, 0);
505 0 : connect_ret = probe_fd == -1 ? -1 : connect(
506 : probe_fd,
507 : (struct sockaddr *)&listen_sockaddr,
508 : sizeof(listen_sockaddr));
509 0 : connect_errno = errno;
510 0 : if (probe_fd != -1) {
511 0 : close(probe_fd);
512 : }
513 :
514 : struct stat current_path_stat;
515 0 : if (connect_ret == -1 && connect_errno == ECONNREFUSED &&
516 0 : lstat(path, ¤t_path_stat) == 0 &&
517 0 : S_ISSOCK(current_path_stat.st_mode) &&
518 0 : current_path_stat.st_dev == path_stat.st_dev &&
519 0 : current_path_stat.st_ino == path_stat.st_ino &&
520 0 : unlink(path) == 0) {
521 0 : ret = bind(_listen_fd, (struct sockaddr *)&listen_sockaddr, sizeof(listen_sockaddr));
522 : }
523 : }
524 : }
525 : }
526 0 : if (ret == -1) {
527 0 : fprintf(stderr, "bind failed on Unix domain socket %s - %s\n", path, strerror(errno));
528 0 : exit(1);
529 : }
530 0 : SocketAPM_native::register_unix_path(_listen_fd, path);
531 :
532 0 : if (listen(_listen_fd, 5) == -1) {
533 0 : fprintf(stderr, "listen failed - %s\n", strerror(errno));
534 0 : exit(1);
535 : }
536 :
537 0 : fprintf(stderr, "SERIAL%u on Unix domain socket %s\n", _portNumber, path);
538 0 : fflush(stdout);
539 : }
540 :
541 0 : if (wait_for_connection) {
542 0 : fprintf(stdout, "Waiting for connection ....\n");
543 0 : fflush(stdout);
544 0 : _fd = accept(_listen_fd, nullptr, nullptr);
545 0 : if (_fd == -1) {
546 0 : fprintf(stderr, "accept() error - %s", strerror(errno));
547 0 : exit(1);
548 : }
549 0 : if (fcntl(_fd, F_SETFD, FD_CLOEXEC) == -1) {
550 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
551 0 : exit(1);
552 : }
553 0 : _connected = true;
554 0 : fprintf(stdout, "Connection on Unix domain socket for SERIAL%u\n", _portNumber);
555 : }
556 : }
557 :
558 :
559 : /*
560 : start a TCP client connection for the serial port.
561 : */
562 1 : void UARTDriver::_tcp_start_client(const char *address, uint16_t port)
563 : {
564 :
565 1 : if (_connected) {
566 0 : return;
567 : }
568 :
569 1 : _use_send_recv = true;
570 :
571 1 : if (_fd != -1) {
572 0 : close(_fd);
573 : }
574 :
575 : struct sockaddr_in sockaddr;
576 1 : memset(&sockaddr,0,sizeof(sockaddr));
577 :
578 : #ifdef HAVE_SOCK_SIN_LEN
579 : sockaddr.sin_len = sizeof(sockaddr);
580 : #endif
581 1 : sockaddr.sin_port = htons(port);
582 1 : sockaddr.sin_family = AF_INET;
583 1 : sockaddr.sin_addr.s_addr = inet_addr(address);
584 :
585 1 : constexpr auto one=1;
586 : int ret;
587 : // Retry for up to 30 seconds. Companion processes (e.g. AP_Periph
588 : // connecting to an ArduPlane SITL TCP listener for a PPP link) can be
589 : // launched ahead of the peer's socket, so a few seconds of grace is
590 : // not enough when the peer is also rebuilding.
591 1 : constexpr uint8_t max_attempts = 30;
592 1 : for (uint8_t attempt = 0; attempt < max_attempts; ++attempt) {
593 1 : _fd = socket(AF_INET, SOCK_STREAM, 0);
594 1 : if (_fd == -1) {
595 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
596 0 : exit(1);
597 : }
598 1 : ret = fcntl(_fd, F_SETFD, FD_CLOEXEC);
599 1 : if (ret == -1) {
600 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
601 0 : exit(1);
602 : }
603 :
604 : /* we want to be able to re-use ports quickly */
605 1 : setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
606 :
607 1 : ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
608 1 : if (ret == 0) {
609 1 : break;
610 : }
611 0 : fprintf(stderr, "connect failed on port %u - %s at %d retrying\n",
612 0 : (unsigned) ntohs(sockaddr.sin_port), strerror(errno), AP_HAL::millis());
613 0 : close(_fd);
614 : // If connection failed, wait for a bit before retrying
615 0 : sleep(1);
616 : }
617 :
618 1 : if (ret == -1) {
619 0 : close(_fd);
620 0 : fprintf(stderr, "connect failed on port %u - %s\n",
621 0 : (unsigned)ntohs(sockaddr.sin_port),
622 0 : strerror(errno));
623 0 : exit(1);
624 : }
625 :
626 1 : setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
627 1 : setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
628 1 : fcntl(_fd, F_SETFD, FD_CLOEXEC);
629 1 : _connected = true;
630 1 : fprintf(stdout, "New remote connection on serial port %u, p %u at %d\n", _portNumber, (unsigned) ntohs(sockaddr.sin_port), AP_HAL::millis());
631 : }
632 :
633 :
634 : /*
635 : start a UDP client connection for the serial port.
636 : */
637 2 : void UARTDriver::_udp_start_client(const char *address, uint16_t port)
638 : {
639 : struct sockaddr_in sockaddr;
640 : int ret;
641 :
642 2 : if (_connected) {
643 0 : return;
644 : }
645 :
646 2 : _use_send_recv = true;
647 :
648 2 : if (_fd != -1) {
649 0 : close(_fd);
650 : }
651 :
652 2 : memset(&sockaddr,0,sizeof(sockaddr));
653 :
654 : #ifdef HAVE_SOCK_SIN_LEN
655 : sockaddr.sin_len = sizeof(sockaddr);
656 : #endif
657 2 : sockaddr.sin_port = htons(port);
658 2 : sockaddr.sin_family = AF_INET;
659 2 : sockaddr.sin_addr.s_addr = inet_addr(address);
660 :
661 2 : _fd = socket(AF_INET, SOCK_DGRAM, 0);
662 2 : if (_fd == -1) {
663 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
664 0 : exit(1);
665 : }
666 2 : ret = fcntl(_fd, F_SETFD, FD_CLOEXEC);
667 2 : if (ret == -1) {
668 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
669 0 : exit(1);
670 : }
671 :
672 : // try to setup for broadcast, this may fail if insufficient privileges
673 2 : int one = 1;
674 2 : setsockopt(_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
675 :
676 2 : ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
677 2 : if (ret == -1) {
678 0 : fprintf(stderr, "udp connect failed on port %u - %s\n",
679 0 : (unsigned)ntohs(sockaddr.sin_port),
680 0 : strerror(errno));
681 0 : exit(1);
682 : }
683 :
684 2 : _is_udp = true;
685 : #if HAL_GCS_ENABLED
686 2 : _packetise = true;
687 : #endif
688 2 : _connected = true;
689 : }
690 :
691 : /*
692 : start a UDP multicast connection
693 : */
694 2 : void UARTDriver::_udp_start_multicast(const char *address, uint16_t port)
695 : {
696 2 : if (_connected) {
697 0 : return;
698 : }
699 :
700 : // establish the listening port
701 : struct sockaddr_in sockaddr;
702 : int ret;
703 :
704 2 : memset(&sockaddr,0,sizeof(sockaddr));
705 :
706 : #ifdef HAVE_SOCK_SIN_LEN
707 : sockaddr.sin_len = sizeof(sockaddr);
708 : #endif
709 2 : sockaddr.sin_port = htons(port);
710 2 : sockaddr.sin_family = AF_INET;
711 2 : sockaddr.sin_addr.s_addr = inet_addr(address);
712 :
713 2 : _mc_fd = socket(AF_INET, SOCK_DGRAM, 0);
714 2 : if (_mc_fd == -1) {
715 0 : fprintf(stderr, "socket failed - %s\n", strerror(errno));
716 0 : exit(1);
717 : }
718 2 : ret = fcntl(_mc_fd, F_SETFD, FD_CLOEXEC);
719 2 : if (ret == -1) {
720 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
721 0 : exit(1);
722 : }
723 2 : int one = 1;
724 2 : if (setsockopt(_mc_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) {
725 0 : fprintf(stderr, "setsockopt failed: %s\n", strerror(errno));
726 0 : exit(1);
727 : }
728 :
729 : // close on exec, to allow reboot
730 2 : fcntl(_mc_fd, F_SETFD, FD_CLOEXEC);
731 :
732 : #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
733 : /*
734 : on cygwin you need to bind to INADDR_ANY then use the multicast
735 : IP_ADD_MEMBERSHIP to get on the right address
736 : */
737 : sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
738 : #endif
739 :
740 2 : ret = bind(_mc_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
741 2 : if (ret == -1) {
742 0 : fprintf(stderr, "multicast bind failed on port %u - %s\n",
743 0 : (unsigned)ntohs(sockaddr.sin_port),
744 0 : strerror(errno));
745 0 : exit(1);
746 : }
747 :
748 2 : struct ip_mreq mreq {};
749 2 : mreq.imr_multiaddr.s_addr = inet_addr(address);
750 2 : mreq.imr_interface.s_addr = htonl(INADDR_ANY);
751 :
752 2 : ret = setsockopt(_mc_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
753 2 : if (ret == -1) {
754 0 : fprintf(stderr, "multicast membership add failed on port %u - %s\n",
755 0 : (unsigned)ntohs(sockaddr.sin_port),
756 0 : strerror(errno));
757 0 : exit(1);
758 : }
759 :
760 : // now start the outgoing connection as an ordinary UDP connection
761 2 : _udp_start_client(address, port);
762 : }
763 :
764 :
765 : /*
766 : start a UART connection for the serial port
767 : */
768 0 : void UARTDriver::_uart_start_connection(void)
769 : {
770 0 : struct termios t {};
771 0 : if (!_connected) {
772 0 : _fd = ::open(_uart_path, O_RDWR | O_CLOEXEC);
773 0 : if (_fd == -1) {
774 : static uint32_t last_error_print_ms;
775 0 : if (AP_HAL::millis() - last_error_print_ms > 5000) {
776 0 : ::printf("Failed to open (%s): %s\n", _uart_path, strerror(errno));
777 0 : last_error_print_ms = AP_HAL::millis();
778 : }
779 0 : return;
780 : }
781 : // use much smaller buffer sizes on real UARTs
782 0 : _writebuffer.set_size(1024);
783 0 : _readbuffer.set_size(512);
784 0 : ::printf("Opened %s\n", _uart_path);
785 : }
786 :
787 0 : if (_fd == -1) {
788 0 : AP_HAL::panic("Unable to open UART %s", _uart_path);
789 : }
790 :
791 : // set non-blocking
792 0 : int flags = fcntl(_fd, F_GETFL, 0);
793 0 : flags = flags | O_NONBLOCK;
794 0 : fcntl(_fd, F_SETFL, flags);
795 :
796 : // disable LF -> CR/LF
797 0 : tcgetattr(_fd, &t);
798 0 : t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF);
799 0 : t.c_oflag &= ~(OPOST | ONLCR);
800 0 : t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
801 0 : t.c_cc[VMIN] = 0;
802 0 : if (_flow_control != FLOW_CONTROL_DISABLE) {
803 0 : t.c_cflag |= CRTSCTS;
804 : }
805 0 : tcsetattr(_fd, TCSANOW, &t);
806 :
807 : // set baudrate
808 0 : if (_uart_baudrate != 0) {
809 0 : set_speed(_uart_baudrate);
810 : }
811 :
812 0 : _connected = true;
813 0 : _use_send_recv = false;
814 : }
815 :
816 : /*
817 : see if a new connection is coming in
818 : */
819 566824738 : void UARTDriver::_check_connection(void)
820 : {
821 566824738 : if (_connected) {
822 : // we only want 1 connection at a time
823 316420754 : return;
824 : }
825 250403984 : if (_select_check(_listen_fd)) {
826 242 : _fd = accept(_listen_fd, nullptr, nullptr);
827 242 : if (_fd != -1) {
828 242 : int one = 1;
829 242 : if (!_is_unix_socket) {
830 242 : setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
831 : }
832 242 : setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
833 242 : if (fcntl(_fd, F_SETFD, FD_CLOEXEC) == -1) {
834 0 : fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno));
835 0 : close(_fd);
836 0 : _fd = -1;
837 0 : return;
838 : }
839 242 : _connected = true;
840 242 : fprintf(stdout, "New connection on SERIAL%u\n", _portNumber);
841 : }
842 : }
843 : }
844 :
845 : /*
846 : use select() to see if something is pending
847 : */
848 504612816 : bool UARTDriver::_select_check(int fd)
849 : {
850 504612816 : if (fd == -1) {
851 6 : return false;
852 : }
853 : #if !APM_BUILD_TYPE(APM_BUILD_Replay)
854 : fd_set fds;
855 : struct timeval tv;
856 :
857 8578415656 : FD_ZERO(&fds);
858 504612810 : FD_SET(fd, &fds);
859 :
860 : // zero time means immediate return from select()
861 504612810 : tv.tv_sec = 0;
862 504612810 : tv.tv_usec = 0;
863 :
864 504612810 : if (select(fd+1, &fds, nullptr, nullptr, &tv) == 1) {
865 2065918 : return true;
866 : }
867 : #endif
868 502546912 : return false;
869 : }
870 :
871 :
872 20 : bool UARTDriver::set_unbuffered_writes(bool on) {
873 20 : if (_fd == -1) {
874 20 : return false;
875 : }
876 0 : _unbuffered_writes = on;
877 :
878 : // this has no effect
879 0 : unsigned v = fcntl(_fd, F_GETFL, 0);
880 0 : v &= ~O_NONBLOCK;
881 : #if defined(__APPLE__) && defined(__MACH__)
882 : fcntl(_fd, F_SETFL | F_NOCACHE, v | O_SYNC);
883 : #elif defined(__OpenBSD__)
884 : fcntl(_fd, F_SETFL, v | O_SYNC);
885 : #else
886 0 : fcntl(_fd, F_SETFL, v | O_DIRECT | O_SYNC);
887 : #endif
888 0 : return _unbuffered_writes;
889 : }
890 :
891 3370231908 : void UARTDriver::_check_reconnect(void)
892 : {
893 3370231908 : if (!_uart_path) {
894 3370231908 : return;
895 : }
896 0 : _uart_start_connection();
897 : }
898 :
899 0 : uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space)
900 : {
901 0 : if (_fd == -1) {
902 0 : return 0;
903 : }
904 0 : const uint32_t micros = AP_HAL::micros();
905 0 : if (micros < 5000000) {
906 : // don't inject for the first several seconds
907 0 : return 0;
908 : }
909 :
910 : uint8_t i;
911 0 : for (i=0; i<space; i++) {
912 0 : if (logic_async_csv.loaded) {
913 0 : const uint32_t emit_timestamp_us = micros - logic_async_csv.first_emit_micros_us;
914 0 : const uint32_t data_timestamp_us = logic_async_csv.loaded_data.timestamp_us - logic_async_csv.first_timestamp_us;
915 0 : if (data_timestamp_us > emit_timestamp_us) {
916 0 : return i;
917 : }
918 0 : buffer[i] = logic_async_csv.loaded_data.b;
919 0 : logic_async_csv.loaded = false;
920 : }
921 :
922 0 : while (!logic_async_csv.loaded) {
923 : uint8_t c;
924 0 : const ssize_t nread = ::read(_fd, &c, 1);
925 0 : if (nread == 0) {
926 : // EOF
927 0 : close(_fd);
928 0 : _fd = -1;
929 0 : return i;
930 : }
931 :
932 : // feed data into CSV Reader, handle new state:
933 0 : auto retcode = logic_async_csv.csvreader.feed(c);
934 0 : switch (retcode) {
935 0 : case AP_CSVReader::RetCode::OK:
936 0 : continue;
937 0 : case AP_CSVReader::RetCode::ERROR:
938 0 : AP_HAL::panic("Malformed CSV?");
939 0 : case AP_CSVReader::RetCode::TERM_DONE:
940 : case AP_CSVReader::RetCode::VECTOR_DONE:
941 0 : switch (logic_async_csv.terms_seen) {
942 0 : case 0: // start_time
943 0 : if (!logic_async_csv.done_first_line) {
944 0 : break;
945 : }
946 0 : logic_async_csv.loaded_data.timestamp_us = atof((char*)logic_async_csv.term) * 1000000; // seconds to microseconds
947 0 : break;
948 0 : case 1: // data
949 0 : if (!logic_async_csv.done_first_line) {
950 0 : break;
951 : }
952 0 : if (!hex_twochars_to_uint8((const char*)&logic_async_csv.term[2], logic_async_csv.loaded_data.b)) {
953 : // invalid character; panic as we do for other
954 : // malformed-CSV cases rather than silently stopping
955 0 : AP_HAL::panic("Malformed CSV?");
956 : }
957 0 : break;
958 0 : case 2: // error
959 : case 3: // framing error
960 0 : break;
961 0 : case 4:
962 0 : AP_HAL::panic("Too many terms in CSV, want (name,type,start_time,duration,data");
963 : }
964 0 : logic_async_csv.terms_seen++;
965 0 : if (retcode != AP_CSVReader::RetCode::VECTOR_DONE) {
966 0 : break;
967 : }
968 :
969 : // we've handled the last term, now handle the vector:
970 0 : if (logic_async_csv.terms_seen != 4) {
971 0 : AP_HAL::panic("Incorrect number off terms in CSV, want (Time [s],Value,Parity Error,Framing Error)");
972 : }
973 0 : logic_async_csv.terms_seen = 0;
974 0 : if (!logic_async_csv.done_first_line) {
975 : // skip the headers
976 0 : logic_async_csv.done_first_line = true;
977 0 : break;
978 : }
979 0 : if (logic_async_csv.first_timestamp_us == 0) {
980 0 : logic_async_csv.first_timestamp_us = logic_async_csv.loaded_data.timestamp_us;
981 0 : logic_async_csv.first_emit_micros_us = micros;
982 : }
983 0 : logic_async_csv.loaded = true;
984 : }
985 : }
986 : }
987 0 : return i;
988 : }
989 :
990 2433580250 : void UARTDriver::handle_writing_from_writebuffer_to_device()
991 : {
992 2433580250 : WITH_SEMAPHORE(write_mtx);
993 2433580250 : if (!_connected) {
994 1685115954 : _check_reconnect();
995 1685115954 : return;
996 : }
997 748464296 : ssize_t nwritten = 0;
998 748464296 : uint32_t max_bytes = 10000;
999 : #if !defined(HAL_BUILD_AP_PERIPH)
1000 748464296 : SITL::SIM *_sitl = AP::sitl();
1001 748464296 : if (_sitl && _sitl->telem_baudlimit_enable) {
1002 : // limit byte rate to configured baudrate
1003 : // Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
1004 0 : max_bytes = baud_limits.write.max_bytes(float(_uart_baudrate) * 0.1);
1005 0 : if (max_bytes == 0) {
1006 0 : return;
1007 : }
1008 : }
1009 : #endif
1010 748464296 : if (_packetise) {
1011 78802 : uint16_t n = _writebuffer.available();
1012 78802 : n = MIN(n, max_bytes);
1013 : #if HAL_GCS_ENABLED
1014 78802 : if (n > 0) {
1015 302 : n = mavlink_packetise(_writebuffer, n);
1016 : }
1017 : #endif
1018 78802 : if (n > 0) {
1019 : // keep as a single UDP packet
1020 302 : uint8_t tmpbuf[n];
1021 302 : _writebuffer.peekbytes(tmpbuf, n);
1022 302 : ssize_t ret = send(_fd, tmpbuf, n, MSG_DONTWAIT);
1023 302 : if (ret > 0) {
1024 302 : _writebuffer.advance(ret);
1025 302 : _tx_stats_bytes += ret;
1026 : }
1027 302 : }
1028 : } else {
1029 : uint32_t navail;
1030 748385494 : const uint8_t *readptr = _writebuffer.readptr(navail);
1031 748385494 : if (readptr && navail > 0) {
1032 6097160 : navail = MIN(navail, max_bytes);
1033 6097160 : if (_sim_serial_device != nullptr) {
1034 3961457 : nwritten = _sim_serial_device->write_to_device((const char*)readptr, navail);
1035 2135703 : } else if (!_use_send_recv) {
1036 : // For ENABLE/AUTO modes on a real UART, check CTS via ioctl
1037 : // before writing. This provides reliable flow control on
1038 : // platforms where kernel CRTSCTS may not work (e.g. macOS
1039 : // with USB-serial adapters).
1040 0 : bool cts_blocked = false;
1041 0 : if (_flow_control != FLOW_CONTROL_DISABLE) {
1042 0 : int modem_bits = 0;
1043 0 : if (ioctl(_fd, TIOCMGET, &modem_bits) == 0 &&
1044 0 : !(modem_bits & TIOCM_CTS)) {
1045 : // CTS deasserted - remote end asked us to stop sending
1046 0 : cts_blocked = true;
1047 : }
1048 : }
1049 0 : if (!cts_blocked) {
1050 0 : nwritten = ::write(_fd, readptr, navail);
1051 0 : if (nwritten == -1 && errno != EAGAIN && _uart_path) {
1052 0 : close(_fd);
1053 0 : _fd = -1;
1054 0 : _connected = false;
1055 : }
1056 : }
1057 : // AUTO flow control detection: mirror ChibiOS 500ms timeout logic
1058 0 : if (_flow_control == FLOW_CONTROL_AUTO && _uart_path) {
1059 0 : if (_auto_flow_start_ms == 0) {
1060 0 : _auto_flow_start_ms = AP_HAL::millis();
1061 : }
1062 0 : if (nwritten > 0 && !_auto_flow_detected) {
1063 : // first successful write - CTS active
1064 0 : ::printf("SERIAL%u: hardware flow control enabled\n", _portNumber);
1065 0 : _auto_flow_detected = true;
1066 0 : } else if (!_auto_flow_detected && AP_HAL::millis() - _auto_flow_start_ms > 500U) {
1067 : // CTS never seen since AUTO was set - actually disable
1068 0 : ::printf("SERIAL%u: hardware flow control not detected, disabled\n", _portNumber);
1069 0 : set_flow_control(FLOW_CONTROL_DISABLE);
1070 : }
1071 : }
1072 : } else {
1073 2135703 : nwritten = send(_fd, readptr, navail, MSG_DONTWAIT);
1074 : }
1075 6097160 : if (nwritten > 0) {
1076 4258209 : _writebuffer.advance(nwritten);
1077 4258209 : _tx_stats_bytes += nwritten;
1078 : }
1079 : }
1080 : }
1081 2433580250 : }
1082 :
1083 2434445787 : void UARTDriver::handle_reading_from_device_to_readbuffer()
1084 : {
1085 2434445787 : if (!_connected) {
1086 1685115954 : _check_reconnect();
1087 1688536700 : return;
1088 : }
1089 :
1090 749329833 : uint32_t space = _readbuffer.space();
1091 749329833 : if (space == 0) {
1092 3420188 : return;
1093 : }
1094 :
1095 745909645 : uint32_t max_bytes = 10000;
1096 : #if !defined(HAL_BUILD_AP_PERIPH)
1097 745909645 : SITL::SIM *_sitl = AP::sitl();
1098 745909645 : if (_sitl && _sitl->telem_baudlimit_enable) {
1099 : // limit byte rate to configured baudrate
1100 : // Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
1101 0 : max_bytes = baud_limits.read.max_bytes(float(_uart_baudrate) * 0.1);
1102 0 : if (max_bytes == 0) {
1103 0 : return;
1104 : }
1105 : }
1106 : #endif
1107 :
1108 745909645 : space = MIN(space, max_bytes);
1109 :
1110 745909645 : char buf[space];
1111 745909645 : ssize_t nread = 0;
1112 745909645 : if (_mc_fd >= 0) {
1113 78802 : if (_select_check(_mc_fd)) {
1114 : struct sockaddr_in from;
1115 303 : socklen_t fromlen = sizeof(from);
1116 303 : nread = recvfrom(_mc_fd, buf, space, MSG_DONTWAIT, (struct sockaddr *)&from, &fromlen);
1117 303 : uint16_t port = ntohs(from.sin_port);
1118 303 : if (_mc_myport == 0) {
1119 : // get our own address, so we can recognise packets from ourself
1120 : struct sockaddr_in myaddr;
1121 : socklen_t myaddrlen;
1122 293 : if (getsockname(_fd, (struct sockaddr *)&myaddr, &myaddrlen) == 0) {
1123 1 : _mc_myport = ntohs(myaddr.sin_port);
1124 : }
1125 : }
1126 303 : if (_mc_myport == port) {
1127 : // assume this is a packet from ourselves. This is not
1128 : // entirely accurate, as it could be a packet from
1129 : // another machine that has assigned the same port,
1130 : // unfortunately we don't have a better way to detect
1131 : // packets from ourselves
1132 10 : nread = 0;
1133 : }
1134 : }
1135 745830843 : } else if (_sim_serial_device != nullptr) {
1136 491700826 : nread = _sim_serial_device->read_from_device(buf, space);
1137 254130017 : } else if (logic_async_csv.active) {
1138 0 : nread = read_from_async_csv((uint8_t*)buf, space);
1139 254130017 : } else if (!_use_send_recv) {
1140 0 : if (!_select_check(_fd)) {
1141 0 : return;
1142 : }
1143 0 : int fd = _console?0:_fd;
1144 0 : nread = ::read(fd, buf, space);
1145 0 : if (nread == -1 && errno != EAGAIN && _uart_path) {
1146 0 : close(_fd);
1147 0 : _fd = -1;
1148 0 : _connected = false;
1149 : }
1150 254130017 : } else if (_select_check(_fd)) {
1151 2065373 : nread = recv(_fd, buf, space, MSG_DONTWAIT);
1152 2065372 : if (nread <= 0 && !_is_udp) {
1153 : // the socket has reached EOF
1154 558 : close(_fd);
1155 558 : _fd = -1;
1156 558 : _connected = false;
1157 558 : fprintf(stdout, "Closed connection on SERIAL%u\n", _portNumber);
1158 558 : fflush(stdout);
1159 : #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
1160 : if (_portNumber == 0) {
1161 : // exit on cygwin port 0 is almost certainly closing the
1162 : // connection in MissionPlanner SITL. We want to exit or
1163 : // we leave a stray process which confuses restart
1164 : exit(0);
1165 : }
1166 : #endif
1167 558 : return;
1168 : }
1169 : }
1170 745909086 : if (nread > 0) {
1171 3439489 : _readbuffer.write((uint8_t *)buf, nread);
1172 3439489 : _receive_timestamp = AP_HAL::micros64();
1173 : }
1174 2434445787 : }
1175 :
1176 2433580250 : void UARTDriver::_timer_tick(void)
1177 : {
1178 2433580250 : handle_writing_from_writebuffer_to_device();
1179 2433580250 : handle_reading_from_device_to_readbuffer();
1180 2433580249 : }
1181 :
1182 :
1183 : /*
1184 : return timestamp estimate in microseconds for when the start of
1185 : a nbytes packet arrived on the uart. This should be treated as a
1186 : time constraint, not an exact time. It is guaranteed that the
1187 : packet did not start being received after this time, but it
1188 : could have been in a system buffer before the returned time.
1189 :
1190 : This takes account of the baudrate of the link. For transports
1191 : that have no baudrate (such as USB) the time estimate may be
1192 : less accurate.
1193 :
1194 : A return value of zero means the HAL does not support this API
1195 : */
1196 726391 : uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
1197 : {
1198 726391 : uint64_t last_receive_us = _receive_timestamp;
1199 726391 : if (_uart_baudrate > 0) {
1200 : // assume 10 bits per byte.
1201 726391 : uint32_t transport_time_us = (1000000UL * 10UL / _uart_baudrate) * (nbytes+available());
1202 726391 : last_receive_us -= transport_time_us;
1203 : }
1204 726391 : return last_receive_us;
1205 : }
1206 :
1207 87644331 : ssize_t UARTDriver::get_system_outqueue_length() const
1208 : {
1209 87644331 : if (!_connected) {
1210 980 : return 0;
1211 : }
1212 :
1213 : #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
1214 : return 0;
1215 : #elif defined(__APPLE__) && defined(__MACH__)
1216 : return 0;
1217 : #else
1218 : int size;
1219 87643351 : if (ioctl(_fd, TIOCOUTQ, &size) == -1) {
1220 : // ::fprintf(stderr, "ioctl TIOCOUTQ failed: %m\n");
1221 0 : return 0;
1222 : }
1223 87643351 : return size;
1224 : #endif
1225 : }
1226 :
1227 87641061 : ssize_t UARTDriver::get_system_outqueue_limit() const
1228 : {
1229 : // AF_UNIX TIOCOUTQ includes per-write kernel accounting and data already
1230 : // delivered to the peer. Allow enough room for several full UART writes
1231 : // before applying the anti-lag throttle, while retaining the TCP limit.
1232 87641061 : return _is_unix_socket ? 65536 : 1024;
1233 : }
1234 :
1235 7012 : uint32_t UARTDriver::bw_in_bytes_per_second() const
1236 : {
1237 : // if connected, assume at least a 10/100Mbps connection if not limited
1238 7012 : bool baud_limit = false;
1239 : #if !defined(HAL_BUILD_AP_PERIPH)
1240 7012 : SITL::SIM *_sitl = AP::sitl();
1241 7012 : baud_limit = (_sitl != nullptr) && _sitl->telem_baudlimit_enable;
1242 : #endif
1243 7012 : const uint32_t bitrate = (_connected && !baud_limit) ? 10E6 : _uart_baudrate;
1244 7012 : return bitrate/10; // convert bits to bytes minus overhead
1245 : };
1246 :
1247 : #if HAL_UART_STATS_ENABLED
1248 : // request information on uart I/O for @SYS/uarts.txt for this uart
1249 11484 : void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint32_t dt_ms)
1250 : {
1251 11484 : const uint32_t tx_bytes = stats.tx.update(_tx_stats_bytes);
1252 11484 : const uint32_t rx_bytes = stats.rx.update(_rx_stats_bytes);
1253 :
1254 11484 : str.printf("TX=%8u RX=%8u TXBD=%6u RXBD=%6u %s (%s)\n",
1255 : unsigned(tx_bytes),
1256 : unsigned(rx_bytes),
1257 11484 : unsigned((tx_bytes * 10000) / dt_ms),
1258 11484 : unsigned((rx_bytes * 10000) / dt_ms),
1259 11484 : _connected ? "connected " : "not connected",
1260 11484 : _sitlState->_serial_path[_portNumber]);
1261 11484 : }
1262 : #endif
1263 :
1264 : #endif // CONFIG_HAL_BOARD
|
