BOOL ChangeTimerQueueTimer(HANDLE TimerQueue, HANDLE Timer, ULONG DueTime, ULONG Period)
{
struct timespec CurrentTime;
WINPR_TIMER_QUEUE* timerQueue;
WINPR_TIMER_QUEUE_TIMER* timer;
if (!TimerQueue || !Timer)
return FALSE;
timespec_gettimeofday(&CurrentTime);
timerQueue = (WINPR_TIMER_QUEUE*) TimerQueue;
timer = (WINPR_TIMER_QUEUE_TIMER*) Timer;
pthread_mutex_lock(&(timerQueue->cond_mutex));
RemoveTimerQueueTimer(&(timerQueue->activeHead), timer);
RemoveTimerQueueTimer(&(timerQueue->inactiveHead), timer);
timer->DueTime = DueTime;
timer->Period = Period;
timer->next = NULL;
timespec_copy(&(timer->StartTime), &CurrentTime);
timespec_add_ms(&(timer->StartTime), DueTime);
timespec_copy(&(timer->ExpirationTime), &(timer->StartTime));
InsertTimerQueueTimer(&(timerQueue->activeHead), timer);
pthread_cond_signal(&(timerQueue->cond));
pthread_mutex_unlock(&(timerQueue->cond_mutex));
return TRUE;
}
static void* TimerQueueThread(void* arg)
{
int status;
struct timespec timeout;
WINPR_TIMER_QUEUE* timerQueue = (WINPR_TIMER_QUEUE*) arg;
while (1)
{
pthread_mutex_lock(&(timerQueue->cond_mutex));
timespec_gettimeofday(&timeout);
if (!timerQueue->activeHead)
{
timespec_add_ms(&timeout, 50);
}
else
{
if (timespec_compare(&timeout, &(timerQueue->activeHead->ExpirationTime)) < 0)
{
timespec_copy(&timeout, &(timerQueue->activeHead->ExpirationTime));
}
}
status = pthread_cond_timedwait(&(timerQueue->cond), &(timerQueue->cond_mutex), &timeout);
FireExpiredTimerQueueTimers(timerQueue);
pthread_mutex_unlock(&(timerQueue->cond_mutex));
if (timerQueue->bCancelled)
break;
}
return NULL;
}
static
bool poll_complete(const struct timespec * const now)
{
req_func = htu21d_req_temp;
timespec_add_ms(now, poll_interval_ms, &ts_next_req);
return true;
}
TEST(semaphore, sem_timedwait) {
sem_t s;
ASSERT_EQ(0, sem_init(&s, 0, 0));
timespec ts;
ASSERT_EQ(0, clock_gettime(CLOCK_REALTIME, &ts));
timespec_add_ms(ts, 100);
errno = 0;
ASSERT_EQ(-1, sem_timedwait(&s, &ts));
ASSERT_EQ(ETIMEDOUT, errno);
// A negative timeout is an error.
errno = 0;
ts.tv_nsec = -1;
ASSERT_EQ(-1, sem_timedwait(&s, &ts));
ASSERT_EQ(EINVAL, errno);
errno = 0;
ts.tv_nsec = NS_PER_S;
ASSERT_EQ(-1, sem_timedwait(&s, &ts));
ASSERT_EQ(EINVAL, errno);
errno = 0;
ts.tv_nsec = NS_PER_S - 1;
ts.tv_sec = -1;
ASSERT_EQ(-1, sem_timedwait(&s, &ts));
ASSERT_EQ(ETIMEDOUT, errno);
ASSERT_EQ(0, sem_destroy(&s));
}
static void sem_timedwait_helper(clockid_t clock,
int (*wait_function)(sem_t* __sem, const timespec* __ts)) {
sem_t s;
ASSERT_EQ(0, sem_init(&s, 0, 0));
timespec ts;
ASSERT_EQ(0, clock_gettime(clock, &ts));
timespec_add_ms(ts, 100);
errno = 0;
ASSERT_EQ(-1, wait_function(&s, &ts));
ASSERT_EQ(ETIMEDOUT, errno);
// A negative timeout is an error.
errno = 0;
ts.tv_nsec = -1;
ASSERT_EQ(-1, wait_function(&s, &ts));
ASSERT_EQ(EINVAL, errno);
errno = 0;
ts.tv_nsec = NS_PER_S;
ASSERT_EQ(-1, wait_function(&s, &ts));
ASSERT_EQ(EINVAL, errno);
errno = 0;
ts.tv_nsec = NS_PER_S - 1;
ts.tv_sec = -1;
ASSERT_EQ(-1, wait_function(&s, &ts));
ASSERT_EQ(ETIMEDOUT, errno);
ASSERT_EQ(0, sem_destroy(&s));
}
int dgp_reader_read(struct dgp_reader *dr, int fd)
{
int ret;
int off;
do {
ret = read(fd, dr->buf + dr->bytes,
sizeof(dr->buf) - dr->bytes);
} while (ret < 0 && errno == EINTR);
if (ret <= 0) {
if (ret < 0) {
if (errno == EAGAIN)
return 0;
perror("dgp_reader_read");
}
return -1;
}
dr->bytes += ret;
iv_timer_unregister(&dr->keepalive_timeout);
iv_validate_now();
dr->keepalive_timeout.expires = iv_now;
timespec_add_ms(&dr->keepalive_timeout.expires,
1000 * KEEPALIVE_TIMEOUT, 1000 * KEEPALIVE_TIMEOUT);
iv_timer_register(&dr->keepalive_timeout);
off = 0;
while (off < dr->bytes) {
int len;
struct lsa *lsa;
len = lsa_deserialise(&lsa, dr->buf + off, dr->bytes - off);
if (len < 0)
return -1;
if (len == 0) {
if (off == 0 && dr->bytes == sizeof(dr->buf))
return -1;
break;
}
if (lsa != NULL) {
if (dr->remoteid != NULL)
adj_rib_in_add_lsa(&dr->adj_rib_in, lsa);
lsa_put(lsa);
}
off += len;
}
dr->bytes -= off;
memmove(dr->buf, dr->buf + off, dr->bytes);
return 0;
}
static
bool htu21d_req_humid(const int fd, const struct timespec *now)
{
if (1 != write(fd, "\xf5", 1))
return false;
req_func = htu21d_rcv_humid;
timespec_add_ms(now, 16, &ts_next_req);
return true;
}
static
bool htu21d_req_temp(const int fd, const struct timespec *now)
{
if (1 != write(fd, "\xf3", 1))
return false;
req_func = htu21d_rcv_temp;
timespec_add_ms(now, 50, &ts_next_req);
return true;
}
void MeterS0::check_ref_for_overflow()
{
// check whether _ms_last_impulse get's too long
// and has risk for overflow (roughly once a month with 32bit unsigned long)
if (_ms_last_impulse > (1ul<<30)) {
// now we enter a race condition so there might be wrong impulse now!
timespec_add_ms(_time_last_ref, 1ul<<30 );
_ms_last_impulse -= 1ul << 30;
}
}
/*
* waitms: -1 for block until event occurs
* 0 perform non blocking check
* >0 Timeout in milliseconds.
*/
void
PhysicalMedium::interval(int waitms)
{
int rv;
struct itimerspec ts;
struct itimerspec ts_left;
/* One shot timer, armed with the time after the interval specified */
timespec_add_ms(&pimpl->curTime, waitms);
ts.it_interval.tv_sec = 0;
ts.it_interval.tv_nsec = 0;
ts.it_value = pimpl->curTime;
/* As we are using a timer any adjustments while this asbsolute timer
* is armed will also be adjusted. */
rv = timer_settime(pimpl->timer, TIMER_ABSTIME, &ts, NULL);
if (rv == -1)
throw "PhysicalMedium::interval: failed to arm timer";
do {
rv = epoll_wait(pimpl->poller.epfd, pimpl->poller.events,
EPOLL_MAX_EVENTS, waitms);
if (rv == -1) {
if (errno != EINTR)
/* Not EINTR so we do have a problem */
throw "PhysicalMedium::interval: epoll failure";
} else {
//rv is 0 or number of file descriptors to process.
if (rv) {
processPollerEvents(rv);
break;
}
}
if (timer_gettime(pimpl->timer, &ts_left) == -1)
throw "PhysicalMedium::interval: failed to get timer";
// Readjust wait ms based on time left.
waitms = (ts_left.it_value.tv_sec * 1000)
+ (ts_left.it_value.tv_nsec / 1000000);
} while(rv > 0 && waitms > 0);
}
int FireExpiredTimerQueueTimers(WINPR_TIMER_QUEUE* timerQueue)
{
struct timespec CurrentTime;
WINPR_TIMER_QUEUE_TIMER* node;
if (!timerQueue->activeHead)
return 0;
timespec_gettimeofday(&CurrentTime);
node = timerQueue->activeHead;
while (node)
{
if (timespec_compare(&CurrentTime, &(node->ExpirationTime)) >= 0)
{
node->Callback(node->Parameter, TRUE);
node->FireCount++;
timerQueue->activeHead = node->next;
node->next = NULL;
if (node->Period)
{
timespec_add_ms(&(node->ExpirationTime), node->Period);
InsertTimerQueueTimer(&(timerQueue->activeHead), node);
}
else
{
InsertTimerQueueTimer(&(timerQueue->inactiveHead), node);
}
node = timerQueue->activeHead;
}
else
{
break;
}
}
return 0;
}
static int motors_action(char action, char setting)
{
frame_t frame;
char data[2];
if (action == 'e') /* start motors */
{
data[0] = action;
data[1] = setting;
build_frame(frame, OUT_FC_MOTORS_ACTION,
(const plchar_t *)data, 2);
}
else /* stop motors */
{
data[0] = 'd';
build_frame(frame, OUT_FC_MOTORS_ACTION,
(const plchar_t *)data, 1);
}
int result;
do
{
serial_write_line(&port, frame);
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
ts = timespec_add_ms(ts, 100);
pthread_mutex_lock(&ack_mutex);
result = pthread_cond_timedwait(&ack, &ack_mutex, &ts);
pthread_mutex_unlock(&ack_mutex);
}
while (result == ETIMEDOUT);
return 0;
}
void dgp_reader_register(struct dgp_reader *dr)
{
dr->bytes = 0;
if (dr->remoteid != NULL) {
dr->adj_rib_in.myid = dr->myid;
dr->adj_rib_in.remoteid = dr->remoteid;
adj_rib_in_init(&dr->adj_rib_in);
dr->to_loc.dest = dr->rib;
rib_listener_to_loc_init(&dr->to_loc);
adj_rib_in_listener_register(&dr->adj_rib_in, &dr->to_loc.rl);
}
IV_TIMER_INIT(&dr->keepalive_timeout);
iv_validate_now();
dr->keepalive_timeout.expires = iv_now;
timespec_add_ms(&dr->keepalive_timeout.expires,
1000 * KEEPALIVE_TIMEOUT, 1000 * KEEPALIVE_TIMEOUT);
dr->keepalive_timeout.cookie = dr;
dr->keepalive_timeout.handler = dgp_reader_keepalive_timeout;
iv_timer_register(&dr->keepalive_timeout);
}
BOOL CreateTimerQueueTimer(PHANDLE phNewTimer, HANDLE TimerQueue,
WAITORTIMERCALLBACK Callback, PVOID Parameter, DWORD DueTime, DWORD Period, ULONG Flags)
{
struct timespec CurrentTime;
WINPR_TIMER_QUEUE* timerQueue;
WINPR_TIMER_QUEUE_TIMER* timer;
if (!TimerQueue)
return FALSE;
timespec_gettimeofday(&CurrentTime);
timerQueue = (WINPR_TIMER_QUEUE*) TimerQueue;
timer = (WINPR_TIMER_QUEUE_TIMER*) malloc(sizeof(WINPR_TIMER_QUEUE_TIMER));
if (!timer)
return FALSE;
WINPR_HANDLE_SET_TYPE(timer, HANDLE_TYPE_TIMER_QUEUE_TIMER);
*((UINT_PTR*) phNewTimer) = (UINT_PTR)(HANDLE) timer;
timespec_copy(&(timer->StartTime), &CurrentTime);
timespec_add_ms(&(timer->StartTime), DueTime);
timespec_copy(&(timer->ExpirationTime), &(timer->StartTime));
timer->Flags = Flags;
timer->DueTime = DueTime;
timer->Period = Period;
timer->Callback = Callback;
timer->Parameter = Parameter;
timer->timerQueue = (WINPR_TIMER_QUEUE*) TimerQueue;
timer->FireCount = 0;
timer->next = NULL;
pthread_mutex_lock(&(timerQueue->cond_mutex));
InsertTimerQueueTimer(&(timerQueue->activeHead), timer);
pthread_cond_signal(&(timerQueue->cond));
pthread_mutex_unlock(&(timerQueue->cond_mutex));
return TRUE;
}
ssize_t MeterS0::read(std::vector<Reading> &rds, size_t n) {
ssize_t ret = 0;
if (!_hwif) return 0;
if (n<4) return 0; // would be worth a debug msg!
// wait till last+1s (even if we are already later)
struct timespec req = _time_last_read;
// (or even more seconds if !send_zero
unsigned int t_imp;
unsigned int t_imp_neg;
bool is_zero = true;
do{
req.tv_sec += 1;
while (EINTR == clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &req, NULL));
// check from counter_thread the current impulses:
t_imp = _impulses;
t_imp_neg = _impulses_neg;
if (t_imp > 0 || t_imp_neg > 0 ) {
is_zero = false;
// reduce _impulses to avoid wraps. there is no race cond here as it's ok if _impulses is >0 afterwards if new impulses arrived in the meantime. That's why we don't set to 0!
_impulses -= t_imp;
_impulses_neg -= t_imp_neg;
}
} while (!_send_zero && (is_zero)); // so we are blocking is send_zero is false and no impulse coming!
// todo check thread cancellation on program termination
// we got t_imp and/or t_imp_neq between _time_last_read and req
clock_gettime(CLOCK_REALTIME, &req);
double t1;
double t2;
if (_hwif->is_blocking()) {
// if is_zero we need to correct the time here as no impulse occured!
if (is_zero) {
// we simply add the time from req-_time_last_read to _time_last_ref:
struct timespec d1s;
timespec_sub(req, _time_last_read, d1s);
timespec_add(_time_last_ref, d1s);
// this has a little racecond as well (if after existing while loop a impulse returned the ms_last_impulse might have been increased already based on old time_last_ref
}
// we use the time from last impulse
t1 = _time_last_impulse_returned.tv_sec + _time_last_impulse_returned.tv_nsec / 1e9;
struct timespec temp_ts = _time_last_ref;
timespec_add_ms(temp_ts, _ms_last_impulse);
check_ref_for_overflow();
t2 = temp_ts.tv_sec + temp_ts.tv_nsec / 1e9;
_time_last_impulse_returned = temp_ts;
_time_last_read = req;
req = _time_last_impulse_returned;
} else {
// we use the time from last read call
t1= _time_last_read.tv_sec + _time_last_read.tv_nsec / 1e9;
t2 = req.tv_sec + req.tv_nsec / 1e9;
_time_last_read = req;
}
if (t2==t1) t2+=0.000001;
if (_send_zero || t_imp > 0) {
if (!_first_impulse) {
double value = (3600000 / ((t2-t1) * _resolution)) * t_imp;
rds[ret].identifier(new StringIdentifier("Power"));
rds[ret].time(req);
rds[ret].value(value);
++ret;
}
rds[ret].identifier(new StringIdentifier("Impulse"));
rds[ret].time(req);
rds[ret].value(t_imp);
++ret;
}
if (_send_zero || t_imp_neg > 0) {
if (!_first_impulse) {
double value = (3600000 / ((t2-t1) * _resolution)) * t_imp_neg;
rds[ret].identifier(new StringIdentifier("Power_neg"));
rds[ret].time(req);
rds[ret].value(value);
++ret;
}
rds[ret].identifier(new StringIdentifier("Impulse_neg"));
rds[ret].time(req);
rds[ret].value(t_imp_neg);
++ret;
}
if (_first_impulse && ret>0)
_first_impulse = false;
print(log_finest, "Reading S0 - returning %d readings (n=%d n_neg = %d)", name().c_str(), ret, t_imp, t_imp_neg);
return ret;
}
