/**
* \brief Procedure to initialize the communication of navdata with the drone.
* \return 0 if success, -1 if an error occured
*/
int navdata_init()
{
fd_set fds;
FD_ZERO(&fds);
FD_SET(sock_navdata, &fds);
struct timeval timeout;
timeout.tv_sec = 5;
timeout.tv_usec = 0;
if (sendto(sock_navdata, "\x01", 1, 0, (struct sockaddr*)&addr_drone_navdata, sizeof(addr_drone_navdata)) < 0) {
perror("[~][navdata] Can't send ping\n");
return -1;
}
if (select(sock_navdata+1, &fds, NULL, NULL, &timeout) <= 0) {
perror("[~][navdata] Ping ack not received\n");
return -1;
}
if (recv_cmd() < 0) {
perror("[~][navdata] First navdata packet not received\n");
return -1;
}
if (data.raw.ardrone_state & (1 << 11)) {
fprintf(stderr, "[*][navdata] bootstrap: %d\n", (data.raw.ardrone_state & (1 << 11))%2);
}
if (data.raw.ardrone_state & (1 << 15)) {
fprintf(stderr, "[*][navdata] Battery charge too low: %d\n", data.raw.ardrone_state & (1 << 15));
return -1;
}
if (init_navdata_bootstrap() < 0){
fprintf(stderr, "[~][navdata] bootstrap init failed\n");
return -1;
}
if (recv_cmd() < 0)
perror("[~][navdata] Second navdata packet not received");
if (data.raw.ardrone_state & (1 << 6)) {
fprintf(stderr, "[*][navdata] control command ACK: %d\n", (data.raw.ardrone_state & (1 << 6))%2);
}
if (init_navdata_ack() < 0){
fprintf(stderr, "[~][navdata] Init ack failed\n");
return -1;
}
if (data.raw.ardrone_state & (1 << 11)) {
fprintf(stderr, "[~][navdata] bootstrap end: %d\n", (data.raw.ardrone_state & (1 << 11))%2);
}
return 0;
}
/**
* \brief Procedure to initialize the communication of navdata with the drone.
* \return 0 if success, -1 if an error occured
*/
static int navdata_init()
{
fd_set fds;
FD_ZERO(&fds);
FD_SET(sock_navdata, &fds);
struct timeval timeout;
timeout.tv_sec = 5;
timeout.tv_usec = 0;
//FBO
//fprintf(stderr, "navdata_init sending packet to %s:%d\n", inet_ntoa(addr_drone_navdata.sin_addr),
// ntohs(addr_drone_navdata.sin_port));
//LBO
if (sendto(sock_navdata, "\x01", 1, 0, (struct sockaddr*)&addr_drone_navdata, sizeof(addr_drone_navdata)) < 0) {
perror("[~][navdata] Can't send ping");
return -1;
}
if (select(sock_navdata+1, &fds, NULL, NULL, &timeout) <= 0) {
perror("[~][navdata] Ping ack not received");
return -1;
}
if (recv_cmd() < 0) {
perror("[~][navdata] First navdata packet not received");
return -1;
}
if (data.raw.ardrone_state & (1 << 11)) {
fprintf(stderr, "[*][navdata] bootstrap: %d\n", (data.raw.ardrone_state >> 11) & 1);
}
uint32_t process_cmd(
MEI_HANDLE *ptrHandle,
uint32_t cmd_id,
struct data_buffer buf_ptr_in[],
struct data_buffer buf_ptr_out[],
uint32_t num_params)
{
uint32_t status = TEE_SUCCESSFUL;
int ret = 0;
if (!buf_ptr_in || !buf_ptr_out || !num_params)
return TEE_FAIL_INVALID_PARAM;
ret = validate_data_buffer_params(buf_ptr_in, buf_ptr_out, num_params);
if (ret) {
LOGERR("Data buffer params invalid, ret=0x%x", ret);
return TEE_FAIL_INVALID_PARAM;
}
ret = send_cmd(ptrHandle, cmd_id, buf_ptr_in, num_params);
if (ret) {
LOGERR("Send Command, error=0x%08x, cmd-id=0x%08x\n", ret, cmd_id);
}
ret = recv_cmd(ptrHandle, cmd_id, buf_ptr_out, num_params);
if (ret) {
LOGERR("Receive Command, error=0x%08x, cmd-id=0x%08x\n", ret, cmd_id);
}
final_exit:
return ret;
}
static int execute_cmd(int sockfd, t_serv_fs *serv_fs)
{
char command[MAX_CMD_SIZE];
char **args;
int nb_args;
int ret;
command[4095] = '\0';
if (recv_cmd(sockfd, command, MAX_CMD_SIZE - 1) < 0)
return (-1);
nb_args = get_args(command, &args);
if (nb_args < 0)
return (-1);
if ((ret = exe_from_arg(sockfd, args, nb_args, serv_fs)) == 0)
printf("Unable to find cmd %s\n", command);
free_args(&args, nb_args);
return (ret);
}
/**
* \brief navdata_thread routine which keep the connection alive.
*/
void* navdata_routine(void* args)
{
pthread_mutex_lock(&mutex_stopped);
while (!stopped_navdata) {
pthread_mutex_unlock(&mutex_stopped);
if (recv_cmd() < 0)
perror("[~][navdata] Failed to receive navdata");
usleep(NAVDATA_INTERVAL*1000);
if (sendto(sock_navdata, "\x01", 1, 0, (struct sockaddr*)&addr_drone_navdata, sizeof(addr_drone_navdata)) < 0) {
perror("[~][navdata] Failed to send ping\n");
pthread_exit(NULL);
}
pthread_mutex_lock(&mutex_stopped);
}
pthread_mutex_unlock(&mutex_stopped);
pthread_exit(NULL);
}
void process_loop(int listen_fd)
{
int num_connections = 0;
int connections[MAX_CONNECTIONS];
int pipefds[2];
int exit_pipe_read;
bzero(connections, sizeof(connections));
// A pipe is used to allow the render threads to request an exit by the main process
if (pipe(pipefds)) {
fprintf(stderr, "Failed to create pipe\n");
return;
}
exit_pipe_fd = pipefds[1];
exit_pipe_read = pipefds[0];
while (1) {
struct sockaddr_un in_addr;
socklen_t in_addrlen = sizeof(in_addr);
fd_set rd;
int incoming, num, nfds, i;
FD_ZERO(&rd);
FD_SET(listen_fd, &rd);
nfds = listen_fd+1;
for (i=0; i<num_connections; i++) {
FD_SET(connections[i], &rd);
nfds = MAX(nfds, connections[i]+1);
}
FD_SET(exit_pipe_read, &rd);
nfds = MAX(nfds, exit_pipe_read+1);
num = select(nfds, &rd, NULL, NULL, NULL);
if (num == -1)
perror("select()");
else if (num) {
if (FD_ISSET(exit_pipe_read, &rd)) {
// A render thread wants us to exit
break;
}
//printf("Data is available now on %d fds\n", num);
if (FD_ISSET(listen_fd, &rd)) {
num--;
incoming = accept(listen_fd, (struct sockaddr *) &in_addr, &in_addrlen);
if (incoming < 0) {
perror("accept()");
} else {
if (num_connections == MAX_CONNECTIONS) {
syslog(LOG_WARNING, "Connection limit(%d) reached. Dropping connection\n", MAX_CONNECTIONS);
close(incoming);
} else {
connections[num_connections++] = incoming;
syslog(LOG_DEBUG, "DEBUG: Got incoming connection, fd %d, number %d\n", incoming, num_connections);
}
}
}
for (i=0; num && (i<num_connections); i++) {
int fd = connections[i];
if (FD_ISSET(fd, &rd)) {
struct protocol cmd;
int ret = 0;
memset(&cmd,0,sizeof(cmd));
// TODO: to get highest performance we should loop here until we get EAGAIN
ret = recv_cmd(&cmd, fd, 0);
if (ret < 1) {
int j;
num_connections--;
syslog(LOG_DEBUG, "DEBUG: Connection %d, fd %d closed, now %d left\n", i, fd, num_connections);
for (j=i; j < num_connections; j++)
connections[j] = connections[j+1];
request_queue_clear_requests_by_fd(render_request_queue, fd);
close(fd);
} else {
enum protoCmd rsp = rx_request(&cmd, fd);
if (rsp == cmdNotDone) {
cmd.cmd = rsp;
syslog(LOG_DEBUG, "DEBUG: Sending NotDone response(%d)\n", rsp);
ret = send_cmd(&cmd, fd);
}
}
}
}
} else {
syslog(LOG_ERR, "Select timeout");
}
}
}
int gs_build_server(int port, int listen_num)
{
int server_sockfd, client_sockfd;
int server_len, client_len;
struct sockaddr_in server_address;
struct sockaddr_in client_address;
int result;
fd_set readfds, testfds;
log_msg(__FUNCTION__);
/* Create and name a socket for the server. */
server_sockfd = socket(AF_INET, SOCK_STREAM, 0);
server_address.sin_family = AF_INET;
server_address.sin_addr.s_addr = htonl(INADDR_ANY);
server_address.sin_port = htons(port);
server_len = sizeof(server_address);
bind(server_sockfd, (struct sockaddr *)&server_address, server_len);
/* Create a connection queue and initialize readfds to handle input from server_sockfd. */
listen(server_sockfd, listen_num);
FD_ZERO(&readfds);
FD_SET(server_sockfd, &readfds);
/* Now wait for clients and requests.
Since we have passed a null pointer as the timeout parameter, no timeout will occur.
The program will exit and report an error if select returns a value of less than 1. */
while(1)
{
int fd;
int nread;
testfds = readfds;
printf("server waiting\n");
result = select(FD_SETSIZE, &testfds, (fd_set *)0,
(fd_set *)0, (struct timeval *) 0);
if(result < 1)
{
perror("select() failed");
log_err("gs_build_server()", ERR_SELECT_FAILED);
continue;
}
/* Once we know we've got activity,
we find which descriptor it's on by checking each in turn using FD_ISSET. */
for(fd = 0; fd < FD_SETSIZE; fd++)
{
if(FD_ISSET(fd,&testfds))
{
/* If the activity is on server_sockfd, it must be a request for a new connection
and we add the associated client_sockfd to the descriptor set. */
if(fd == server_sockfd)
{
client_len = sizeof(client_address);
client_sockfd = accept(server_sockfd, (struct sockaddr *)&client_address, (socklen_t *)&client_len);
FD_SET(client_sockfd, &readfds);
}
/* If it isn't the server, it must be client activity.
If close is received, the client has gone away and we remove it from the descriptor set.
Otherwise, we 'serve' the client as in the previous examples. */
else
{
ioctl(fd, FIONREAD, &nread);
if(nread != 0)
{
log_msg("do recv_cmd()");
recv_cmd(fd, client_address.sin_addr.s_addr);
log_msg("done recv_cmd()");
}
close(fd);
FD_CLR(fd, &readfds);
//printf("removing client on fd %d\n", fd);
}
}
}
}
}
int main (int argc, char **argv)
{
char command [CMD_SIZE];
int initialize_tape = 1;
int tape_status = 5;
int status;
unsigned char *beam_ptr;
int first_time = TRUE;
LL_beam_rec *enet_pak;
extern process_exit();
extern print_target_elev ();
/* trap signals */
(void) signal (SIGINT, ((void (*) ()) process_exit));
(void) signal (SIGHUP, ((void (*) ()) process_exit));
(void) signal (SIGTERM, ((void (*) ()) process_exit));
enet_pak = (LL_beam_rec *) malloc ((unsigned) sizeof (LL_beam_rec));
if (enet_pak == NULL)
{
(void) printf ("Malloc error \n");
return (-1);
}
if ((!get_params (&Glob)))
exit (2);
/* initialize streams (fmq and/or shmem) for sending beam data */
if (init_streams ())
{
(void) fprintf (stderr, "streams initialization error");
return (1);
}
if (system ("clear") != 0)
perror ("clear");
/* Determine the source of the data to be reformatted */
if (!Glob.real_time)
{
/* initialize the shared memory for receiving commands*/
if (recv_cmd (1, Glob.cmd_shmem_key, (unsigned char **) &command) == -1)
{
(void) printf ("recv_cmd fatal error\n");
exit (-1);
}
for (;;)
{
if ((status = get_shm_cmd (command)) > 0)
break;
else
if (first_time)
{
first_time = FALSE;
(void) printf ("waiting for menu process to send start command\n");
}
sleep (2);
}
source = parse_settings (command, &Glob);
if (source < 0)
return (0);
}
else
source = RADAR;
switch (source)
{
case RADAR:
if (recv_shm (1, Glob.input_shmem_key, &beam_ptr))
{
(void) printf ("recv_shm fatal error\n");
exit (-1);
}
for (;go ;)
{
if (get_shmem_data (enet_pak, Glob.input_shmem_key)
== 0)
{
if (first_time)
{
(void) printf ("waiting for data\n");
first_time = FALSE;
}
usleep (no_packet);
}
/* Check for a stop command */
if (!Glob.real_time)
{
if ((status = get_shm_cmd (command)) > 0)
{
source = parse_settings (command, &Glob);
if (source < 0) go = 0;
if (source == FLAG_CHANGE)
{
(void) printf ("\n Flag status: Dealias failure = %s\n",
Glob.caf ? "TRUE":"FALSE");
(void) printf (" Point Target = %s\n",
Glob.ctf ? "TRUE":"FALSE");
(void) printf (" SNR thresholding = %s\n",
Glob.cvf ? "TRUE":"FALSE");
(void) printf (" Clutter = %s\n",
Glob.ccv ? "TRUE":"FALSE");
(void) printf (" low PRF in valid gate = %s\n\n",
Glob.cv ? "TRUE":"FALSE");
}
}
}
}
break;
case TAPE:
if (Glob.remote_tape)
{
if (tape_init (&fd_tape, Glob.tape_device))
{
(void) printf("Error initializing the tape device %s",
Glob.tape_device);
source = -1;
return (0);
}
}
for (;go ;)
{
if (initialize_tape)
{
initialize_tape = FALSE;
tape_command ();
}
tape_status = get_tape_data (fd_tape, data_rate, enet_pak);
switch (tape_status)
{
case 0:
/* automatically rewind the tape
* when end of tape is detected
*/
/*
tape_rewind = REWIND;
initialize_tape = TRUE;
*/
/* or stop the system at the end of the tape */
(void) printf ("Received STOP command \n");
go = 0;
exit (0);
break;
case INITIALIZE_RMT:
(void) close (fd_tape);
if (tape_init (&fd_tape, Glob.tape_device))
{
(void) printf("Error initializing the tape device %s",
Glob.tape_device);
source = -1;
return (0);
}
break;
case INITIALIZE:
initialize_tape = TRUE;
break;
case REWIND:
/*go = 0;
exit (0);
break;*/
tape_rewind = REWIND;
initialize_tape = TRUE;
break;
case TAPE_ERR:
(void) printf ("Tape read error / received STOP command \n");
go = 0;
break;
case RATE_CHANGE:
initialize_tape = FALSE;
break;
case PAUSE:
for (;;)
{
if (Glob.remote_tape)
{
(void) rmt_close ();
}
if ((status = get_shm_cmd (command)) > 0)
{
if (Glob.remote_tape)
{
if (tape_init (&fd_tape, Glob.tape_device))
{
(void) printf("Error initializing the tape device %s", Glob.tape_device);
go = 0;
}
}
break;
}
else
usleep (3000);
}
break;
case FLAG_CHANGE:
(void) printf ("\n Flag status: Dealias failure = %s\n",
Glob.caf ? "TRUE":"FALSE");
(void) printf (" Point Target = %s\n",
Glob.ctf ? "TRUE":"FALSE");
(void) printf (" SNR thresholding = %s\n",
Glob.cvf ? "TRUE":"FALSE");
(void) printf (" Clutter = %s\n",
Glob.ccv ? "TRUE":"FALSE");
(void) printf (" low PRF in valid gate = %s\n\n",
Glob.cv ? "TRUE":"FALSE");
}
}
break;
default:
(void) printf ("source unrecognized \n");
break;
}
return (0);
}
static int ui_android_read(UI *ui, UI_STRING *uis)
{
int ok = 0;
int num = 0;
const char *str;
char **args;
switch (UI_get_string_type(uis)) {
case UIT_BOOLEAN:
android_log(PRG_DEBUG, "called ui_android_read() output boolean %s",
UI_get0_output_string(uis));
android_log(PRG_DEBUG, "called ui_android_read() action boolean %s",
UI_get0_action_string(uis));
// UI_get_input_flags(uis) & UI_INPUT_FLAG_ECHO
case UIT_PROMPT:
str = UI_get0_output_string(uis);
android_log(PRG_DEBUG, "called ui_android_read() output prompt %s",
str);
break;
case UIT_VERIFY:
str = UI_get0_output_string(uis);
android_log(PRG_DEBUG, "called ui_android_read() verify output %s",
str);
break;
default:
return 1;
break;
}
strlcat(ui_buf, "=X=", sizeof(ui_buf));
strlcat(ui_buf, str, sizeof(ui_buf));
process:
if (send_req(ui_buf) < 0) {
android_log(PRG_ERR, "send_req() failed");
goto out;
}
if (recv_cmd(&num, &args) < 0) {
android_log(PRG_ERR, "recv_cmd() failed");
goto out;
}
if (num != 1) {
android_log(PRG_ERR, "parameter number mismatch");
goto out;
}
android_log(PRG_DEBUG, "ui_android_read() cmd: %s %d", args[0], num);
send_ack(num);
UI_set_result(ui, uis, args[0]);
ok = 1;
out:
if (num)
free_cmd(num, args);
ui_buf[0] = '\0';
return ok;
}
/* called by both caller and callee, DONE */
static int
call_run(int sock_fd, int p_rfd, call_fn_t *op, tmeo_t *tmeo)
{
char buf[64];
int ret = 0;
char c;
int maxfd;
fd_set rset;
struct timeval tv;
while (1)
{
memset(buf, '\0', 64);
FD_ZERO(&rset);
FD_SET(sock_fd, &rset);
FD_SET(p_rfd, &rset);
maxfd = U_MAX(p_rfd, sock_fd) + 1;
printf("--- timeout = %d ---\n", *(tmeo->tmeo));
if (*(tmeo->tmeo) > 0)
{
tv.tv_sec = *(tmeo->tmeo);
tv.tv_usec = 0;
ret = select(maxfd, &rset, NULL, NULL, &tv);
}
else if (*(tmeo->tmeo) == 0)
{
tv.tv_sec = tmeo->max_tmeo;
tv.tv_usec = 0;
ret = select(maxfd, &rset, NULL, NULL, &tv);
}
else if (*(tmeo->tmeo) < 0)
{
ret = select(maxfd, &rset, NULL, NULL, NULL);
}
if (ret < 0)
{
if (errno == EINTR)
{
printf("--- select interrupted ---\n");
continue;
}
else
{
printf("--- select error ---\n");
return -1;
}
}
else if (ret > 0)
{
*(tmeo->tmeo) = 0;
if (FD_ISSET(sock_fd, &rset))
{
printf("--- received net cmd ---\n");
if ((ret = recv_cmd(sock_fd, op->nfn)) < 0) break;
}
if (FD_ISSET(p_rfd, &rset))
{
printf("--- detect pipe cmd ---\n");
if (u_read(p_rfd, &c, 1)==1)
{
printf("--- received pipe cmd: %c ---\n", c);
if ((ret = op->pfn(c)) < 0) break;
}
}
}
else if (ret == 0)
{
printf("--- timedout ---\n");
if ((ret = op->tfn()) < 0) break;
}
}
return ret;
}
/**
* XBH server thread
*/
static void xbh_srv_task(void *arg){/*{{{*/
struct sockaddr_in listen_addr;
struct sockaddr_in clnt_addr;
int srv_sock = -1, clnt_sock = -1;
socklen_t clnt_addr_sz;
int retval;
xbhsrv_state state = XBHSRV_ACCEPT;
//Setup socket and listen/*{{{*/
memset(&listen_addr, 0, sizeof(listen_addr));
listen_addr.sin_family = AF_INET;
listen_addr.sin_port = htons(TCP_XBH_PORT);
listen_addr.sin_addr.s_addr = htonl(INADDR_ANY);
srv_sock = socket(PF_INET,SOCK_STREAM,0);
LOOP_ERRMSG(srv_sock < 0, "Could not create socket\n");
retval = bind(srv_sock, (struct sockaddr *)&listen_addr, (socklen_t)sizeof(listen_addr));
LOOP_ERRMSG(retval != 0, "Could not bind socket to address\n");
retval = listen(srv_sock, 0);
LOOP_ERRMSG(retval != 0, "Could not listen on socket\n");
/*}}}*/
while(true){
//If socket closed, go back to accepting another connection
if(send(clnt_sock, NULL, 0, 0) < 0){
state = XBHSRV_ACCEPT;
}
switch(state){
case XBHSRV_ACCEPT:
clnt_addr_sz = sizeof(clnt_addr);
clnt_sock = accept(srv_sock, (struct sockaddr *)&clnt_addr, &clnt_addr_sz);
LOOP_ERRMSG(clnt_sock < 0, "Failed to accept connection\n");
uart_printf("New connection from %hd.%hd.%hd.%hd, port = %d\n",
((uint8_t *)&clnt_addr.sin_addr.s_addr)[0],
((uint8_t *)&clnt_addr.sin_addr.s_addr)[1],
((uint8_t *)&clnt_addr.sin_addr.s_addr)[2],
((uint8_t *)&clnt_addr.sin_addr.s_addr)[3],
clnt_addr.sin_port);
state = XBHSRV_CMD;
break;
case XBHSRV_CMD:
{
ssize_t len = recv_cmd(clnt_sock, xbh_cmd, XBH_PACKET_SIZE_MAX);
if (len < 0){
goto cmd_err;
}
#if 0
// Reset if reset cmd received
if(!memcmp(xbh_cmd, XBH_CMD[XBH_CMD_rsr], len)){
xbh_reset();
}
#endif
struct xbh_srv_to_hndlr_msg tx_msg;
tx_msg.cmd_buf = xbh_cmd;
tx_msg.len = len;
xQueueSend(xbh_srv_to_hndlr_q_handle, &tx_msg, portMAX_DELAY);
portYIELD();
state = XBHSRV_WAIT;
break;
cmd_err:
close(clnt_sock);
state = XBHSRV_ACCEPT;
break;
}
case XBHSRV_WAIT:
{
static TickType_t last_sent = 0;
// Check for xbh handle completion and issue reply to PC
struct xbh_hndlr_to_srv_msg rx_msg;
if(xQueueReceive(xbh_hndlr_to_srv_q_handle, &rx_msg, 1) == pdTRUE){
if(rx_msg.type == XBH_HNDLR_DONE){
size_t len = rx_msg.len;
// Prepend length to buffer
len2hex(len, reply_buf);
len += CMDLEN_SZ;
sendall(clnt_sock, reply_buf, len, 0);
state = XBHSRV_CMD;
}
last_sent = xTaskGetTickCount();
//If last keepalive greater than 1k ticks ago, send
//keepalive
}else if (xTaskGetTickCount() - last_sent > 1000){
memcpy(wait_buf+CMDLEN_SZ, XBH_CMD[XBH_CMD_kao], XBH_COMMAND_LEN);
len2hex(XBH_COMMAND_LEN, wait_buf);
int retval = sendall(clnt_sock, wait_buf, XBH_COMMAND_LEN+CMDLEN_SZ, 0);
if (retval < 0) xbh_reset();
last_sent = xTaskGetTickCount();
}
// Unneeded, just reset if connection terminated. Ugly, but should work
// If enabling, add GEN(rsr) to XBH_CMD
#if 0
//Check for RST from pc
struct timeval tv;
fd_set read_fds;
int nready = 0;
memset(&tv, 0, sizeof(tv));
FD_SET(clnt_sock, &read_fds);
nready = select(clnt_sock+1, &read_fds, NULL, NULL, &tv);
if(nready < 0){
goto wait_err;
}
if(nready == 1){
ssize_t len = recv_cmd(clnt_sock, wait_buf, XBH_WAITBUF_MAX);
if(len < 0){
goto wait_err;
}else if(!memcmp(wait_buf, XBH_CMD[XBH_CMD_rsr], len)){
xbh_reset();
}
}
break;
wait_err:
close(clnt_sock);
state = XBHSRV_ACCEPT;
break;
#endif
}
}
#if DEBUG_STACK
DEBUG_OUT("Stack Usage: %s: %d\n", __PRETTY_FUNCTION__, uxTaskGetStackHighWaterMark(NULL));
#endif
}
}/*}}}*/
