static gint
default_sort (GNCPrice *price_a, GNCPrice *price_b)
{
gnc_commodity *curr_a, *curr_b;
Timespec ts_a, ts_b;
gint result;
/* Primary sort (i.e. commodity name) handled by the tree structure. */
/* secondary sort: currency */
curr_a = gnc_price_get_currency (price_a);
curr_b = gnc_price_get_currency (price_b);
result = safe_utf8_collate (gnc_commodity_get_namespace (curr_a),
gnc_commodity_get_namespace (curr_b));
if (result != 0) return result;
result = safe_utf8_collate (gnc_commodity_get_mnemonic (curr_a),
gnc_commodity_get_mnemonic (curr_b));
if (result != 0) return result;
/* tertiary sort: time */
ts_a = gnc_price_get_time (price_a);
ts_b = gnc_price_get_time (price_b);
result = timespec_cmp (&ts_a, &ts_b);
if (result)
/* Reverse the result to present the most recent quote first. */
return -result;
/* last sort: value */
return gnc_numeric_compare (gnc_price_get_value (price_a),
gnc_price_get_value (price_b));
}
/* Accumulate directory data. */
static inline void
duinfo_add (struct duinfo *a, struct duinfo const *b)
{
a->size += b->size;
if (timespec_cmp (a->tmax, b->tmax) < 0)
a->tmax = b->tmax;
}
int
compare_mtime(const void *a, const void *b)
{
const struct ls_entry *p = a, *q = b;
return (timespec_cmp(&q->st->st_mtimespec, &p->st->st_mtimespec));
}
static void
run_timers (void)
{
struct timespec now = current_timespec ();
while (atimers && timespec_cmp (atimers->expiration, now) <= 0)
{
struct atimer *t = atimers;
atimers = atimers->next;
t->fn (t);
if (t->type == ATIMER_CONTINUOUS)
{
t->expiration = timespec_add (now, t->interval);
schedule_atimer (t);
}
else
{
t->next = free_atimers;
free_atimers = t;
}
}
set_alarm ();
}
/* Accumulate directory data. */
static inline void
duinfo_add (struct duinfo *a, struct duinfo const *b)
{
uintmax_t sum = a->size + b->size;
a->size = a->size <= sum ? sum : UINTMAX_MAX;
if (timespec_cmp (a->tmax, b->tmax) < 0)
a->tmax = b->tmax;
}
gint
kvp_value_compare(const KvpValue * kva, const KvpValue * kvb)
{
if (kva == kvb) return 0;
/* nothing is always less than something */
if (!kva && kvb) return -1;
if (kva && !kvb) return 1;
if (kva->type < kvb->type) return -1;
if (kva->type > kvb->type) return 1;
switch (kva->type)
{
case KVP_TYPE_GINT64:
if (kva->value.int64 < kvb->value.int64) return -1;
if (kva->value.int64 > kvb->value.int64) return 1;
return 0;
break;
case KVP_TYPE_DOUBLE:
return double_compare(kva->value.dbl, kvb->value.dbl);
break;
case KVP_TYPE_NUMERIC:
return gnc_numeric_compare (kva->value.numeric, kvb->value.numeric);
break;
case KVP_TYPE_STRING:
return strcmp(kva->value.str, kvb->value.str);
break;
case KVP_TYPE_GUID:
return guid_compare(kva->value.guid, kvb->value.guid);
break;
case KVP_TYPE_TIMESPEC:
return timespec_cmp(&(kva->value.timespec), &(kvb->value.timespec));
break;
case KVP_TYPE_GDATE:
return g_date_compare(&(kva->value.gdate), &(kvb->value.gdate));
break;
case KVP_TYPE_BINARY:
/* I don't know that this is a good compare. Ab is bigger than Acef.
But I'm not sure that actually matters here. */
if (kva->value.binary.datasize < kvb->value.binary.datasize) return -1;
if (kva->value.binary.datasize > kvb->value.binary.datasize) return 1;
return memcmp(kva->value.binary.data,
kvb->value.binary.data,
kva->value.binary.datasize);
break;
case KVP_TYPE_GLIST:
return kvp_glist_compare(kva->value.list, kvb->value.list);
break;
case KVP_TYPE_FRAME:
return kvp_frame_compare(kva->value.frame, kvb->value.frame);
break;
default:
break;
}
PERR ("reached unreachable code.");
return FALSE;
}
static int add_to_bucket(net_timer_data_t *nd, int interval_msecs)
{
int64_t msecs;
int bucket;
int rv;
clock_gettime(CLOCK_MONOTONIC, &nd->deadline_ts);
pthread_mutex_lock(&hrt_thread_mutex);
msecs = timespec_diff_msecs(&HRT_BASE_BUCKET_TS, &nd->deadline_ts);
if (msecs < 0) {
msecs = 0;
}
msecs += interval_msecs;
timespec_add_msecs(&nd->deadline_ts, interval_msecs);
if (msecs > MAX_INT_INTERVAL_MSECS) {
/*
* HRT thread has not run in greater than MAX_INT_INTERVAL_MSECS,
* signal it to process requests and return a retry indication.
*/
rv = pthread_cond_signal(&hrt_thread_cv);
if (rv) {
glob_hrt_cond_signal_failed++;
DBG_LOG(SEVERE, MOD_HRT, "CV signal failed, rv=%d", rv);
DBG_ERR(SEVERE, "CV signal failed, rv=%d", rv);
}
pthread_mutex_unlock(&hrt_thread_mutex);
glob_hrt_invalid_interval++;
return -1; // Retry
}
bucket = MSECS_TO_BUCKET(msecs);
bucket = (HRT_BASE_BUCKET + bucket) % TIMER_BUCKETS;
NTD_LINK_TAIL(&timer_bucket[bucket], nd, active_timer_data);
TRACE_BUCKET(bucket);
if (HRT_THREAD_SLEEPING &&
(timespec_cmp(&nd->deadline_ts, &HRT_THREAD_TS) < 0)) {
/*
* HRT Thread is sleeping beyond this request's timeout, signal it
* to recompute the sleep interval.
*/
rv = pthread_cond_signal(&hrt_thread_cv);
if (rv) {
glob_hrt_cond_signal_failed++;
DBG_LOG(SEVERE, MOD_HRT, "CV(2) signal failed, rv=%d", rv);
DBG_ERR(SEVERE, "CV(2) signal failed, rv=%d", rv);
}
}
pthread_mutex_unlock(&hrt_thread_mutex);
return 0; // Success
}
int gncEntryCompare (const GncEntry *a, const GncEntry *b)
{
int compare;
if (a == b) return 0;
if (!a && b) return -1;
if (a && !b) return 1;
compare = timespec_cmp (&(a->date), &(b->date));
if (compare) return compare;
compare = timespec_cmp (&(a->date_entered), &(b->date_entered));
if (compare) return compare;
compare = safe_strcmp (a->desc, b->desc);
if (compare) return compare;
compare = safe_strcmp (a->action, b->action);
if (compare) return compare;
return qof_instance_guid_compare(a, b);
}
static void
debug_timer_callback (struct atimer *t)
{
struct timespec now = current_timespec ();
struct atimer_result *r = (struct atimer_result *) t->client_data;
int result = timespec_cmp (now, r->expected);
if (result < 0)
/* Too early. */
r->intime = 0;
else if (result >= 0)
{
#ifdef HAVE_SETITIMER
struct timespec delta = timespec_sub (now, r->expected);
/* Too late if later than expected + 0.02s. FIXME:
this should depend from system clock resolution. */
if (timespec_cmp (delta, make_timespec (0, 20000000)) > 0)
r->intime = 0;
else
#endif /* HAVE_SETITIMER */
r->intime = 1;
}
}
int
timer_settime( timer_t id,
int flags,
const struct itimerspec* spec,
struct itimerspec* ospec )
{
if (spec == NULL) {
errno = EINVAL;
return -1;
}
if ( __likely(!TIMER_ID_IS_WRAPPED(id)) ) {
return __timer_settime( id, flags, spec, ospec );
} else {
thr_timer_t* timer = thr_timer_from_id(id);
struct timespec expires, now;
if (timer == NULL) {
errno = EINVAL;
return -1;
}
thr_timer_lock(timer);
/* return current timer value if ospec isn't NULL */
if (ospec != NULL) {
timer_gettime_internal(timer, ospec );
}
/* compute next expiration time. note that if the
* new it_interval is 0, we should disarm the timer
*/
expires = spec->it_value;
if (!timespec_is_zero(&expires)) {
clock_gettime( timer->clock, &now );
if (!(flags & TIMER_ABSTIME)) {
timespec_add(&expires, &now);
} else {
if (timespec_cmp(&expires, &now) < 0)
expires = now;
}
}
timer->expires = expires;
timer->period = spec->it_interval;
thr_timer_unlock( timer );
/* signal the change to the thread */
pthread_cond_signal( &timer->cond );
}
return 0;
}
static void
test_dom_tree_to_timespec(void)
{
int i;
for (i = 0; i < 20; i++)
{
Timespec *test_spec1;
Timespec test_spec2;
xmlNodePtr test_node;
test_spec1 = get_random_timespec();
test_node = timespec_to_dom_tree("test-spec", test_spec1);
test_spec2 = dom_tree_to_timespec(test_node);
if (!dom_tree_valid_timespec(&test_spec2, (const xmlChar*)"test-spec"))
{
failure_args("dom_tree_to_timespec",
__FILE__, __LINE__, "NULL return");
printf("Node looks like:\n");
xmlElemDump(stdout, NULL, test_node);
printf("\n");
}
else if (timespec_cmp(test_spec1, &test_spec2) == 0)
{
success("dom_tree_to_timespec");
}
else
{
failure("dom_tree_to_timespec");
printf("Node looks like:\n");
xmlElemDump(stdout, NULL, test_node);
printf("\n");
printf("Secs are %" G_GUINT64_FORMAT " vs %" G_GUINT64_FORMAT " :: ",
test_spec1->tv_sec,
test_spec2.tv_sec);
printf("NSecs are %ld vs %ld\n",
test_spec1->tv_nsec,
test_spec2.tv_nsec);
}
g_free(test_spec1);
xmlFreeNode(test_node);
}
}
static void
set_alarm (void)
{
if (atimers)
{
#ifdef HAVE_SETITIMER
struct itimerval it;
#endif
struct timespec now, interval;
#ifdef HAVE_ITIMERSPEC
if (0 <= timerfd || alarm_timer_ok)
{
struct itimerspec ispec;
ispec.it_value = atimers->expiration;
ispec.it_interval.tv_sec = ispec.it_interval.tv_nsec = 0;
# ifdef HAVE_TIMERFD
if (timerfd_settime (timerfd, TFD_TIMER_ABSTIME, &ispec, 0) == 0)
{
add_timer_wait_descriptor (timerfd);
return;
}
# endif
if (alarm_timer_ok
&& timer_settime (alarm_timer, TIMER_ABSTIME, &ispec, 0) == 0)
return;
}
#endif
/* Determine interval till the next timer is ripe.
Don't set the interval to 0; this disables the timer. */
now = current_timespec ();
interval = (timespec_cmp (atimers->expiration, now) <= 0
? make_timespec (0, 1000 * 1000)
: timespec_sub (atimers->expiration, now));
#ifdef HAVE_SETITIMER
memset (&it, 0, sizeof it);
it.it_value = make_timeval (interval);
setitimer (ITIMER_REAL, &it, 0);
#else /* not HAVE_SETITIMER */
alarm (max (interval.tv_sec, 1));
#endif /* not HAVE_SETITIMER */
}
}
static void
schedule_atimer (struct atimer *t)
{
struct atimer *a = atimers, *prev = NULL;
/* Look for the first atimer that is ripe after T. */
while (a && timespec_cmp (a->expiration, t->expiration) < 0)
prev = a, a = a->next;
/* Insert T in front of the atimer found, if any. */
if (prev)
prev->next = t;
else
atimers = t;
t->next = a;
}
gint
gncOwnerLotsSortFunc (GNCLot *lotA, GNCLot *lotB)
{
GncInvoice *ia, *ib;
Timespec da, db;
ia = gncInvoiceGetInvoiceFromLot (lotA);
ib = gncInvoiceGetInvoiceFromLot (lotB);
if (ia)
da = gncInvoiceGetDateDue (ia);
else
da = xaccTransRetDatePostedTS (xaccSplitGetParent (gnc_lot_get_earliest_split (lotA)));
if (ib)
db = gncInvoiceGetDateDue (ib);
else
db = xaccTransRetDatePostedTS (xaccSplitGetParent (gnc_lot_get_earliest_split (lotB)));
return timespec_cmp (&da, &db);
}
static gint
sort_by_date (GtkTreeModel *f_model,
GtkTreeIter *f_iter_a,
GtkTreeIter *f_iter_b,
gpointer user_data)
{
GNCPrice *price_a, *price_b;
Timespec ts_a, ts_b;
gboolean result;
if (!get_prices (f_model, f_iter_a, f_iter_b, &price_a, &price_b))
return sort_ns_or_cm (f_model, f_iter_a, f_iter_b);
/* sort by time first */
ts_a = gnc_price_get_time (price_a);
ts_b = gnc_price_get_time (price_b);
result = timespec_cmp (&ts_a, &ts_b);
if (result)
/* Reverse the result to present the most recent quote first. */
return -result;
return default_sort (price_a, price_b);
}
void
fetchname (char const *at, int strip_leading, char **pname,
char **ptimestr, struct timespec *pstamp)
{
char *name;
const char *t;
char *timestr = NULL;
struct timespec stamp;
stamp.tv_sec = -1;
stamp.tv_nsec = 0;
while (ISSPACE ((unsigned char) *at))
at++;
if (debug & 128)
say ("fetchname %s %d\n", at, strip_leading);
if (*at == '"')
{
name = parse_c_string (at, &t);
if (! name)
{
if (debug & 128)
say ("ignoring malformed filename %s\n", quotearg (at));
return;
}
}
else
{
for (t = at; *t; t++)
{
if (ISSPACE ((unsigned char) *t))
{
/* Allow file names with internal spaces,
but only if a tab separates the file name from the date. */
char const *u = t;
while (*u != '\t' && ISSPACE ((unsigned char) u[1]))
u++;
if (*u != '\t' && (strchr (u + 1, pstamp ? '\t' : '\n')))
continue;
break;
}
}
name = savebuf (at, t - at + 1);
name[t - at] = 0;
}
/* If the name is "/dev/null", ignore the name and mark the file
as being nonexistent. The name "/dev/null" appears in patches
regardless of how NULL_DEVICE is spelled. */
if (strcmp (name, "/dev/null") == 0)
{
free (name);
if (pstamp)
{
pstamp->tv_sec = 0;
pstamp->tv_nsec = 0;
}
return;
}
/* Ignore the name if it doesn't have enough slashes to strip off. */
if (! strip_leading_slashes (name, strip_leading))
{
free (name);
return;
}
if (ptimestr)
{
char const *u = t + strlen (t);
if (u != t && *(u-1) == '\n')
u--;
if (u != t && *(u-1) == '\r')
u--;
timestr = savebuf (t, u - t + 1);
timestr[u - t] = 0;
}
if (*t != '\n')
{
if (! pstamp)
{
free (name);
free (timestr);
return;
}
if (set_time | set_utc)
get_date (&stamp, t, &initial_time);
else
{
/* The head says the file is nonexistent if the
timestamp is the epoch; but the listed time is
local time, not UTC, and POSIX.1 allows local
time offset anywhere in the range -25:00 <
offset < +26:00. Match any time in that range. */
const struct timespec lower = { -25L * 60 * 60 },
upper = { 26L * 60 * 60 };
if (get_date (&stamp, t, &initial_time)
&& timespec_cmp (stamp, lower) > 0
&& timespec_cmp (stamp, upper) < 0) {
stamp.tv_sec = 0;
stamp.tv_nsec = 0;
}
}
}
free (*pname);
*pname = name;
if (ptimestr)
{
free (*ptimestr);
*ptimestr = timestr;
}
if (pstamp)
*pstamp = stamp;
}
/*
* HRT thread handler
*/
static void *hrt_handler_func(void *arg)
{
UNUSED_ARGUMENT(arg);
int rv;
pthread_condattr_t cattr;
struct timespec cur_ts;
struct timespec ts;
int64_t clk_diff_msecs;
int64_t msecs;
int n;
int end_bucket;
int bucket;
if (!hrt_init_complete) {
/* Name thread */
prctl(PR_SET_NAME, "nvsd-hrt", 0, 0, 0);
/* Use CLOCK_MONOTONIC for pthread_cond_timedwait */
pthread_condattr_init(&cattr);
pthread_condattr_setclock(&cattr, CLOCK_MONOTONIC);
pthread_cond_init(&hrt_thread_cv, &cattr);
/* Setup initialization state */
pthread_mutex_lock(&hrt_thread_mutex);
HRT_BASE_BUCKET = 0;
clock_gettime(CLOCK_MONOTONIC, &cur_ts);
HRT_THREAD_TS = cur_ts;
HRT_BASE_BUCKET_TS = HRT_THREAD_TS;
HRT_THREAD_SLEEPING = 0;
hrt_init_complete = 1;
}
while (1) {
clock_gettime(CLOCK_MONOTONIC, &cur_ts);
clk_diff_msecs = timespec_diff_msecs(&HRT_BASE_BUCKET_TS, &cur_ts);
if (clk_diff_msecs < 0) {
clk_diff_msecs = 0;
}
bucket = MSECS_TO_BUCKET(clk_diff_msecs);
end_bucket = (HRT_BASE_BUCKET + bucket) % TIMER_BUCKETS;
if (hrt_thread_trace_enabled) {
DBG_LOG(MSG, MOD_HRT,
"Scan bucket=%d-%d ts=%ld %ld base_ts=%ld %ld "
"sleeped=%ld\n", HRT_BASE_BUCKET, end_bucket,
cur_ts.tv_sec, cur_ts.tv_nsec,
HRT_BASE_BUCKET_TS.tv_sec, HRT_BASE_BUCKET_TS.tv_nsec,
clk_diff_msecs);
}
n = HRT_BASE_BUCKET;
while (1) {
TRACE_BUCKET(n);
while (process_timer_bucket(n)) {
pthread_mutex_lock(&hrt_thread_mutex);
}
pthread_mutex_lock(&hrt_thread_mutex);
if (n == end_bucket) {
break;
} else {
n = (n + 1) % TIMER_BUCKETS;
}
}
/* Note if callout processing exceeds MSECS_PER_BUCKET */
clock_gettime(CLOCK_MONOTONIC, &ts);
msecs = timespec_diff_msecs(&cur_ts, &ts);
if (msecs > MSECS_PER_BUCKET) {
glob_hrt_long_callout++;
DBG_LOG(MSG, MOD_HRT,
"Callout processing time exceeded, "
"%ld msecs %d msecs limit", msecs, MSECS_PER_BUCKET);
}
/* Determine the delay time to the next bucket */
for (n = 1; n < TIMER_BUCKETS; n++) {
bucket = (end_bucket + n) % TIMER_BUCKETS;
if (timer_bucket[bucket].entries) {
break;
}
}
HRT_BASE_BUCKET = end_bucket;
HRT_BASE_BUCKET_TS = cur_ts;
if (n < TIMER_BUCKETS) {
HRT_THREAD_TS = timer_bucket[bucket].d.prev->deadline_ts;
if (timespec_cmp(&cur_ts, &HRT_THREAD_TS) >= 0) {
/* Bucket already expired, process it */
HRT_BASE_BUCKET = bucket;
glob_hrt_callout_behind++;
continue;
}
} else {
HRT_THREAD_TS = cur_ts;
timespec_add_msecs(&HRT_THREAD_TS,
(TIMER_BUCKETS * MSECS_PER_BUCKET));
}
if (hrt_thread_trace_enabled) {
int sleep_time_msecs = timespec_diff_msecs(&cur_ts, &HRT_THREAD_TS);
DBG_LOG(MSG, MOD_HRT,
"Sleep %d msecs base_bucket=%d cur_ts=%ld %ld "
"hrt_ts=%ld %ld\n", sleep_time_msecs,
HRT_BASE_BUCKET, cur_ts.tv_sec, cur_ts.tv_nsec,
HRT_THREAD_TS.tv_sec, HRT_THREAD_TS.tv_nsec);
}
HRT_THREAD_SLEEPING = 1;
rv = pthread_cond_timedwait(&hrt_thread_cv, &hrt_thread_mutex,
&HRT_THREAD_TS);
HRT_THREAD_SLEEPING = 0;
}
return NULL;
}
static void
gncEntryRecomputeValues (GncEntry *entry)
{
int denom;
/* See if either tax table changed since we last computed values */
if (entry->i_tax_table)
{
Timespec modtime = gncTaxTableLastModified (entry->i_tax_table);
if (timespec_cmp (&entry->i_taxtable_modtime, &modtime))
{
entry->values_dirty = TRUE;
entry->i_taxtable_modtime = modtime;
}
}
if (entry->b_tax_table)
{
Timespec modtime = gncTaxTableLastModified (entry->b_tax_table);
if (timespec_cmp (&entry->b_taxtable_modtime, &modtime))
{
entry->values_dirty = TRUE;
entry->b_taxtable_modtime = modtime;
}
}
if (!entry->values_dirty)
return;
/* Clear the last-computed tax values */
if (entry->i_tax_values)
{
gncAccountValueDestroy (entry->i_tax_values);
entry->i_tax_values = NULL;
}
if (entry->b_tax_values)
{
gncAccountValueDestroy (entry->b_tax_values);
entry->b_tax_values = NULL;
}
/* Determine the commodity denominator */
denom = get_entry_commodity_denom (entry);
/* Compute the invoice values */
gncEntryComputeValue (entry->quantity, entry->i_price,
(entry->i_taxable ? entry->i_tax_table : NULL),
entry->i_taxincluded,
entry->i_discount, entry->i_disc_type,
entry->i_disc_how,
denom,
&(entry->i_value), &(entry->i_disc_value),
&(entry->i_tax_values));
/* Compute the bill values */
gncEntryComputeValue (entry->quantity, entry->b_price,
(entry->b_taxable ? entry->b_tax_table : NULL),
entry->b_taxincluded,
gnc_numeric_zero(), GNC_AMT_TYPE_VALUE, GNC_DISC_PRETAX,
denom,
&(entry->b_value), NULL, &(entry->b_tax_values));
entry->i_value_rounded = gnc_numeric_convert (entry->i_value, denom,
GNC_HOW_RND_ROUND);
entry->i_disc_value_rounded = gnc_numeric_convert (entry->i_disc_value, denom,
GNC_HOW_RND_ROUND);
entry->i_tax_value = gncAccountValueTotal (entry->i_tax_values);
entry->i_tax_value_rounded = gnc_numeric_convert (entry->i_tax_value, denom,
GNC_HOW_RND_ROUND);
entry->b_value_rounded = gnc_numeric_convert (entry->b_value, denom,
GNC_HOW_RND_ROUND);
entry->b_tax_value = gncAccountValueTotal (entry->b_tax_values);
entry->b_tax_value_rounded = gnc_numeric_convert (entry->b_tax_value, denom,
GNC_HOW_RND_ROUND);
entry->values_dirty = FALSE;
}
int
xg_select (int fds_lim, fd_set *rfds, fd_set *wfds, fd_set *efds,
struct timespec const *timeout, sigset_t const *sigmask)
{
fd_set all_rfds, all_wfds;
struct timespec tmo;
struct timespec const *tmop = timeout;
GMainContext *context;
int have_wfds = wfds != NULL;
GPollFD gfds_buf[128];
GPollFD *gfds = gfds_buf;
int gfds_size = ARRAYELTS (gfds_buf);
int n_gfds, retval = 0, our_fds = 0, max_fds = fds_lim - 1;
int i, nfds, tmo_in_millisec;
bool need_to_dispatch;
USE_SAFE_ALLOCA;
context = g_main_context_default ();
if (rfds) all_rfds = *rfds;
else FD_ZERO (&all_rfds);
if (wfds) all_wfds = *wfds;
else FD_ZERO (&all_wfds);
n_gfds = g_main_context_query (context, G_PRIORITY_LOW, &tmo_in_millisec,
gfds, gfds_size);
if (gfds_size < n_gfds)
{
SAFE_NALLOCA (gfds, sizeof *gfds, n_gfds);
gfds_size = n_gfds;
n_gfds = g_main_context_query (context, G_PRIORITY_LOW, &tmo_in_millisec,
gfds, gfds_size);
}
for (i = 0; i < n_gfds; ++i)
{
if (gfds[i].events & G_IO_IN)
{
FD_SET (gfds[i].fd, &all_rfds);
if (gfds[i].fd > max_fds) max_fds = gfds[i].fd;
}
if (gfds[i].events & G_IO_OUT)
{
FD_SET (gfds[i].fd, &all_wfds);
if (gfds[i].fd > max_fds) max_fds = gfds[i].fd;
have_wfds = 1;
}
}
SAFE_FREE ();
if (tmo_in_millisec >= 0)
{
tmo = make_timespec (tmo_in_millisec / 1000,
1000 * 1000 * (tmo_in_millisec % 1000));
if (!timeout || timespec_cmp (tmo, *timeout) < 0)
tmop = &tmo;
}
fds_lim = max_fds + 1;
nfds = pselect (fds_lim, &all_rfds, have_wfds ? &all_wfds : NULL,
efds, tmop, sigmask);
if (nfds < 0)
retval = nfds;
else if (nfds > 0)
{
for (i = 0; i < fds_lim; ++i)
{
if (FD_ISSET (i, &all_rfds))
{
if (rfds && FD_ISSET (i, rfds)) ++retval;
else ++our_fds;
}
else if (rfds)
FD_CLR (i, rfds);
if (have_wfds && FD_ISSET (i, &all_wfds))
{
if (wfds && FD_ISSET (i, wfds)) ++retval;
else ++our_fds;
}
else if (wfds)
FD_CLR (i, wfds);
if (efds && FD_ISSET (i, efds))
++retval;
}
}
/* If Gtk+ is in use eventually gtk_main_iteration will be called,
unless retval is zero. */
#ifdef USE_GTK
need_to_dispatch = retval == 0;
#else
need_to_dispatch = true;
#endif
if (need_to_dispatch)
{
int pselect_errno = errno;
/* Prevent g_main_dispatch recursion, that would occur without
block_input wrapper, because event handlers call
unblock_input. Event loop recursion was causing Bug#15801. */
block_input ();
while (g_main_context_pending (context))
g_main_context_dispatch (context);
unblock_input ();
errno = pselect_errno;
}
/* To not have to recalculate timeout, return like this. */
if ((our_fds > 0 || (nfds == 0 && tmop == &tmo)) && (retval == 0))
{
retval = -1;
errno = EINTR;
}
return retval;
}
int main(int argc, char* argv[])
{
GError *error = NULL;
GOptionContext *context;
context = g_option_context_new("- rawsock listenr");
g_option_context_add_main_entries(context, entries, NULL);
if (!g_option_context_parse(context, &argc, &argv, &error))
{
printf("error: %s\n", error->message);
exit(1);
}
if (interface == NULL) {
printf("error: must specify network interface\n");
exit(2);
}
void* rs = openavbRawsockOpen(interface, TRUE, FALSE, ethertype, 0, MAX_NUM_FRAMES);
if (!rs) {
printf("error: failed to open raw socket (are you root?)\n");
exit(3);
}
hdr_info_t hdr;
U8 *pBuf, *pFrame, tmp8;
U32 offset, len;
U16 uid, i;
long nTotal = 0, nRecv[NUM_STREAMS];
for (i = 0; i < NUM_STREAMS; i++)
nRecv[i] = 0;
struct timespec now, report;
clock_gettime(CLOCK_MONOTONIC, &report);
report.tv_sec += REPORT_SECONDS;
while (bRunning) {
pBuf = openavbRawsockGetRxFrame(rs, 1000, &offset, &len);
if (pBuf) {
pFrame = pBuf + offset;
offset = openavbRawsockRxParseHdr(rs, pBuf, &hdr);
if ((int)offset < 0) {
printf("error parsing frame header");
}
else {
#ifndef UBUNTU
if (hdr.ethertype == ETHERTYPE_8021Q) {
// Oh! Need to look past the VLAN tag
U16 vlan_bits = ntohs(*(U16 *)(pFrame + offset));
hdr.vlan = TRUE;
hdr.vlan_vid = vlan_bits & 0x0FFF;
hdr.vlan_pcp = (vlan_bits >> 13) & 0x0007;
offset += 2;
hdr.ethertype = ntohs(*(U16 *)(pFrame + offset));
offset += 2;
}
#endif
if (hdr.ethertype == ETHERTYPE_AVTP) {
//dumpFrame(pFrame + offset, len - offset, &hdr);
// Look for stream data frames
// (ignore control frames, including MAAP)
tmp8 = *(pFrame + offset);
if ((tmp8 & 0x80) == 0) {
// Find the unique ID in the streamID
uid = htons(*(U16*)(pFrame + offset + 10));
if (uid < NUM_STREAMS)
nRecv[uid]++;
nTotal++;
}
}
}
openavbRawsockRelRxFrame(rs, pBuf);
}
clock_gettime(CLOCK_MONOTONIC, &now);
if (timespec_cmp(&now, &report) >= 0) {
printf("total=%ld\t", nTotal);
nTotal = 0;
for (i = 0; i < NUM_STREAMS-1; i++) {
if (nRecv[i] > 0) {
printf("%d=%ld\t", i, nRecv[i]);
nRecv[i] = 0;
}
}
printf("\n");
report.tv_sec += REPORT_SECONDS;
}
}
int
xg_select (int fds_lim, fd_set *rfds, fd_set *wfds, fd_set *efds,
struct timespec *timeout, sigset_t *sigmask)
{
fd_set all_rfds, all_wfds;
struct timespec tmo;
struct timespec *tmop = timeout;
GMainContext *context;
bool have_wfds = wfds != NULL;
GPollFD gfds_buf[128];
GPollFD *gfds = gfds_buf;
int gfds_size = ARRAYELTS (gfds_buf);
int n_gfds, retval = 0, our_fds = 0, max_fds = fds_lim - 1;
bool context_acquired = false;
int i, nfds, tmo_in_millisec, must_free = 0;
bool need_to_dispatch;
context = g_main_context_default ();
context_acquired = g_main_context_acquire (context);
/* FIXME: If we couldn't acquire the context, we just silently proceed
because this function handles more than just glib file descriptors.
Note that, as implemented, this failure is completely silent: there is
no feedback to the caller. */
if (rfds) all_rfds = *rfds;
else FD_ZERO (&all_rfds);
if (wfds) all_wfds = *wfds;
else FD_ZERO (&all_wfds);
n_gfds = (context_acquired
? g_main_context_query (context, G_PRIORITY_LOW, &tmo_in_millisec,
gfds, gfds_size)
: -1);
if (gfds_size < n_gfds)
{
/* Avoid using SAFE_NALLOCA, as that implicitly refers to the
current thread. Using xnmalloc avoids thread-switching
problems here. */
gfds = xnmalloc (n_gfds, sizeof *gfds);
must_free = 1;
gfds_size = n_gfds;
n_gfds = g_main_context_query (context, G_PRIORITY_LOW, &tmo_in_millisec,
gfds, gfds_size);
}
for (i = 0; i < n_gfds; ++i)
{
if (gfds[i].events & G_IO_IN)
{
FD_SET (gfds[i].fd, &all_rfds);
if (gfds[i].fd > max_fds) max_fds = gfds[i].fd;
}
if (gfds[i].events & G_IO_OUT)
{
FD_SET (gfds[i].fd, &all_wfds);
if (gfds[i].fd > max_fds) max_fds = gfds[i].fd;
have_wfds = true;
}
}
if (must_free)
xfree (gfds);
if (n_gfds >= 0 && tmo_in_millisec >= 0)
{
tmo = make_timespec (tmo_in_millisec / 1000,
1000 * 1000 * (tmo_in_millisec % 1000));
if (!timeout || timespec_cmp (tmo, *timeout) < 0)
tmop = &tmo;
}
fds_lim = max_fds + 1;
nfds = thread_select (pselect, fds_lim,
&all_rfds, have_wfds ? &all_wfds : NULL, efds,
tmop, sigmask);
if (nfds < 0)
retval = nfds;
else if (nfds > 0)
{
for (i = 0; i < fds_lim; ++i)
{
if (FD_ISSET (i, &all_rfds))
{
if (rfds && FD_ISSET (i, rfds)) ++retval;
else ++our_fds;
}
else if (rfds)
FD_CLR (i, rfds);
if (have_wfds && FD_ISSET (i, &all_wfds))
{
if (wfds && FD_ISSET (i, wfds)) ++retval;
else ++our_fds;
}
else if (wfds)
FD_CLR (i, wfds);
if (efds && FD_ISSET (i, efds))
++retval;
}
}
/* If Gtk+ is in use eventually gtk_main_iteration will be called,
unless retval is zero. */
#ifdef USE_GTK
need_to_dispatch = retval == 0;
#else
need_to_dispatch = true;
#endif
if (need_to_dispatch && context_acquired)
{
int pselect_errno = errno;
/* Prevent g_main_dispatch recursion, that would occur without
block_input wrapper, because event handlers call
unblock_input. Event loop recursion was causing Bug#15801. */
block_input ();
while (g_main_context_pending (context))
g_main_context_dispatch (context);
unblock_input ();
errno = pselect_errno;
}
if (context_acquired)
g_main_context_release (context);
/* To not have to recalculate timeout, return like this. */
if ((our_fds > 0 || (nfds == 0 && tmop == &tmo)) && (retval == 0))
{
retval = -1;
errno = EINTR;
}
return retval;
}
// ------------------------------------------------------------------------------
// Write Thread
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
write_thread(void)
{
//Timespec Structures
struct timespec now;
struct timespec next;
struct timespec start;
start.tv_sec = 0;
start.tv_nsec = 0;
struct timespec miss_time;
struct timespec wcet;
// signal startup
writing_status = 2;
printf("Writing Thread Running...\n");
// prepare an initial setpoint, just stay put
mavlink_set_position_target_local_ned_t sp;
sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY &
MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;
sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
sp.vx = 0.0;
sp.vy = 0.0;
sp.vz = 0.0;
sp.yaw_rate = 0.0;
// set position target
current_setpoint = sp;
// write a message and signal writing
//write_setpoint();
writing_status = true;
// Pixhawk needs to see off-board commands at minimum 2Hz,
// otherwise it will go into fail safe
long period_us = WritePeriod.tv_sec*1e6 + WritePeriod.tv_nsec/1000;
printf("Write Thread period: %ld us\n",period_us);
clock_gettime(CLOCK_REALTIME, &next);
while ( not time_to_exit )
{
clock_gettime(CLOCK_REALTIME,&now);
timespec_add_us(&next,period_us);
timespec_sub(&miss_time,&next,&now);
timespec_sub(&wcet,&now,&start);
int64_t miss_time_us = miss_time.tv_sec*1e6 + miss_time.tv_nsec/1e3;
int64_t wcet_us = wcet.tv_sec*1e6 + wcet.tv_nsec/1e3;
fprintf(logfile,"%d, %ld, %ld, %d, %ld, %d, %ld \n",start.tv_sec, start.tv_nsec, wcet_us, now.tv_sec, now.tv_nsec, next.tv_sec, next.tv_nsec);
/*
printf("\n");
printf("START : %ld us \n", start.tv_nsec/1000);
printf("NEXT : %ld us \n", next.tv_nsec/1000);
printf("END : %ld us \n", now.tv_nsec/1000);
printf("WCET : %d us \n", wcet_us);
printf("MISS : %d us \n", miss_time_us);
printf("\n");
*/
if(timespec_cmp(&next,&now) == -1)
{
fprintf(stderr,"write thread: DeadLine Miss! DT: %ld us\n",miss_time_us);
printf("wcet : %ld us \n\n",wcet_us);
continue;
}
clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&next,0);
clock_gettime(CLOCK_REALTIME,&start);
if (hil_mode)
{
//write_setpoint();
/*
pthread_mutex_lock(&mut);
if(blocked == 0)
{
blocked = 1;
pthread_cond_wait(&cond,&mut);
blocked = 0;
}
else
{
pthread_cond_signal(&cond);
}
write_hilsensors();
pthread_mutex_unlock(&mut);
*/
write_hilsensors();
}
else
{
write_hilsensors();
//write_heartbeat();
//write_raw();
//pthread_mutex_lock(&mut);
//if(blocked == 0)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
//else
// {
// pthread_cond_signal(&cond);
// }
//write_setpoint();
//write_heartbeat();
//pthread_mutex_unlock(&mut);
}
}
// signal end
writing_status = false;
return;
}
static void*
timer_thread_start( void* _arg )
{
thr_timer_t* timer = _arg;
thr_timer_lock( timer );
/* we loop until timer->done is set in timer_delete() */
while (!timer->done)
{
struct timespec expires = timer->expires;
struct timespec period = timer->period;
struct timespec now;
/* if the timer is stopped or disarmed, wait indefinitely
* for a state change from timer_settime/_delete/_start_stop
*/
if ( timer->stopped || timespec_is_zero(&expires) )
{
pthread_cond_wait( &timer->cond, &timer->mutex );
continue;
}
/* otherwise, we need to do a timed wait until either a
* state change of the timer expiration time.
*/
clock_gettime(timer->clock, &now);
if (timespec_cmp( &expires, &now ) > 0)
{
/* cool, there was no overrun, so compute the
* relative timeout as 'expires - now', then wait
*/
int ret;
struct timespec diff = expires;
timespec_sub( &diff, &now );
ret = __pthread_cond_timedwait_relative(
&timer->cond, &timer->mutex, &diff);
/* if we didn't timeout, it means that a state change
* occured, so reloop to take care of it.
*/
if (ret != ETIMEDOUT)
continue;
}
else
{
/* overrun was detected before we could wait ! */
if (!timespec_is_zero( &period ) )
{
/* for periodic timers, compute total overrun count */
do {
timespec_add( &expires, &period );
if (timer->overruns < DELAYTIMER_MAX)
timer->overruns += 1;
} while ( timespec_cmp( &expires, &now ) < 0 );
/* backtrack the last one, because we're going to
* add the same value just a bit later */
timespec_sub( &expires, &period );
}
else
{
/* for non-periodic timer, things are simple */
timer->overruns = 1;
}
}
/* if we get there, a timeout was detected.
* first reload/disarm the timer has needed
*/
if ( !timespec_is_zero(&period) ) {
timespec_add( &expires, &period );
} else {
timespec_zero( &expires );
}
timer->expires = expires;
/* now call the timer callback function. release the
* lock to allow the function to modify the timer setting
* or call timer_getoverrun().
*
* NOTE: at this point we trust the callback not to be a
* total moron and pthread_kill() the timer thread
*/
thr_timer_unlock(timer);
timer->callback( timer->value );
thr_timer_lock(timer);
/* now clear the overruns counter. it only makes sense
* within the callback */
timer->overruns = 0;
}
thr_timer_unlock( timer );
/* free the timer object now. there is no need to call
* __timer_table_get() since we're guaranteed that __timer_table
* is initialized in this thread
*/
thr_timer_table_free(__timer_table, timer);
return NULL;
}
