10104756908

Committed 25 Jul 2024 07:10PM UTC coverage: 37.372%. First build

Build # 10104756908

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2360 from mavlink/pr-backport-socket

[BACKPORT v2.12] core: Correctly close sockets

Run Details

23 of 30 new or added lines in 4 files covered. (76.67%)

11097 of 29693 relevant lines covered (37.37%)

255.21 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/core/tcp_connection.cpp

#include "tcp_connection.h"
#include "log.h"

#ifdef WINDOWS
#ifndef MINGW
#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
#endif
#else
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <errno.h>
#include <netdb.h>
#endif

#include <cassert>
#include <utility>

#ifndef WINDOWS
#define GET_ERROR(_x) strerror(_x)
#else
#define GET_ERROR(_x) WSAGetLastError()
#endif

namespace mavsdk {

/* change to remote_ip and remote_port */
TcpConnection::TcpConnection(
    Connection::ReceiverCallback receiver_callback,
    std::string remote_ip,
    int remote_port,
    ForwardingOption forwarding_option) :
    Connection(std::move(receiver_callback), forwarding_option),
    _remote_ip(std::move(remote_ip)),
    _remote_port_number(remote_port),
    _should_exit(false)
{}

TcpConnection::~TcpConnection()
{
    // If no one explicitly called stop before, we should at least do it.
    stop();
}

ConnectionResult TcpConnection::start()
{
    if (!start_mavlink_receiver()) {
        return ConnectionResult::ConnectionsExhausted;
    }

    ConnectionResult ret = setup_port();
    if (ret != ConnectionResult::Success) {
        return ret;
    }

    start_recv_thread();

    return ConnectionResult::Success;
}

ConnectionResult TcpConnection::setup_port()
{
#ifdef WINDOWS
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
        LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
        _is_ok = false;
        return ConnectionResult::SocketError;
    }
#endif

    _socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));

    if (_socket_fd.empty()) {
        LogErr() << "socket error" << GET_ERROR(errno);
        _is_ok = false;
        return ConnectionResult::SocketError;
    }

    struct sockaddr_in remote_addr {};
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(_remote_port_number);

    struct hostent* hp;
    hp = gethostbyname(_remote_ip.c_str());
    if (hp == nullptr) {
        LogErr() << "Could not get host by name";
        _is_ok = false;
        return ConnectionResult::SocketConnectionError;
    }

    memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);

    if (connect(
            _socket_fd.get(),
            reinterpret_cast<sockaddr*>(&remote_addr),
            sizeof(struct sockaddr_in)) < 0) {
        LogErr() << "connect error: " << GET_ERROR(errno);
        _is_ok = false;
        return ConnectionResult::SocketConnectionError;
    }

    _is_ok = true;
    return ConnectionResult::Success;
}

void TcpConnection::start_recv_thread()
{
    _recv_thread = std::make_unique<std::thread>(&TcpConnection::receive, this);
}

ConnectionResult TcpConnection::stop()
{
    _should_exit = true;

    _socket_fd.close();

    if (_recv_thread) {
        _recv_thread->join();
        _recv_thread.reset();
    }

    // We need to stop this after stopping the receive thread, otherwise
    // it can happen that we interfere with the parsing of a message.
    stop_mavlink_receiver();

    return ConnectionResult::Success;
}

bool TcpConnection::send_message(const mavlink_message_t& message)
{
    if (!_is_ok) {
        return false;
    }

    if (_remote_ip.empty()) {
        LogErr() << "Remote IP unknown";
        return false;
    }

    if (_remote_port_number == 0) {
        LogErr() << "Remote port unknown";
        return false;
    }

    struct sockaddr_in dest_addr {};
    dest_addr.sin_family = AF_INET;

    inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);

    dest_addr.sin_port = htons(_remote_port_number);

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);

    // TODO: remove this assert again
    assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);

#if !defined(MSG_NOSIGNAL)
    auto flags = 0;
#else
    auto flags = MSG_NOSIGNAL;
#endif

    const auto send_len = sendto(
        _socket_fd.get(),
        reinterpret_cast<char*>(buffer),
        buffer_len,
        flags,
        reinterpret_cast<const sockaddr*>(&dest_addr),
        sizeof(dest_addr));

    if (send_len != buffer_len) {
        LogErr() << "sendto failure: " << GET_ERROR(errno);
        _is_ok = false;
        return false;
    }
    return true;
}

void TcpConnection::receive()
{
    // Enough for MTU 1500 bytes.
    char buffer[2048];

    while (!_should_exit) {
        if (!_is_ok) {
            LogErr() << "TCP receive error, trying to reconnect...";
            std::this_thread::sleep_for(std::chrono::seconds(1));
            setup_port();
        }

        const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);

        if (recv_len == 0) {
            // This can happen when shutdown is called on the socket,
            // therefore we check _should_exit again.
            _is_ok = false;
            continue;
        }

        if (recv_len < 0) {
            // This happens on destruction when close(_socket_fd.get()) is called,
            // therefore be quiet.
            // LogErr() << "recvfrom error: " << GET_ERROR(errno);
            // Something went wrong, we should try to re-connect in next iteration.
            _is_ok = false;
            continue;
        }

        _mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));

        // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
        while (_mavlink_receiver->parse_message()) {
            receive_message(_mavlink_receiver->get_last_message(), this);
        }
    }
}

} // namespace mavsdk

1	#include "tcp_connection.h"
2	#include "log.h"
3
4	#ifdef WINDOWS
5	#ifndef MINGW
6	#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
7	#endif
8	#else
9	#include <netinet/in.h>
10	#include <sys/socket.h>
11	#include <arpa/inet.h>
12	#include <errno.h>
13	#include <netdb.h>
14	#endif
15
16	#include <cassert>
17	#include <utility>
18
19	#ifndef WINDOWS
20	#define GET_ERROR(_x) strerror(_x)
21	#else
22	#define GET_ERROR(_x) WSAGetLastError()
23	#endif
24
25	namespace mavsdk {
26
27	/* change to remote_ip and remote_port */
28	TcpConnection::TcpConnection(	×
29	Connection::ReceiverCallback receiver_callback,
30	std::string remote_ip,
31	int remote_port,
32	ForwardingOption forwarding_option) :	×
33	Connection(std::move(receiver_callback), forwarding_option),	×
34	_remote_ip(std::move(remote_ip)),	×
35	_remote_port_number(remote_port),
36	_should_exit(false)	×
37	{}	×
38
39	TcpConnection::~TcpConnection()	×
40	{
41	// If no one explicitly called stop before, we should at least do it.
42	stop();	×
43	}	×
44
45	ConnectionResult TcpConnection::start()	×
46	{
47	if (!start_mavlink_receiver()) {	×
48	return ConnectionResult::ConnectionsExhausted;	×
49	}
50
51	ConnectionResult ret = setup_port();	×
52	if (ret != ConnectionResult::Success) {	×
53	return ret;	×
54	}
55
56	start_recv_thread();	×
57
58	return ConnectionResult::Success;	×
59	}
60
61	ConnectionResult TcpConnection::setup_port()	×
62	{
63	#ifdef WINDOWS
64	WSADATA wsa;
65	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
66	LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
67	_is_ok = false;
68	return ConnectionResult::SocketError;
69	}
70	#endif
71
NEW 72	_socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));	×
73
NEW 74	if (_socket_fd.empty()) {	×
75	LogErr() << "socket error" << GET_ERROR(errno);	×
76	_is_ok = false;	×
77	return ConnectionResult::SocketError;	×
78	}
79
80	struct sockaddr_in remote_addr {};	×
81	remote_addr.sin_family = AF_INET;	×
82	remote_addr.sin_port = htons(_remote_port_number);	×
83
84	struct hostent* hp;
85	hp = gethostbyname(_remote_ip.c_str());	×
86	if (hp == nullptr) {	×
87	LogErr() << "Could not get host by name";	×
88	_is_ok = false;	×
89	return ConnectionResult::SocketConnectionError;	×
90	}
91
92	memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);	×
93
NEW 94	if (connect(	×
95	_socket_fd.get(),
96	reinterpret_cast<sockaddr*>(&remote_addr),
NEW 97	sizeof(struct sockaddr_in)) < 0) {	×
98	LogErr() << "connect error: " << GET_ERROR(errno);	×
99	_is_ok = false;	×
100	return ConnectionResult::SocketConnectionError;	×
101	}
102
103	_is_ok = true;	×
104	return ConnectionResult::Success;	×
105	}
106
107	void TcpConnection::start_recv_thread()	×
108	{
109	_recv_thread = std::make_unique<std::thread>(&TcpConnection::receive, this);	×
110	}	×
111
112	ConnectionResult TcpConnection::stop()	×
113	{
114	_should_exit = true;	×
115
NEW 116	_socket_fd.close();	×
117
118	if (_recv_thread) {	×
119	_recv_thread->join();	×
120	_recv_thread.reset();	×
121	}
122
123	// We need to stop this after stopping the receive thread, otherwise
124	// it can happen that we interfere with the parsing of a message.
125	stop_mavlink_receiver();	×
126
127	return ConnectionResult::Success;	×
128	}
129
130	bool TcpConnection::send_message(const mavlink_message_t& message)	×
131	{
132	if (!_is_ok) {	×
133	return false;	×
134	}
135
136	if (_remote_ip.empty()) {	×
137	LogErr() << "Remote IP unknown";	×
138	return false;	×
139	}
140
141	if (_remote_port_number == 0) {	×
142	LogErr() << "Remote port unknown";	×
143	return false;	×
144	}
145
146	struct sockaddr_in dest_addr {};	×
147	dest_addr.sin_family = AF_INET;	×
148
149	inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);	×
150
151	dest_addr.sin_port = htons(_remote_port_number);	×
152
153	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];	×
154	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	×
155
156	// TODO: remove this assert again
157	assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);	×
158
159	#if !defined(MSG_NOSIGNAL)
160	auto flags = 0;
161	#else
162	auto flags = MSG_NOSIGNAL;	×
163	#endif
164
165	const auto send_len = sendto(	×
166	_socket_fd.get(),
167	reinterpret_cast<char*>(buffer),
168	buffer_len,
169	flags,
170	reinterpret_cast<const sockaddr*>(&dest_addr),
171	sizeof(dest_addr));
172
173	if (send_len != buffer_len) {	×
174	LogErr() << "sendto failure: " << GET_ERROR(errno);	×
175	_is_ok = false;	×
176	return false;	×
177	}
178	return true;	×
179	}
180
181	void TcpConnection::receive()	×
182	{
183	// Enough for MTU 1500 bytes.
184	char buffer[2048];	×
185
186	while (!_should_exit) {	×
187	if (!_is_ok) {	×
188	LogErr() << "TCP receive error, trying to reconnect...";	×
189	std::this_thread::sleep_for(std::chrono::seconds(1));	×
190	setup_port();	×
191	}
192
NEW 193	const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);	×
194
195	if (recv_len == 0) {	×
196	// This can happen when shutdown is called on the socket,
197	// therefore we check _should_exit again.
198	_is_ok = false;	×
199	continue;	×
200	}
201
202	if (recv_len < 0) {	×
203	// This happens on destruction when close(_socket_fd.get()) is called,
204	// therefore be quiet.
205	// LogErr() << "recvfrom error: " << GET_ERROR(errno);
206	// Something went wrong, we should try to re-connect in next iteration.
207	_is_ok = false;	×
208	continue;	×
209	}
210
211	_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	×
212
213	// Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
214	while (_mavlink_receiver->parse_message()) {	×
215	receive_message(_mavlink_receiver->get_last_message(), this);	×
216	}
217	}
218	}	×
219
220	} // namespace mavsdk

mavlink / MAVSDK / 10104756908

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous