Line data    Source code

       1             : #include "UDPDevice.h"
       2             : 
       3             : #include <fcntl.h>
       4             : #include <stdio.h>
       5             : #include <sys/ioctl.h>
       6             : 
       7             : #include <AP_HAL/AP_HAL.h>
       8             : 
       9             : // UDP read() will give up after receiving this many packets
      10             : // including ones that are discarded (e.g. our own multicasts)
      11             : // Tune in hwdef.dat if necessary
      12             : #ifndef HAL_LINUX_MAX_UDP_PACKETS
      13             :     #define HAL_LINUX_MAX_UDP_PACKETS 20
      14             : #endif
      15             : 
      16           0 : UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast, bool input):
      17           0 :     _ip(ip),
      18           0 :     _port(port),
      19           0 :     _bcast(bcast),
      20           0 :     _input(input)
      21             : {
      22           0 : }
      23             : 
      24           0 : UDPDevice::~UDPDevice()
      25             : {
      26           0 : }
      27             : 
      28           0 : ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
      29             : {
      30           0 :     if (!socket.pollout(0)) {
      31           0 :         return -1;
      32             :     }
      33           0 :     if (_connected) {
      34           0 :         return socket.send(buf, n);
      35             :     }
      36           0 :     if (_input) {
      37             :         // can't send yet
      38           0 :         return -1;
      39             :     }
      40           0 :     return socket.sendto(buf, n, _ip, _port);
      41             : }
      42             : 
      43           0 : ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
      44             : {
      45           0 :     ssize_t bytes_read = 0;
      46           0 :     ssize_t packets_read = 0;
      47             : 
      48             :     // recv() only retrieves a single datagram on UDP sockets
      49             :     // peek & read multiple until the buffer full
      50             :     // poll each time because FIONREAD can't distinguish between
      51             :     // no data and empty datagrams
      52           0 :     while (bytes_read < n && socket.pollin(0) && packets_read < HAL_LINUX_MAX_UDP_PACKETS) {
      53           0 :         int in_size = 0;
      54           0 :         if (ioctl(socket.get_read_fd(), FIONREAD, &in_size) < 0) {
      55           0 :             return -1;
      56             :         }
      57             : 
      58             :         // read the first packet unconditionally
      59             :         // read subsequent packets if they fit into the remaining buffer
      60             :         // this ensures that if the incoming packet is somehow larger than
      61             :         // the entire RX ring-buffer, we get a truncated packet rather
      62             :         // than end up being unable to read any further
      63             :         // NOTE: Linux HAL allocates a 8192 byte RX buffer so this should
      64             :         // not happen unless the buffer size is redefined
      65           0 :         if (bytes_read > 0 && in_size > n - bytes_read) {
      66           0 :             break;
      67             :         }
      68             : 
      69           0 :         ssize_t ret = socket.recv(&buf[bytes_read], n - bytes_read, 0);
      70           0 :         if (!_connected && ret > 0) {
      71             :             const char *ip;
      72             :             uint16_t port;
      73           0 :             socket.last_recv_address(ip, port);
      74           0 :             _connected = socket.connect(ip, port);
      75             :         }
      76             : 
      77             :         // socket.read() may return -1 if we're reading ourselves on multicast
      78           0 :         if (ret > 0) {
      79           0 :             bytes_read += ret;
      80             :         }
      81             : 
      82           0 :         packets_read++;
      83             :     }
      84             : 
      85           0 :     if (bytes_read == 0) {
      86           0 :         return -1;
      87             :     }
      88             : 
      89           0 :     return bytes_read;
      90             : }
      91             : 
      92           0 : bool UDPDevice::open()
      93             : {
      94           0 :     if (_input) {
      95           0 :         socket.bind(_ip, _port);
      96           0 :         return true;
      97             :     }
      98           0 :     if (_bcast) {
      99             :         // open now, then connect on first received packet
     100           0 :         socket.set_broadcast();
     101           0 :         return true;
     102             :     }
     103           0 :     _connected = socket.connect(_ip, _port);
     104           0 :     return _connected;
     105             : }
     106             : 
     107           0 : bool UDPDevice::close()
     108             : {
     109           0 :     return true;
     110             : }
     111             : 
     112           0 : void UDPDevice::set_blocking(bool blocking)
     113             : {
     114           0 :     socket.set_blocking(blocking);
     115           0 : }
     116             : 
     117           0 : void UDPDevice::set_speed(uint32_t speed)
     118             : {
     119             : 
     120           0 : }