示例#1
0
/**
  * \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;
}
示例#2
0
文件: navdata.c 项目: Drone2/utils
/**
  * \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);
	}
示例#3
0
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;

}
示例#4
0
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);
}
示例#5
0
/**
  * \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);
}
示例#6
0
文件: daemon.c 项目: Zverik/mod_tile
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");
        }
    }
}
示例#7
0
文件: game_server.c 项目: xiaomu/yhg
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);

                }
            }
        }
    }
}
示例#8
0
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;
}
示例#10
0
/* 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;
}
示例#11
0
文件: xbh_server.c 项目: GMUCERG/xbh
/**
 * 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
    }
}/*}}}*/