Line data Source code
1 : #include "UARTDriver.h"
2 :
3 : #include <arpa/inet.h>
4 : #include <assert.h>
5 : #include <errno.h>
6 : #include <fcntl.h>
7 : #include <netinet/in.h>
8 : #include <netinet/tcp.h>
9 : #include <poll.h>
10 : #include <stdio.h>
11 : #include <stdlib.h>
12 : #include <string.h>
13 : #include <sys/ioctl.h>
14 : #include <sys/socket.h>
15 : #include <sys/stat.h>
16 : #include <sys/types.h>
17 : #include <termios.h>
18 : #include <unistd.h>
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 :
22 : #include "ConsoleDevice.h"
23 : #include "TCPServerDevice.h"
24 : #include "UARTDevice.h"
25 : #include "UDPDevice.h"
26 : #include <GCS_MAVLink/GCS.h>
27 : #if HAL_GCS_ENABLED
28 : #include <AP_HAL/utility/packetise.h>
29 : #endif
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : using namespace Linux;
34 :
35 480 : UARTDriver::UARTDriver(bool default_console) :
36 480 : _device{new ConsoleDevice()}
37 : {
38 480 : if (default_console) {
39 48 : _console = true;
40 : }
41 480 : }
42 :
43 : /*
44 : set the tty device to use for this UART
45 : */
46 0 : void UARTDriver::set_device_path(const char *path)
47 : {
48 0 : device_path = path;
49 0 : }
50 :
51 0 : void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
52 : {
53 0 : if (!_initialised) {
54 0 : if (device_path == nullptr && _console) {
55 0 : _device = NEW_NOTHROW ConsoleDevice();
56 : } else {
57 0 : if (device_path == nullptr) {
58 0 : return;
59 : }
60 :
61 0 : _device = _parseDevicePath(device_path);
62 :
63 0 : if (!_device.get()) {
64 0 : ::fprintf(stderr, "Argument is not valid. Fallback to console.\n"
65 : "Launch with --help to see an example.\n");
66 0 : _device = NEW_NOTHROW ConsoleDevice();
67 : }
68 : }
69 : }
70 :
71 0 : if (!_connected) {
72 0 : _connected = _device->open();
73 0 : if (_connected) {
74 0 : _device->set_blocking(false);
75 : }
76 : }
77 0 : _initialised = false;
78 :
79 0 : while (_in_timer) hal.scheduler->delay(1);
80 :
81 0 : bool clear_buffers = false;
82 0 : if (b != 0 && _baudrate != b) {
83 0 : _device->set_speed(b);
84 0 : _baudrate = b;
85 0 : if (hal.console != this) {
86 0 : clear_buffers = true;
87 : }
88 : }
89 :
90 0 : _allocate_buffers(rxS, txS);
91 :
92 0 : if (clear_buffers) {
93 0 : _readbuf.clear();
94 0 : _writebuf.clear();
95 : }
96 : }
97 :
98 0 : void UARTDriver::_allocate_buffers(uint16_t rxS, uint16_t txS)
99 : {
100 : /* we have enough memory to have a larger transmit buffer for
101 : * all ports. This means we don't get delays while waiting to
102 : * write GPS config packets
103 : */
104 :
105 0 : if (rxS < 8192) {
106 0 : rxS = 8192;
107 : }
108 0 : if (txS < 32000) {
109 0 : txS = 32000;
110 : }
111 :
112 0 : if (_writebuf.set_size(txS) && _readbuf.set_size(rxS)) {
113 0 : _initialised = true;
114 : }
115 0 : }
116 :
117 0 : void UARTDriver::_deallocate_buffers()
118 : {
119 0 : _readbuf.set_size(0);
120 0 : _writebuf.set_size(0);
121 0 : }
122 :
123 : /*
124 : Device path accepts the following syntaxes:
125 : - /dev/ttyO1
126 : - tcp:*:1243:wait
127 : - udp:192.168.2.15:1243
128 : */
129 0 : AP_HAL::OwnPtr<SerialDevice> UARTDriver::_parseDevicePath(const char *arg)
130 : {
131 : struct stat st;
132 :
133 0 : if (stat(arg, &st) == 0 && S_ISCHR(st.st_mode)) {
134 0 : return AP_HAL::OwnPtr<SerialDevice>(NEW_NOTHROW UARTDevice(arg));
135 0 : } else if (strncmp(arg, "tcp:", 4) != 0 &&
136 0 : strncmp(arg, "udp:", 4) != 0 &&
137 0 : strncmp(arg, "udpin:", 6)) {
138 0 : return nullptr;
139 : }
140 :
141 0 : char *devstr = strdup(arg);
142 :
143 0 : if (devstr == nullptr) {
144 0 : return nullptr;
145 : }
146 :
147 0 : char *saveptr = nullptr;
148 : char *protocol, *ip, *port, *flag;
149 :
150 0 : protocol = strtok_r(devstr, ":", &saveptr);
151 0 : ip = strtok_r(nullptr, ":", &saveptr);
152 0 : port = strtok_r(nullptr, ":", &saveptr);
153 0 : flag = strtok_r(nullptr, ":", &saveptr);
154 :
155 0 : if (ip == nullptr || port == nullptr) {
156 0 : free(devstr);
157 0 : return nullptr;
158 : }
159 :
160 0 : if (_ip) {
161 0 : free(_ip);
162 0 : _ip = nullptr;
163 : }
164 :
165 0 : if (_flag) {
166 0 : free(_flag);
167 0 : _flag = nullptr;
168 : }
169 :
170 0 : _base_port = (uint16_t) atoi(port);
171 0 : _ip = strdup(ip);
172 :
173 : /* Optional flag for TCP */
174 0 : if (flag != nullptr) {
175 0 : _flag = strdup(flag);
176 : }
177 :
178 0 : AP_HAL::OwnPtr<SerialDevice> device = nullptr;
179 :
180 0 : if (strcmp(protocol, "udp") == 0 || strcmp(protocol, "udpin") == 0) {
181 0 : bool bcast = (_flag && strcmp(_flag, "bcast") == 0);
182 : #if HAL_GCS_ENABLED
183 0 : _packetise = true;
184 : #endif
185 0 : if (strcmp(protocol, "udp") == 0) {
186 0 : device = NEW_NOTHROW UDPDevice(_ip, _base_port, bcast, false);
187 : } else {
188 0 : if (bcast) {
189 0 : AP_HAL::panic("Can't combine udpin with bcast");
190 : }
191 0 : device = NEW_NOTHROW UDPDevice(_ip, _base_port, false, true);
192 :
193 : }
194 0 : } else {
195 0 : bool wait = (_flag && strcmp(_flag, "wait") == 0);
196 0 : device = NEW_NOTHROW TCPServerDevice(_ip, _base_port, wait);
197 : }
198 :
199 0 : free(devstr);
200 0 : return device;
201 0 : }
202 :
203 : /*
204 : shutdown a UART
205 : */
206 0 : void UARTDriver::_end()
207 : {
208 0 : _initialised = false;
209 0 : _connected = false;
210 :
211 0 : while (_in_timer) {
212 0 : hal.scheduler->delay(1);
213 : }
214 :
215 0 : _device->close();
216 0 : _deallocate_buffers();
217 0 : }
218 :
219 :
220 0 : void UARTDriver::_flush()
221 : {
222 : // we are not doing any buffering, so flush is a no-op
223 0 : }
224 :
225 :
226 : /*
227 : return true if the UART is initialised
228 : */
229 0 : bool UARTDriver::is_initialized()
230 : {
231 0 : return _initialised;
232 : }
233 :
234 :
235 : /*
236 : do we have any bytes pending transmission?
237 : */
238 0 : bool UARTDriver::tx_pending()
239 : {
240 0 : return (_writebuf.available() > 0);
241 : }
242 :
243 : /*
244 : return the number of bytes available to be read
245 : */
246 0 : uint32_t UARTDriver::_available()
247 : {
248 0 : if (!_initialised) {
249 0 : return 0;
250 : }
251 0 : return _readbuf.available();
252 : }
253 :
254 : /*
255 : how many bytes are available in the output buffer?
256 : */
257 0 : uint32_t UARTDriver::txspace()
258 : {
259 0 : if (!_initialised) {
260 0 : return 0;
261 : }
262 0 : return _writebuf.space();
263 : }
264 :
265 0 : ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t count)
266 : {
267 0 : if (!_initialised) {
268 0 : return 0;
269 : }
270 :
271 0 : return _readbuf.read(buffer, count);
272 : }
273 :
274 0 : bool UARTDriver::_discard_input()
275 : {
276 0 : if (!_initialised) {
277 0 : return false;
278 : }
279 0 : _readbuf.clear();
280 0 : return true;
281 : }
282 :
283 : /*
284 : write size bytes to the write buffer
285 : */
286 0 : size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
287 : {
288 0 : if (!_initialised) {
289 0 : return 0;
290 : }
291 0 : if (!_write_mutex.take_nonblocking()) {
292 0 : return 0;
293 : }
294 :
295 0 : size_t ret = _writebuf.write(buffer, size);
296 0 : _write_mutex.give();
297 0 : return ret;
298 : }
299 :
300 : /*
301 : try writing n bytes, handling an unresponsive port
302 : */
303 0 : int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
304 : {
305 : /*
306 : allow for delayed connection. This allows ArduPilot to start
307 : before a network interface is available.
308 : */
309 0 : if (!_connected) {
310 0 : _connected = _device->open();
311 : }
312 0 : if (!_connected) {
313 0 : return 0;
314 : }
315 :
316 0 : return _device->write(buf, n);
317 : }
318 :
319 : /*
320 : try reading n bytes, handling an unresponsive port
321 : */
322 0 : int UARTDriver::_read_fd(uint8_t *buf, uint16_t n)
323 : {
324 0 : return _device->read(buf, n);
325 : }
326 :
327 :
328 : /*
329 : try to push out one lump of pending bytes
330 : return true if progress is made
331 : */
332 0 : bool UARTDriver::_write_pending_bytes(void)
333 : {
334 : // write any pending bytes
335 0 : uint32_t available_bytes = _writebuf.available();
336 0 : uint16_t n = available_bytes;
337 :
338 : #if HAL_GCS_ENABLED
339 0 : if (_packetise && n > 0) {
340 : // send on MAVLink packet boundaries if possible
341 0 : n = mavlink_packetise(_writebuf, n);
342 : }
343 : #endif
344 :
345 0 : if (n > 0) {
346 : int ret;
347 :
348 0 : if (_packetise) {
349 : // keep as a single UDP packet
350 0 : uint8_t tmpbuf[n];
351 0 : _writebuf.peekbytes(tmpbuf, n);
352 0 : ret = _write_fd(tmpbuf, n);
353 0 : if (ret > 0)
354 0 : _writebuf.advance(ret);
355 0 : } else {
356 : ByteBuffer::IoVec vec[2];
357 0 : const auto n_vec = _writebuf.peekiovec(vec, n);
358 0 : for (int i = 0; i < n_vec; i++) {
359 0 : ret = _write_fd(vec[i].data, (uint16_t)vec[i].len);
360 0 : if (ret < 0) {
361 0 : break;
362 : }
363 0 : _writebuf.advance(ret);
364 :
365 : /* We wrote less than we asked for, stop */
366 0 : if ((unsigned)ret != vec[i].len) {
367 0 : break;
368 : }
369 : }
370 : }
371 : }
372 :
373 0 : return _writebuf.available() != available_bytes;
374 : }
375 :
376 : /*
377 : push any pending bytes to/from the serial port. This is called at
378 : 1kHz in the timer thread. Doing it this way reduces the system call
379 : overhead in the main task enormously.
380 : */
381 0 : void UARTDriver::_timer_tick(void)
382 : {
383 0 : if (!_initialised) return;
384 :
385 0 : _in_timer = true;
386 :
387 0 : uint8_t num_send = 10;
388 0 : while (num_send != 0 && _write_pending_bytes()) {
389 0 : num_send--;
390 : }
391 :
392 : // try to fill the read buffer
393 : int ret;
394 : ByteBuffer::IoVec vec[2];
395 :
396 0 : const auto n_vec = _readbuf.reserve(vec, _readbuf.space());
397 0 : for (int i = 0; i < n_vec; i++) {
398 0 : ret = _read_fd(vec[i].data, vec[i].len);
399 0 : if (ret < 0) {
400 0 : break;
401 : }
402 0 : _readbuf.commit((unsigned)ret);
403 :
404 : // update receive timestamp
405 0 : _receive_timestamp[_receive_timestamp_idx^1] = AP_HAL::micros64();
406 0 : _receive_timestamp_idx ^= 1;
407 :
408 : /* stop reading as we read less than we asked for */
409 0 : if ((unsigned)ret < vec[i].len) {
410 0 : break;
411 : }
412 : }
413 :
414 0 : _in_timer = false;
415 : }
416 :
417 0 : void UARTDriver::configure_parity(uint8_t v) {
418 0 : UARTDriver::parity = v;
419 0 : _device->set_parity(v);
420 0 : }
421 :
422 : /*
423 : return timestamp estimate in microseconds for when the start of
424 : a nbytes packet arrived on the uart. This should be treated as a
425 : time constraint, not an exact time. It is guaranteed that the
426 : packet did not start being received after this time, but it
427 : could have been in a system buffer before the returned time.
428 :
429 : This takes account of the baudrate of the link. For transports
430 : that have no baudrate (such as USB) the time estimate may be
431 : less accurate.
432 :
433 : A return value of zero means the HAL does not support this API
434 : */
435 0 : uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
436 : {
437 0 : uint64_t last_receive_us = _receive_timestamp[_receive_timestamp_idx];
438 0 : if (_baudrate > 0) {
439 : // assume 10 bits per byte.
440 0 : uint32_t transport_time_us = (1000000UL * 10UL / _baudrate) * (nbytes+available());
441 0 : last_receive_us -= transport_time_us;
442 : }
443 0 : return last_receive_us;
444 : }
445 :
446 0 : uint32_t UARTDriver::bw_in_bytes_per_second() const
447 : {
448 : // if connected, assume at least a 10/100Mbps connection
449 0 : const uint32_t bitrate = (_connected && _ip != nullptr) ? 10E6 : _baudrate;
450 0 : return bitrate/10; // convert bits to bytes minus overhead
451 : }
|
