@ -1,6 +1,6 @@
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright ( C ) 2016 PX4 Development Team . All rights reserved .
* Copyright ( C ) 2016 , 2018 PX4 Development Team . All rights reserved .
*
* Redistribution and use in source and binary forms , with or without
* modification , are permitted provided that the following conditions
@ -35,29 +35,27 @@
@@ -35,29 +35,27 @@
*
* @ author Julian Oes < julian @ oes . ch >
* @ author Beat Küng < beat - kueng @ gmx . net >
* @ author Mara Bos < m - ou . se @ m - ou . se >
*/
# include <errno.h>
# include <fcntl.h>
# include <unistd.h>
# include <string.h>
# include <string>
# include <pthread.h>
# include <poll.h>
# include <sys/stat.h>
# include <sys/socket.h>
# include <sys/types.h>
# include <sys/un.h>
# include <vector>
# include <px4_log.h>
# include "pxh.h"
# include "server.h"
// In Cygwin opening and closing the same named pipe multiple times within one process doesn't work POSIX compliant.
// As a workaround we open the client send pipe file in read write mode such that we can keep it open all the time.
# if !defined(__PX4_CYGWIN)
# define CLIENT_SEND_PIPE_OFLAGS O_RDONLY
# else
# define CLIENT_SEND_PIPE_OFLAGS O_RDWR
# endif
namespace px4_daemon
{
@ -78,22 +76,37 @@ Server::~Server()
@@ -78,22 +76,37 @@ Server::~Server()
int
Server : : start ( )
{
std : : string client_send_pipe_path = get_client_send_pipe_path ( _instance_id ) ;
std : : string sock_path = get_socket_path ( _instance_id ) ;
// Delete socket in case it exists already.
unlink ( sock_path . c_str ( ) ) ;
_fd = socket ( AF_UNIX , SOCK_STREAM , 0 ) ;
if ( _fd < 0 ) {
PX4_ERR ( " error creating socket " ) ;
return - 1 ;
}
// Delete pipe in case it exists already.
unlink ( client_send_pipe_path . c_str ( ) ) ;
sockaddr_un addr = { } ;
addr . sun_family = AF_UNIX ;
strncpy ( addr . sun_path , sock_path . c_str ( ) , sizeof ( addr . sun_path ) - 1 ) ;
// Create new pipe to listen to clients.
// This needs to happen before we return from this method, so that the caller can launch clients.
mkfifo ( client_send_pipe_path . c_str ( ) , 0666 ) ;
if ( bind ( _fd , ( sockaddr * ) & addr , sizeof ( addr ) ) < 0 ) {
PX4_ERR ( " error binding socket " ) ;
return - 1 ;
}
if ( listen ( _fd , 10 ) < 0 ) {
PX4_ERR ( " error listing to socket " ) ;
return - 1 ;
}
if ( 0 ! = pthread_create ( & _server_main_pthread ,
nullptr ,
_server_main_trampoline ,
nullptr ) ) {
this ) ) {
PX4_ERR ( " error creating client handler thread " ) ;
return - 1 ;
}
@ -101,12 +114,9 @@ Server::start()
@@ -101,12 +114,9 @@ Server::start()
}
void *
Server : : _server_main_trampoline ( void * arg )
Server : : _server_main_trampoline ( void * self )
{
if ( _instance ) {
_instance - > _server_main ( arg ) ;
}
( ( Server * ) self ) - > _server_main ( ) ;
return nullptr ;
}
@ -116,9 +126,8 @@ void Server::_pthread_key_destructor(void *arg)
@@ -116,9 +126,8 @@ void Server::_pthread_key_destructor(void *arg)
}
void
Server : : _server_main ( void * arg )
Server : : _server_main ( )
{
// Set thread specific pipe to supplied file descriptor.
int ret = pthread_key_create ( & _key , _pthread_key_destructor ) ;
if ( ret ! = 0 ) {
@ -126,241 +135,151 @@ Server::_server_main(void *arg)
@@ -126,241 +135,151 @@ Server::_server_main(void *arg)
return ;
}
std : : string client_send_pipe_path = get_client_send_pipe_path ( _instance_id ) ;
int client_send_pipe_fd = open ( client_send_pipe_path . c_str ( ) , CLIENT_SEND_PIPE_OFLAGS ) ;
// The list of file descriptors to watch.
std : : vector < pollfd > poll_fds ;
while ( true ) {
// Watch the listening socket for incoming connections.
poll_fds . push_back ( pollfd { _fd , POLLIN , 0 } ) ;
client_send_packet_s packet ;
while ( true ) {
int n_ready = poll ( poll_fds . data ( ) , poll_fds . size ( ) , - 1 ) ;
// We only read as much as we need, otherwise we might get out of
// sync with packets.
int bytes_read = read ( client_send_pipe_fd , & packet , sizeof ( client_send_packet_s : : header ) ) ;
if ( n_ready < 0 ) {
PX4_ERR ( " poll() failed: %s " , strerror ( errno ) ) ;
return ;
}
if ( bytes_read > 0 ) {
_lock ( ) ;
// Using the header we can determine how big the payload is.
int payload_to_read = sizeof ( packet )
- sizeof ( packet . header )
- sizeof ( packet . payload )
+ packet . header . payload_length ;
// Handle any new connections.
if ( poll_fds [ 0 ] . revents & POLLIN ) {
- - n_ready ;
int client = accept ( _fd , nullptr , nullptr ) ;
// Again, we only read as much as we need because otherwise we need
// hold a buffer and parse it.
bytes_read = read ( client_send_pipe_fd , ( ( uint8_t * ) & packet ) + bytes_read , payload_to_read ) ;
if ( client = = - 1 ) {
PX4_ERR ( " failed to accept client: %s " , strerror ( errno ) ) ;
_unlock ( ) ;
return ;
}
if ( bytes_read > 0 ) {
// Start a new thread to handle the client.
pthread_t * thread = & _fd_to_thread [ client ] ;
ret = pthread_create ( thread , nullptr , Server : : _handle_client , ( void * ) ( intptr_t ) client ) ;
_parse_client_send_packet ( packet ) ;
if ( ret ! = 0 ) {
PX4_ERR ( " could not start pthread (%i) " , ret ) ;
_fd_to_thread . erase ( client ) ;
close ( client ) ;
} else {
// We won't join the thread, so detach to automatically release resources at its end
pthread_detach ( * thread ) ;
}
}
if ( bytes_read = = 0 ) {
// 0 means the pipe has been closed by all clients
// and we need to re-open it.
close ( client_send_pipe_fd ) ;
client_send_pipe_fd = open ( client_send_pipe_path . c_str ( ) , O_RDONLY ) ;
// Start listening for the client hanging up.
poll_fds . push_back ( pollfd { client , POLLHUP , 0 } ) ;
}
}
close ( client_send_pipe_fd ) ;
}
// Handle any closed connections.
for ( size_t i = 1 ; n_ready > 0 & & i < poll_fds . size ( ) ; ) {
if ( poll_fds [ i ] . revents ) {
- - n_ready ;
auto thread = _fd_to_thread . find ( poll_fds [ i ] . fd ) ;
void
Server : : _parse_client_send_packet ( const client_send_packet_s & packet )
{
switch ( packet . header . msg_id ) {
case client_send_packet_s : : message_header_s : : e_msg_id : : EXECUTE :
_execute_cmd_packet ( packet ) ;
break ;
case client_send_packet_s : : message_header_s : : e_msg_id : : KILL :
_kill_cmd_packet ( packet ) ;
break ;
default :
PX4_ERR ( " send msg_id not handled " ) ;
break ;
}
}
if ( thread ! = _fd_to_thread . end ( ) ) {
// Thread is still running, so we cancel it.
// TODO: use a more graceful exit method to avoid resource leaks
pthread_cancel ( thread - > second ) ;
_fd_to_thread . erase ( thread ) ;
}
void
Server : : _execute_cmd_packet ( const client_send_packet_s & packet )
{
if ( packet . header . payload_length = = 0 ) {
PX4_ERR ( " command length 0 " ) ;
return ;
}
close ( poll_fds [ i ] . fd ) ;
poll_fds . erase ( poll_fds . begin ( ) + i ) ;
// We open the client's specific pipe to write the return value and stdout back to.
// The pipe's path is created knowing the UUID of the client.
char path [ RECV_PIPE_PATH_LEN ] ;
int ret = get_client_recv_pipe_path ( packet . header . client_uuid , path , RECV_PIPE_PATH_LEN ) ;
if ( ret < 0 ) {
PX4_ERR ( " failed to assemble path " ) ;
return ;
}
int pipe_fd = open ( path , O_WRONLY ) ;
if ( pipe_fd < 0 ) {
PX4_ERR ( " pipe open fail " ) ;
return ;
}
// To execute a command we start a new thread.
pthread_t new_pthread ;
// We need to copy everything that the new thread needs because we will go
// out of scope.
RunCmdArgs * args = new RunCmdArgs ;
strncpy ( args - > cmd , ( char * ) packet . payload . execute_msg . cmd , sizeof ( args - > cmd ) ) ;
args - > client_uuid = packet . header . client_uuid ;
args - > pipe_fd = pipe_fd ;
args - > is_atty = packet . payload . execute_msg . is_atty ;
_lock ( ) ; // need to lock, otherwise the thread could already exit before we insert into the map
ret = pthread_create ( & new_pthread , nullptr , Server : : _run_cmd , ( void * ) args ) ;
} else {
+ + i ;
}
}
if ( ret ! = 0 ) {
PX4_ERR ( " could not start pthread (%i) " , ret ) ;
delete args ;
} else {
// We won't join the thread, so detach to automatically release resources at its end
pthread_detach ( new_pthread ) ;
// We keep two maps for cleanup if a thread is finished or killed.
_client_uuid_to_pthread . insert ( std : : pair < uint64_t , pthread_t >
( packet . header . client_uuid , new_pthread ) ) ;
_pthread_to_pipe_fd . insert ( std : : pair < pthread_t , int >
( new_pthread , pipe_fd ) ) ;
_unlock ( ) ;
}
_unlock ( ) ;
close ( _fd ) ;
}
void
Server : : _kill_cmd_packet ( const client_send_packet_s & packet )
* Server : : _handle_client ( void * arg )
{
_lock ( ) ;
int fd = ( int ) ( intptr_t ) arg ;
// TODO: we currently ignore the signal type .
auto client_uuid_iter = _client_uuid_to_pthread . find ( packet . header . client_uuid ) ;
// Read until the end of the incoming stream.
std : : string cmd ;
if ( client_uuid_iter = = _client_uuid_to_pthread . end ( ) ) {
_unlock ( ) ;
return ;
}
pthread_t pthread_to_kill = client_uuid_iter - > second ;
// TODO: use a more graceful exit method to avoid resource leaks
int ret = pthread_cancel ( pthread_to_kill ) ;
while ( true ) {
size_t n = cmd . size ( ) ;
cmd . resize ( n + 1024 ) ;
ssize_t n_read = read ( fd , & cmd [ n ] , cmd . size ( ) - n ) ;
__cleanup_thread ( packet . header . client_uuid ) ;
if ( n_read < 0 ) {
_cleanup ( fd ) ;
return nullptr ;
}
_unlock ( ) ;
cmd . resize ( n + n_read ) ;
if ( ret ! = 0 ) {
PX4_ERR ( " failed to cancel thread " ) ;
if ( n_read = = 0 ) {
break ;
}
}
// We don't send retval when we get killed.
// The client knows this and just exits without confirmation.
}
void
* Server : : _run_cmd ( void * arg )
{
RunCmdArgs * args = ( RunCmdArgs * ) arg ;
// Copy arguments so that we can cleanup the arg structure.
uint64_t client_uuid = args - > client_uuid ;
int pipe_fd = args - > pipe_fd ;
bool is_atty = args - > is_atty ;
std : : string message_str ( args - > cmd ) ;
if ( cmd . size ( ) < 2 ) {
_cleanup ( fd ) ;
return nullptr ;
}
// Clean up the args from the heap in case the thread gets canceled
// from outside.
delete args ;
// Last byte is 'isatty'.
uint8_t isatty = cmd . back ( ) ;
cmd . pop_back ( ) ;
// We register thread specific data. This is used for PX4_INFO (etc.) log calls.
CmdThreadSpecificData * thread_data_ptr ;
if ( ( thread_data_ptr = ( CmdThreadSpecificData * ) pthread_getspecific ( _instance - > _key ) ) = = nullptr ) {
thread_data_ptr = new CmdThreadSpecificData ;
thread_data_ptr - > pipe_fd = pipe_fd ;
thread_data_ptr - > is_atty = is_atty ;
thread_data_ptr - > packet . header . msg_id = client_recv_packet_s : : message_header_s : : e_msg_id : : STDOUT ;
thread_data_ptr - > fd = fd ;
thread_data_ptr - > is_atty = isatty ;
( void ) pthread_setspecific ( _instance - > _key , ( void * ) thread_data_ptr ) ;
}
// Run the actual command.
int retval = Pxh : : process_line ( message_str , true ) ;
int retval = Pxh : : process_line ( cmd , true ) ;
// Report return value.
_send_retval ( pipe_fd , retval , client_uuid ) ;
// Clean up before returning.
_instance - > _cleanup_thread ( client_uuid ) ;
char buf [ 2 ] = { 0 , ( char ) retval } ;
return nullptr ;
}
void
Server : : _send_retval ( const int pipe_fd , const int retval , const uint64_t client_uuid )
{
client_recv_packet_s packet ;
packet . header . msg_id = client_recv_packet_s : : message_header_s : : e_msg_id : : RETVAL ;
packet . header . payload_length = sizeof ( packet . payload . retval_msg ) ;
packet . payload . retval_msg . retval = retval ;
int bytes_to_send = get_client_recv_packet_length ( & packet ) ;
int bytes_sent = write ( pipe_fd , & packet , bytes_to_send ) ;
if ( bytes_sent ! = bytes_to_send ) {
printf ( " write fail \n " ) ;
return ;
if ( write ( fd , buf , sizeof buf ) < 0 ) {
// Don't care it went wrong, as we're cleaning up anyway.
}
}
void
Server : : _cleanup_thread ( const uint64_t client_uuid )
{
_lock ( ) ;
__cleanup_thread ( client_uuid ) ;
_unlock ( ) ;
_cleanup ( fd ) ;
return nullptr ;
}
void
Server : : __cleanup_thread ( const uint64_t client_uui d )
Server : : _cleanup ( int fd )
{
pthread_t pthread_killed = _client_uuid_to_pthread [ client_uuid ] ;
auto pipe_iter = _pthread_to_pipe_fd . find ( pthread_killed ) ;
if ( pipe_iter = = _pthread_to_pipe_fd . end ( ) ) {
// can happen if the thread already exited and then got a kill packet
PX4_DEBUG ( " pipe fd already closed " ) ;
return ;
}
int pipe_fd = pipe_iter - > second ;
close ( pipe_fd ) ;
char path [ RECV_PIPE_PATH_LEN ] = { } ;
get_client_recv_pipe_path ( client_uuid , path , RECV_PIPE_PATH_LEN ) ;
unlink ( path ) ;
_client_uuid_to_pthread . erase ( client_uuid ) ;
_pthread_to_pipe_fd . erase ( pthread_killed ) ;
_instance - > _lock ( ) ;
_instance - > _fd_to_thread . erase ( fd ) ;
_instance - > _unlock ( ) ;
// We can't close() the fd here, since the main thread is probably
// polling for it: close()ing it causes a race condition.
// So, we only call shutdown(), which causes the main thread to register a
// 'POLLHUP', such that the main thread can close() it for us.
// We already removed this thread from _fd_to_thread, so there is no risk
// of the main thread trying to cancel this thread after it already exited.
shutdown ( fd , SHUT_RDWR ) ;
}
} //namespace px4_daemon
Mara Bos
6 years ago
committed by
Daniel Agar