static int
process_connection(int fd, struct xmpp *xmpp)
{
struct timeval tv;
fd_set fds;
int max_fd, ret = 0;
fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) | O_NONBLOCK);
while (1) {
FD_ZERO(&fds);
FD_SET(fd, &fds);
FD_SET(0, &fds);
max_fd = fd;
tv.tv_sec = keep_alive_ms / 1000;
tv.tv_usec = (keep_alive_ms % 1000) * 1000;
ret = select(max_fd + 1, &fds, 0, 0, (keep_alive_ms > 0) ? &tv : 0);
if (ret < 0)
break;
if (ret > 0) {
if (FD_ISSET(fd, &fds))
if (process_server_input(fd, xmpp))
break;
if (FD_ISSET(0, &fds))
if (process_input(0, xmpp))
break;
} else if ((!use_tls || in_tls) && io_send(1, " ", &fd) < 1)
break;
}
return 0;
}
int do_ioloop(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
unsigned int fpga;
unsigned int size;
unsigned int rate = 0;
if (argc < 3)
return CMD_RET_USAGE;
/*
* FPGA is specified since argc > 2
*/
fpga = simple_strtoul(argv[1], NULL, 10);
/*
* packet size is specified since argc > 2
*/
size = simple_strtoul(argv[2], NULL, 10);
/*
* If another parameter, it is the test rate in packets per second.
*/
if (argc > 3)
rate = simple_strtoul(argv[3], NULL, 10);
/* enable receive path */
FPGA_SET_REG(fpga, ep.rx_tx_control, CTRL_PROC_RECEIVE_ENABLE);
/* set device address to dummy 1*/
FPGA_SET_REG(fpga, ep.device_address, 1);
rx_ctr = 0; tx_ctr = 0; err_ctr = 0;
while (1) {
u16 top_int;
u16 rx_tx_status;
FPGA_GET_REG(fpga, top_interrupt, &top_int);
FPGA_GET_REG(fpga, ep.rx_tx_status, &rx_tx_status);
io_check_status(fpga, rx_tx_status, false);
if (top_int & IRQ_CPU_TRANSMITBUFFER_FREE_STATUS)
io_send(fpga, size);
if (top_int & IRQ_CPU_RECEIVE_DATA_AVAILABLE_STATUS)
io_receive(fpga);
if (rate) {
if (ctrlc())
break;
udelay(1000000 / rate);
if (!(tx_ctr % rate))
printf("d %lld, tx %llu, rx %llu, err %llu\n",
tx_ctr - rx_ctr, tx_ctr, rx_ctr,
err_ctr);
}
}
return 0;
}
static void xboard_send(xboard_t * xboard, const char format[], ...) {
va_list arg_list;
char string[StringSize];
ASSERT(xboard!=NULL);
ASSERT(format!=NULL);
// format
va_start(arg_list,format);
vsprintf(string,format,arg_list);
va_end(arg_list);
// send
io_send(xboard->io,"%s",string);
}
void engine_send(engine_t * engine, const char format[], ...) {
va_list arg_list;
char string[StringSize];
ASSERT(engine_is_ok(engine));
ASSERT(format!=NULL);
// format
va_start(arg_list,format);
vsprintf(string,format,arg_list);
va_end(arg_list);
// send
io_send(engine->io,"%s",string);
}
int bt_uhid_send(struct bt_uhid *uhid, const struct uhid_event *ev)
{
ssize_t len;
struct iovec iov;
if (!uhid->io)
return -ENOTCONN;
iov.iov_base = (void *) ev;
iov.iov_len = sizeof(*ev);
len = io_send(uhid->io, &iov, 1);
if (len < 0)
return -errno;
/* uHID kernel driver does not handle partial writes */
return len != sizeof(*ev) ? -EIO : 0;
}
static int
process_connection(int fd, struct xmpp *xmpp)
{
int res, max_fd;
struct contact *u, *next;
fd_set fds;
struct timeval tv;
add_contact(0, "");
fcntl(fd, F_SETFL, fcntl(fd, F_GETFL) | O_NONBLOCK);
while (1) {
FD_ZERO(&fds);
FD_SET(fd, &fds);
max_fd = fd;
for (u = contacts; u && is_ready; u = u->next)
if (u->fd >= 0) {
FD_SET(u->fd, &fds);
if (u->fd > max_fd)
max_fd = u->fd;
}
tv.tv_sec = keep_alive_ms / 1000;
tv.tv_usec = (keep_alive_ms % 1000) * 1000;
res = select(max_fd + 1, &fds, 0, 0, (keep_alive_ms > 0) ? &tv : 0);
if (res < 0)
break;
if (res > 0) {
if (FD_ISSET(fd, &fds) && process_server_input(fd, xmpp))
break;
for (u = contacts; u; u = next) {
next = u->next;
if (FD_ISSET(u->fd, &fds))
handle_contact_input(xmpp, u);
}
} else if (io_send(1, " ", &fd) < 1)
break;
}
return 0;
}
static void xboard_send(xboard_t * xboard, const char format[], ...) {
va_list arg_list;
char string[StringSize];
ASSERT(xboard!=NULL);
ASSERT(format!=NULL);
// format
va_start(arg_list,format);
vsprintf(string,format,arg_list);
va_end(arg_list);
#ifndef _WIN32
// send
io_send(xboard->io,"%s",string);
#else
puts(string);
fflush(stdout);
my_log("Adapter->Xboard: %s\n",string);
#endif
}
void pipex_writeln(pipex_t *pipex, const char *string){
io_send(pipex->io,"%s",string);
}
static int tcp_server_client_send(object_t parent)
{
return io_send(parent);
}
static bool can_write_data(struct io *io, void *user_data)
{
struct bt_att *att = user_data;
struct att_send_op *op;
struct timeout_data *timeout;
ssize_t ret;
struct iovec iov;
op = pick_next_send_op(att);
if (!op)
return false;
iov.iov_base = op->pdu;
iov.iov_len = op->len;
ret = io_send(io, &iov, 1);
if (ret < 0) {
util_debug(att->debug_callback, att->debug_data,
"write failed: %s", strerror(-ret));
if (op->callback)
op->callback(BT_ATT_OP_ERROR_RSP, NULL, 0,
op->user_data);
destroy_att_send_op(op);
return true;
}
util_debug(att->debug_callback, att->debug_data,
"ATT op 0x%02x", op->opcode);
util_hexdump('<', op->pdu, ret, att->debug_callback, att->debug_data);
/* Based on the operation type, set either the pending request or the
* pending indication. If it came from the write queue, then there is
* no need to keep it around.
*/
switch (op->type) {
case ATT_OP_TYPE_REQ:
att->pending_req = op;
break;
case ATT_OP_TYPE_IND:
att->pending_ind = op;
break;
case ATT_OP_TYPE_RSP:
/* Set in_req to false to indicate that no request is pending */
att->in_req = false;
/* Fall through to the next case */
case ATT_OP_TYPE_CMD:
case ATT_OP_TYPE_NOT:
case ATT_OP_TYPE_CONF:
case ATT_OP_TYPE_UNKNOWN:
default:
destroy_att_send_op(op);
return true;
}
timeout = new0(struct timeout_data, 1);
if (!timeout)
return true;
timeout->att = att;
timeout->id = op->id;
op->timeout_id = timeout_add(ATT_TIMEOUT_INTERVAL, timeout_cb,
timeout, free);
/* Return true as there may be more operations ready to write. */
return true;
}
int exec_line(int argc, char *argv[])
{
struct alias *al;
int i;
if(!argc) return 0;
ui_print("do action: ");
for(i = 0; i < argc; i++)
ui_print("%s ", argv[i]);
ui_print("\n");
if(strcmp(argv[0], "load") == 0) {
ui_animation(1);
} else if(strcmp(argv[0], "unload") == 0) {
ui_animation(3);
} else if(strcmp(argv[0], "poweron") == 0) {
power_state = 1;
} else if(strcmp(argv[0], "poweroff") == 0) {
power_state = 1;
} else if(strcmp(argv[0], "putbin") == 0) {
for(i = 0;;) {
al = get_alias("bin", i++);
if(al) ui_bin_update(i, al->arg);
else break;
}
al = get_alias("timeout", 0);
if(al) ui_bin_update(i, al->arg);
} else if(strncmp(argv[0], "send", 4) == 0) {
if(strcmp(argv[0], "send_txt") == 0) {
io_send("\002");
ui_print("--> <STX>\n");
}
for(i = 1; i < argc; i++) {
const char *s = argv[i];
if(strcmp(s, "power_state") == 0)
s = power_state? "1": "NULL";
al = get_alias(s, 0);
ui_print("--> %s\n", al? al->str: s);
io_send(al? al->str: s);
}
if(strcmp(argv[0], "send_txt") == 0) {
io_send("\003");
ui_print("--> <ETX>\n");
}
} else if(strcmp(argv[0], "wait") == 0) {
for(i = 1; i < argc; i++)
wait_queue_add_tail(argv[i]);
for(;!wait_queue_empty(); ui_animation(2));
} else if(strcmp(argv[0], "wait_timeout") == 0) {
struct timeval a, b, d;
int t;
al = get_alias("timeout", 0);
t = al? al->num: TIMEOUT;
for(i = 1; i < argc; i++)
wait_queue_add_tail(argv[i]);
gettimeofday(&a, NULL);
for(;!wait_queue_empty(); ui_animation(2)) {
gettimeofday(&b, NULL);
timersub(&b, &a, &d);
if(d.tv_sec > t) {
ui_print("timeout for waiting %s\n", wait_queue_head());
al->arg++;
return -1;
}
}
} else if(strcmp(argv[0], "handshake") == 0) {
char word[SZ];
ui_print("--> %s\n", argv[1]);
io_send(argv[1]);
while(io_get_word(word) <= 0);
ui_print("<-- %s\n", word);
if(strcmp(word, argv[2]) != 0)
return -1;
}
return 0;
}
void
PX4IO::task_main()
{
ASSERT(_fd == -1);
log("ready");
/* open the serial port */
_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
if (_fd < 0) {
debug("failed to open serial port for IO: %d", errno);
_task = -1;
_exit(errno);
}
/* protocol stream */
_io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this);
perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors);
/* poll descriptor(s) */
struct pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], 1, 100);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
/* if we timed out waiting, we should send an update */
if (ret == 0)
_send_needed = true;
/* if we have new data from IO, go handle it */
if ((ret > 0) && (fds[0].revents & POLLIN))
io_recv();
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
