Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: implemented TCP server as a single driver 

the wait flag just changes startup behaviour. The TCP server should
always be a server with listen and accept. We don't need two drivers
This commit is contained in:
Andrew Tridgell 2015-07-29 09:37:57 +10:00
parent 596ecde70d
commit 55fc66ec65
5 changed files with 67 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
libraries/AP_HAL_Linux/TCPBlockingServerDevice.cpp
View File

@ -1,138 +0,0 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <netdb.h>
#include "TCPBlockingServerDevice.h"
TCPBlockingServerDevice::TCPBlockingServerDevice(const char *ip, uint16_t port):
_ip(ip),
_port(port)
{
}
TCPBlockingServerDevice::~TCPBlockingServerDevice()
{
}
ssize_t TCPBlockingServerDevice::write(const uint8_t *buf, uint16_t n)
{
return ::send(_net_fd, buf, n, 0);
}
ssize_t TCPBlockingServerDevice::read(uint8_t *buf, uint16_t n)
{
return ::recv(_net_fd, buf, n, 0);
}
bool TCPBlockingServerDevice::open()
{
int one=1;
struct sockaddr_in sockaddr;
int ret;
int client_fd = -1;
uint8_t portNumber = 0; /* connecto to _port + portNumber */
memset(&sockaddr,0,sizeof(sockaddr));
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
sockaddr.sin_port = htons(_port + portNumber);
sockaddr.sin_family = AF_INET;
if (strcmp(_ip, "*") == 0) {
/* bind to all interfaces */
sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
} else {
sockaddr.sin_addr.s_addr = inet_addr(_ip);
}
_listen_fd = socket(AF_INET, SOCK_STREAM, 0);
if (_listen_fd == -1) {
::printf("socket failed - %s\n", strerror(errno));
exit(1);
}
/* we want to be able to re-use ports quickly */
setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == -1) {
::printf("bind failed on port %u - %s\n",
(unsigned)ntohs(sockaddr.sin_port),
strerror(errno));
exit(1);
}
ret = listen(_listen_fd, 5);
if (ret == -1) {
::printf("listen failed - %s\n", strerror(errno));
exit(1);
}
::printf("Serial port %u on TCP port %u\n", portNumber,
_port + portNumber);
::fflush(stdout);
::printf("Waiting for connection ....\n");
::fflush(stdout);
struct sockaddr_storage client_addr;
socklen_t addr_size = sizeof(client_addr);
client_fd = accept(_listen_fd, (struct sockaddr *) &client_addr, &addr_size);
if (client_fd == -1) {
::printf("accept() error - %s", strerror(errno));
exit(1);
}
struct sockaddr_in *sa4 = (struct sockaddr_in*) &client_addr;
printf("%s connected\n", inet_ntoa(sa4->sin_addr) );
setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
setsockopt(client_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
/* always run the file descriptor non-blocking, and deal with |
* blocking IO in the higher level calls */
fcntl(client_fd, F_SETFL, fcntl(client_fd, F_GETFL, 0) | O_NONBLOCK);
_net_fd = client_fd;
return true;
}
bool TCPBlockingServerDevice::close()
{
if (::close(_listen_fd) < 0) {
perror("close");
return false;
}
if (::close(_net_fd) < 0) {
perror("close");
return false;
}
return true;
}
void TCPBlockingServerDevice::set_blocking(bool blocking)
{
}
void TCPBlockingServerDevice::set_speed(uint32_t speed)
{
}
#endif

25
libraries/AP_HAL_Linux/TCPBlockingServerDevice.h
View File

@ -1,25 +0,0 @@
#ifndef __AP_HAL_LINUX_TCPSERVERDEVICE_H__
#define __AP_HAL_LINUX_TCPSERVERDEVICE_H__
#include "SerialDevice.h"
#include "../AP_HAL/utility/Socket.h"
class TCPBlockingServerDevice: public SerialDevice {
public:
TCPBlockingServerDevice(const char *ip, uint16_t port);
virtual ~TCPBlockingServerDevice();
virtual bool open() override;
virtual bool close() override;
virtual void set_blocking(bool blocking) override;
virtual void set_speed(uint32_t speed) override;
virtual ssize_t write(const uint8_t *buf, uint16_t n) override;
virtual ssize_t read(uint8_t *buf, uint16_t n) override;
private:
const char *_ip;
uint16_t _port;
int _net_fd = -1;
int _listen_fd = -1;
};
#endif

69
libraries/AP_HAL_Linux/TCPServerDevice.cpp
View File

@ -9,33 +9,68 @@
#include "TCPServerDevice.h"
TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port):
TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port, bool wait):
_ip(ip),
_port(port)
_port(port),
_wait(wait)
{
}
TCPServerDevice::~TCPServerDevice()
{
if (sock != NULL) {
delete sock;
sock = NULL;
}
}
ssize_t TCPServerDevice::write(const uint8_t *buf, uint16_t n)
{
return listener.send(buf, n);
if (sock == NULL) {
return -1;
}
return sock->send(buf, n);
}
/*
when we try to read we accept new connections if one isn't already
established
*/
ssize_t TCPServerDevice::read(uint8_t *buf, uint16_t n)
{
return listener.recv(buf, n, 1);
if (sock == NULL) {
sock = listener.accept(0);
if (sock != NULL) {
sock->set_blocking(_blocking);
}
}
if (sock == NULL) {
return -1;
}
ssize_t ret = sock->recv(buf, n, 1);
if (ret == 0) {
// EOF, go back to waiting for a new connection
delete sock;
sock = NULL;
return -1;
}
return ret;
}
bool TCPServerDevice::open()
{
listener.reuseaddress();
if (!listener.connect(_ip, _port)) {
::printf("connect failed on %s port %u - %s\n",
if (!listener.bind(_ip, _port)) {
::printf("bind failed on %s port %u - %s\n",
_ip,
_port,
strerror(errno));
::exit(1);
}
if (!listener.listen(1)) {
::printf("listen failed on %s port %u - %s\n",
_ip,
_port,
strerror(errno));
@ -44,22 +79,38 @@ bool TCPServerDevice::open()
listener.set_blocking(false);
if (_wait) {
::printf("Waiting for connection on %s:%u ....\n",
_ip, (unsigned)_port);
::fflush(stdout);
while (sock == NULL) {
sock = listener.accept(1000);
}
sock->set_blocking(_blocking);
::printf("connected\n");
::fflush(stdout);
}
return true;
}
bool TCPServerDevice::close()
{
if (sock != NULL) {
delete sock;
sock = NULL;
}
return true;
}
void TCPServerDevice::set_blocking(bool blocking)
{
listener.set_blocking(blocking);
_blocking = blocking;
listener.set_blocking(_blocking);
}
void TCPServerDevice::set_speed(uint32_t speed)
{
}
#endif

6
libraries/AP_HAL_Linux/TCPServerDevice.h
View File

@ -6,7 +6,7 @@
class TCPServerDevice: public SerialDevice {
public:
TCPServerDevice(const char *ip, uint16_t port);
TCPServerDevice(const char *ip, uint16_t port, bool wait);
virtual ~TCPServerDevice();
virtual bool open() override;
@ -15,10 +15,14 @@ public:
virtual void set_speed(uint32_t speed) override;
virtual ssize_t write(const uint8_t *buf, uint16_t n) override;
virtual ssize_t read(uint8_t *buf, uint16_t n) override;
private:
SocketAPM listener{false};
SocketAPM *sock = NULL;
const char *_ip;
uint16_t _port;
bool _wait;
bool _blocking = false;
};
#endif

12
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -28,7 +28,6 @@
#include "UDPDevice.h"
#include "ConsoleDevice.h"
#include "TCPServerDevice.h"
#include "TCPBlockingServerDevice.h"
extern const AP_HAL::HAL& hal;
@ -274,15 +273,8 @@ void LinuxUARTDriver::_udp_start_connection(void)
void LinuxUARTDriver::_tcp_start_connection(void)
{
if (_flag != NULL) {
if (!strcmp(_flag, "wait")) {
_device = new TCPBlockingServerDevice(_ip, _base_port);
} else {
_device = new TCPServerDevice(_ip, _base_port);
}
} else {
_device = new TCPServerDevice(_ip, _base_port);
}
bool wait = (_flag && strcmp(_flag, "wait") == 0);
_device = new TCPServerDevice(_ip, _base_port, wait);
if (_device->open()) {
_connected = true;