void pubnub_task(void)
{
static uint32_t s_tick_prev;
if (0 == s_tick_prev) {
s_tick_prev = SYS_TMR_TickCountGet();
}
if (m_watcher.apb_size > 0) {
pubnub_t **ppbp;
uint32_t tick_now = SYS_TMR_TickCountGet();
int elapsed = elapsed_ms(s_tick_prev, tick_now);
for (ppbp = m_watcher.apb; ppbp < m_watcher.apb + m_watcher.apb_size; ++ppbp) {
pbnc_fsm(*ppbp);
}
if (elapsed > 0) {
pubnub_t *expired = pubnub_timer_list_as_time_goes_by(&m_watcher.timer_head, elapsed);
while (expired != NULL) {
pubnub_t *next = expired->next;
pbnc_stop(expired, PNR_TIMEOUT);
expired->previous = NULL;
expired->next = NULL;
expired = next;
}
s_tick_prev = tick_now;
}
}
}
void chanhop(struct timeval* tv) {
unsigned long elapsed = 0;
if (gettimeofday(tv, NULL) == -1)
die(1, "gettimeofday()");
elapsed = elapsed_ms(tv, &chaninfo.last_hop);
// need to chan hop
if (elapsed >= hopfreq) {
int c;
c = chaninfo.chan + 1;
if (c > 11)
c = 1;
set_chan(c);
elapsed = hopfreq;
}
// how much can we sleep?
else {
elapsed = hopfreq - elapsed;
}
// ok calculate sleeping time...
tv->tv_sec = elapsed/1000;
tv->tv_usec = (elapsed - tv->tv_sec*1000)*1000;
}
/* create a line in the output for the idle period */
void log_IDLE(char *ts)
{
int elapsed;
elapsed = (int) elapsed_ms(ts, idle_begin);
fprintf(outFP, "%s %-15s %5s > %-15s %5s IDLE%12d %s\n",
ts, current_src, "*", "*", "*", elapsed, idle_begin);
}
void check_seen(struct timeval* tv) {
unsigned long elapsed = 0;
struct timeval now;
int need_refresh = 0;
unsigned long min_wait = 0;
unsigned long will_wait;
will_wait = tv->tv_sec*1000+tv->tv_usec/1000;
min_wait = will_wait;
struct node_info* node = nodes;
if (gettimeofday(&now, NULL) == -1)
die(1, "gettimeofday()");
while(node) {
if (node->signal) {
elapsed = elapsed_ms(&now, &node->seen);
// node is dead...
if (elapsed >= sig_reset) {
node->signal = 0;
display_node(node);
need_refresh = 1;
}
// need to check soon possibly...
else {
unsigned long left;
left = sig_reset - elapsed;
if (left < min_wait)
left = min_wait;
}
}
node = node->next;
}
if (need_refresh)
refresh();
// need to sleep for less...
if (min_wait < will_wait) {
tv->tv_sec = min_wait/1000;
tv->tv_usec = (min_wait - tv->tv_sec*1000)*1000;
}
}
void socket_watcher_task(void *arg)
{
TickType_t xTimePrev = xTaskGetTickCount();
struct SocketWatcherData *pWatcher = (struct SocketWatcherData *)arg;
for (;;) {
ulTaskNotifyTake(pdTRUE, TICKS_TO_WAIT);
if (pdFALSE == xSemaphoreTakeRecursive(m_watcher.mutw, TICKS_TO_WAIT)) {
continue;
}
if (pWatcher->apb_size > 0) {
if (FreeRTOS_select(pWatcher->xFD_set, TICKS_TO_WAIT) != 0) {
pubnub_t **ppbp;
for (ppbp = pWatcher->apb; ppbp < pWatcher->apb + pWatcher->apb_size; ++ppbp) {
if (FreeRTOS_FD_ISSET((*ppbp)->pal.socket, pWatcher->xFD_set)) {
pbnc_fsm(*ppbp);
}
}
}
}
if (PUBNUB_TIMERS_API) {
TickType_t xTimeNow = xTaskGetTickCount();
int elapsed = elapsed_ms(xTimePrev, xTimeNow);
if (elapsed > 0) {
pubnub_t *expired = pubnub_timer_list_as_time_goes_by(&m_watcher.timer_head, elapsed);
while (expired != NULL) {
pubnub_t *next = expired->next;
pbnc_stop(expired, PNR_TIMEOUT);
expired->previous = NULL;
expired->next = NULL;
expired = next;
}
xTimePrev = xTimeNow;
}
}
xSemaphoreGiveRecursive(m_watcher.mutw);
}
}
bool
RazorAHRS::_init_razor()
{
char in;
int result;
struct timeval t0, t1, t2;
const std::string synch_token = "#SYNCH";
const std::string new_line = "\r\n";
// start time
gettimeofday(&t0, nullptr);
// request synch token to see if Razor is really present
const std::string contact_synch_id = "00";
const std::string contact_synch_request = "#s" + contact_synch_id;
const std::string contact_synch_reply = synch_token + contact_synch_id + new_line;
write(_serial_port, contact_synch_request.data(), contact_synch_request.length());
gettimeofday(&t1, nullptr);
// set non-blocking I/O
if (!_set_nonblocking_io()) return false;
/* look for tracker */
while (true)
{
// try to read one byte from the port
result = read(_serial_port, &in, 1);
// one byte read
if (result > 0)
{
if (_read_token(contact_synch_reply, in))
break;
}
// no data available
else if (result == 0)
usleep(1000); // sleep 1ms
// error?
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (1).");
}
// check timeout
gettimeofday(&t2, nullptr);
if (elapsed_ms(t1, t2) > 200)
{
// 200ms elapsed since last request and no answer -> request synch again
// (this happens when DTR is connected and Razor resets on connect)
write(_serial_port, contact_synch_request.data(), contact_synch_request.length());
t1 = t2;
}
if (elapsed_ms(t0, t2) > _connect_timeout_ms)
// timeout -> tracker not present
throw std::runtime_error("Can not init: tracker does not answer.");
}
/* configure tracker */
// set correct binary output mode, enable continuous streaming, disable errors and
// request synch token. So we're good, no matter what state the tracker
// currently is in.
const std::string config_synch_id = "01";
const std::string config_synch_reply = synch_token + config_synch_id + new_line;
std::string config = "#o1#oe0#s" + config_synch_id;
if (_mode == YAW_PITCH_ROLL) config = "#ob" + config;
else if (_mode == ACC_MAG_GYR_RAW) config = "#osrb" + config;
else if (_mode == ACC_MAG_GYR_CALIBRATED) config = "#oscb" + config;
else throw std::runtime_error("Can not init: unknown 'mode' parameter.");
write(_serial_port, config.data(), config.length());
// set blocking I/O
// (actually semi-blocking, because VTIME is set)
if (!_set_blocking_io()) return false;
while (true)
{
// try to read one byte from the port
result = read(_serial_port, &in, 1);
// one byte read
if (result > 0)
{
if (_read_token(config_synch_reply, in))
break; // alrighty
}
// error?
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (2).");
}
}
// we keep using blocking I/O
//if (_set_blocking_io() == -1)
// return false;
return true;
}
void main (int argc, char* argv[])
{
int i;
char input_name[256] = "";
char output_name[256] = "";
long idle_limit = 2000; /* default threshold for idleness in millisec. */
long elapsed;
char parse_line[500];
char discard[50];
char *cursor;
char *vp;
/* Parse the command line */
i = 1;
while (i < argc) {
if (strcmp (argv[i], "-r") == 0) {
if (++i >= argc) Usage (argv[0]);
strcpy (input_name, argv[i]);
}
else if (strcmp (argv[i], "-w") == 0) {
if (++i >= argc) Usage (argv[0]);
strcpy (output_name, argv[i]);
}
else if (strcmp (argv[i], "-I") == 0) {
if (++i >= argc) Usage (argv[0]);
idle_limit = (long)atoi(argv[i]);
}
else
Usage (argv[0]);
i++;
}
/* Open files */
if (strcmp(output_name, "") == 0)
outFP = stdout;
else
{
strcat(output_name, ".activity");
if ((outFP = fopen (output_name, "w")) == NULL) {
fprintf (stderr, "error opening %s\n", output_name);
exit (-1);
}
}
if (strcmp(input_name, "") == 0)
dumpFP = stdin;
else
{
if ((dumpFP = fopen (input_name, "r")) == NULL) {
fprintf (stderr, "error opening %s\n", input_name);
exit (-1);
}
}
/* Read each record in the input file. Look for a change in the
source IP address (which indicates a new client). If a new
client, log the end of an idle period (if any) for the old
client and initialize the connection table for the new client.
If a record for the current client has been read, classify the
type of event it represent and process it to update the client
and connection state.
*/
while (!feof (dumpFP)) {
/* Get and parse line of data */
if (fgets (new_line, sizeof(new_line), dumpFP) == NULL)
break;
/* get first line pieces */
sscanf (new_line, "%s %s %s %s %s %s %s",
&ts, &sh, &sp, >, &dh, &dp, &fl);
/* if an ERR line, just show it */
if (strcmp(fl, "ERR:") == 0)
{
error_line(new_line);
continue;
}
/* now get variable part starting with the ":" considering that */
/* interpretation of the remaining fields depends on the flag value */
/* This is necessary to find the ending timestamp for FIN, RST, and
TRM events.
*/
strcpy(parse_line, new_line);
cursor = parse_line;
vp = (char *)strsep(&cursor, ":" );
if ((cursor == (char *)NULL) ||
(vp == (char *)NULL))
{
error_line(new_line);
continue;
}
/* Classify the event type by looking at the flag field from input
records */
if ((strcmp(fl, "REQ") == 0) ||
(strcmp(fl, "REQ-") == 0))
event_type = REQ;
else
{
if ((strcmp(fl, "RSP") == 0) ||
(strcmp(fl, "RSP-") == 0))
event_type = RSP;
else
{
if ((strcmp(fl, "FIN") == 0) ||
(strcmp(fl, "TRM") == 0) ||
(strcmp(fl, "RST") == 0))
{
/* need the ending timestamp from these record types */
sscanf(cursor, "%s %s", &discard, &earliest_end);
event_type = END;
}
else
{
if (strcmp(fl, "SYN") == 0)
event_type = SYN;
else
{
if (strcmp(fl, "ACT-REQ") == 0)
event_type = ACT_REQ;
else
if (strcmp(fl, "ACT-RSP") == 0)
event_type = ACT_RSP;
}
}
}
}
/* now use data from new trace record to update status */
/* first check to see if this is the same client host */
if (strcmp(current_src, sh) != 0)
{
if (client_state == IDLE)
log_IDLE(last_client_ts);
ClearConnections();
client_state = PENDING_ACTIVE;
strcpy(current_src, sh);
}
/* update the connection status for this client's connection */
set_connection(sp, dh, dp, event_type);
/* The main processing for idle periods is done by maintaining a state
variable (client_status) for the client and looking for specific input
record types at different values of the state variable. The
values of client_state and their implications are:
PENDING_ACTIVE - A new client is started and remains PENDING_ACTIVE
until an activity indication such as ACT-REQ,
ACT-RSP, or REQ is seen in which case it enters
the ACTIVE state. If there is an initial response,
PENDING_IDLE is entered.
ACTIVE - At least one request is outstanding and the state
can only change if there is a response completion
or connection termination.
PENDING_IDLE - There are no requests outstanding but the idle
period threshold has not elapsed since it entered
the PENDING_IDLE state.
IDLE - No outstanding requests for a period greater than
the idle threshold. The IDLE (and PENDING_IDLE)
states are exited on activity indication such as
ACT-REQ, ACT-RSP, or REQ
*/
switch (client_state)
{
case PENDING_ACTIVE:
switch (event_type)
{
case SYN:
break;
case ACT_REQ:
case ACT_RSP:
client_state = ACTIVE;
break;
case REQ:
client_state = ACTIVE;
log_REQ();
break;
case RSP:
client_state = PENDING_IDLE;
strcpy(idle_begin, ts);
log_RSP();
break;
case END:
break;
}
break;
case ACTIVE:
switch (event_type)
{
case SYN:
case ACT_REQ:
case ACT_RSP:
break;
case REQ:
log_REQ();
break;
case RSP:
log_RSP();
if (ConnectionsActive() == 0) /* Any active connections?*/
{
client_state = PENDING_IDLE;
strcpy(idle_begin, ts);
}
break;
case END:
if (ConnectionsActive() == 0) /* Any active connections?*/
{
client_state = PENDING_IDLE;
strcpy(idle_begin, earliest_end);
}
break;
}
break;
case PENDING_IDLE:
/* must start checking time, if > n seconds elapse since
entering PENDING_IDLE state, enter IDLE state */
elapsed = elapsed_ms(ts, idle_begin);
if (elapsed < idle_limit)
{
switch (event_type)
{
case SYN:
case END:
break;
case ACT_REQ:
case ACT_RSP:
client_state = ACTIVE;
break;
case REQ:
client_state = ACTIVE;
log_REQ();
break;
case RSP:
log_RSP();
break;
}
break; /* ends case PENDING_IDLE */
}
else /* it has crossed the idle threshold */
client_state = IDLE;
/* NOTE: drop through to IDLE to handle the current event */
case IDLE:
switch (event_type)
{
case SYN:
case END:
break;
case ACT_REQ:
case ACT_RSP:
client_state = ACTIVE;
log_IDLE(ts);
break;
case REQ:
client_state = ACTIVE;
log_IDLE(ts);
log_REQ();
break;
case RSP:
log_RSP();
break;
break; /* ends case PENDING_IDLE */
}
break;
default:
break;
} /* end switch */
strcpy(last_client_ts, ts);
} /* end while (!feof ....) */
close (dumpFP);
close (outFP);
}
void socket_watcher_thread(void *arg)
{
FILETIME prev_time;
GetSystemTimeAsFileTime(&prev_time);
for (;;) {
const DWORD ms = 100;
EnterCriticalSection(&m_watcher.queue_lock);
if (m_watcher.queue_head != m_watcher.queue_tail) {
pubnub_t *pbp = m_watcher.queue_apb[m_watcher.queue_tail++];
LeaveCriticalSection(&m_watcher.queue_lock);
if (pbp != NULL) {
pubnub_mutex_lock(pbp->monitor);
pbnc_fsm(pbp);
pubnub_mutex_unlock(pbp->monitor);
}
EnterCriticalSection(&m_watcher.queue_lock);
if (m_watcher.queue_tail == m_watcher.queue_size) {
m_watcher.queue_tail = 0;
}
}
LeaveCriticalSection(&m_watcher.queue_lock);
EnterCriticalSection(&m_watcher.mutw);
if (0 == m_watcher.apoll_size) {
LeaveCriticalSection(&m_watcher.mutw);
continue;
}
{
int rslt = WSAPoll(m_watcher.apoll, m_watcher.apoll_size, ms);
if (SOCKET_ERROR == rslt) {
/* error? what to do about it? */
PUBNUB_LOG_WARNING("poll size = %d, error = %d\n", m_watcher.apoll_size, WSAGetLastError());
}
else if (rslt > 0) {
size_t i;
size_t apoll_size = m_watcher.apoll_size;
for (i = 0; i < apoll_size; ++i) {
if (m_watcher.apoll[i].revents & (POLLIN | POLLOUT)) {
pubnub_t *pbp = m_watcher.apb[i];
pubnub_mutex_lock(pbp->monitor);
pbnc_fsm(pbp);
if (apoll_size == m_watcher.apoll_size) {
if (m_watcher.apoll[i].events == POLLOUT) {
if ((pbp->state == PBS_WAIT_DNS_RCV) ||
(pbp->state >= PBS_RX_HTTP_VER)) {
m_watcher.apoll[i].events = POLLIN;
}
}
else {
if ((pbp->state > PBS_WAIT_DNS_RCV) &&
(pbp->state < PBS_RX_HTTP_VER)) {
m_watcher.apoll[i].events = POLLOUT;
}
}
}
else {
PUBNUB_ASSERT_OPT(apoll_size == m_watcher.apoll_size + 1);
apoll_size = m_watcher.apoll_size;
--i;
}
pubnub_mutex_unlock(pbp->monitor);
}
}
}
}
if (PUBNUB_TIMERS_API) {
FILETIME current_time;
int elapsed;
GetSystemTimeAsFileTime(¤t_time);
elapsed = elapsed_ms(prev_time, current_time);
if (elapsed > 0) {
pubnub_t *expired = pubnub_timer_list_as_time_goes_by(&m_watcher.timer_head, elapsed);
while (expired != NULL) {
pubnub_t *next = expired->next;
pubnub_mutex_lock(expired->monitor);
pbnc_stop(expired, PNR_TIMEOUT);
pubnub_mutex_unlock(expired->monitor);
expired->next = NULL;
expired->previous = NULL;
expired = next;
}
prev_time = current_time;
}
}
LeaveCriticalSection(&m_watcher.mutw);
}
}
int main(int argc, char *argv[]) {
/* The shell process itself ignores SIGINT. */
struct sigaction action;
action.sa_handler = SIG_IGN;
action.sa_flags = 0;
sigemptyset(&action.sa_mask);
if (sigaction(SIGINT, &action, NULL) == -1) {
perror("sigaction");
return EXIT_FAILURE;
}
while (1) {
// Read the next command.
command_t *command = read_command();
// Check for finished background processes.
int status;
pid_t pid;
while ((pid = waitpid(-1, &status, WNOHANG)) > 0) {
printf("Background process %d finished\n", pid);
}
if (!command)
continue; // Ignore empty commands.
if (match(command->argv[0], "exit")) {
// Handle built-in exit command.
if (handle_exit(command)) {
free_command(command);
break;
}
} else if (match(command->argv[0], "cd")) {
// Handle built-in cd command.
handle_cd(command);
} else {
// Fork a child process.
pid = fork();
if (pid == -1) {
perror("fork");
} else if (pid == 0) {
// Execute command.
execvp(command->argv[0], command->argv);
perror(command->argv[0]);
exit(EXIT_FAILURE);
} else {
if (command->type == FOREGROUND) {
printf("Spawned foreground process pid: %d\n", pid);
struct timeval t0, t1;
// Wait for foreground process.
gettimeofday(&t0, NULL);
waitpid(pid, &status, 0);
gettimeofday(&t1, NULL);
printf("Foreground process %d terminated\n", pid);
printf("wallclock time: %.3f msec\n", elapsed_ms(&t0, &t1));
} else {
printf("Spawned background process pid: %d\n", pid);
}
}
}
free_command(command);
}
clear_history(); // Clear GNU readline history.
return EXIT_SUCCESS;
}
