Commit 5d5e3790 authored by Aurelio Colosimo's avatar Aurelio Colosimo

fix timer handling when timestamp is changed

parent b74e7a03
...@@ -47,16 +47,25 @@ void pp_get_tstamp(TimeInternal *t) ...@@ -47,16 +47,25 @@ void pp_get_tstamp(TimeInternal *t)
t->nanoseconds = tp.tv_nsec; t->nanoseconds = tp.tv_nsec;
} }
void pp_set_tstamp(TimeInternal *t) int32_t pp_set_tstamp(TimeInternal *t)
{ {
/* FIXME: what happens with timers? */ struct timespec tp_orig;
struct timespec tp; struct timespec tp;
if (clock_gettime(CLOCK_REALTIME, &tp_orig) < 0) {
/* FIXME diag PERROR("clock_gettime() failed, exiting."); */
exit(0);
}
tp.tv_sec = t->seconds; tp.tv_sec = t->seconds;
tp.tv_nsec = t->nanoseconds; tp.tv_nsec = t->nanoseconds;
if (clock_settime(CLOCK_REALTIME, &tp) < 0) { if (clock_settime(CLOCK_REALTIME, &tp) < 0) {
/* FIXME diag PERROR("clock_settime() failed, exiting."); */ /* FIXME diag PERROR("clock_settime() failed, exiting."); */
exit(0); exit(0);
} }
return tp.tv_sec - tp_orig.tv_sec; /* handle only sec field, since
* timer granularity is 1s */
} }
int pp_adj_freq(Integer32 adj) int pp_adj_freq(Integer32 adj)
......
...@@ -74,6 +74,15 @@ extern int posix_timer_expired(struct pp_timer *tm) ...@@ -74,6 +74,15 @@ extern int posix_timer_expired(struct pp_timer *tm)
return 0; return 0;
} }
extern void pp_timer_adjust_all(struct pp_instance *ppi, int32_t diff)
{
int i;
for (i = 0; i < PP_TIMER_ARRAY_SIZE; i++) {
ppi->timers[i] += diff;
}
}
int pp_timer_init(struct pp_instance *ppi) int pp_timer_init(struct pp_instance *ppi)
__attribute__((alias("posix_timer_init"))); __attribute__((alias("posix_timer_init")));
......
...@@ -250,6 +250,9 @@ extern int pp_timer_init(struct pp_instance *ppi); /* initializes timer common ...@@ -250,6 +250,9 @@ extern int pp_timer_init(struct pp_instance *ppi); /* initializes timer common
extern int pp_timer_start(uint32_t interval, struct pp_timer *tm); extern int pp_timer_start(uint32_t interval, struct pp_timer *tm);
extern int pp_timer_stop(struct pp_timer *tm); extern int pp_timer_stop(struct pp_timer *tm);
extern int pp_timer_expired(struct pp_timer *tm); /* returns 1 when expired */ extern int pp_timer_expired(struct pp_timer *tm); /* returns 1 when expired */
/* pp_adj_timers is called after pp_set_tstamp and must be defined for those
* platform who rely on system timestamp for timer expiration handling */
extern void pp_timer_adjust_all(struct pp_instance *ppi, int32_t diff);
/* Servo */ /* Servo */
extern void pp_init_clock(struct pp_instance *ppi); extern void pp_init_clock(struct pp_instance *ppi);
...@@ -324,7 +327,8 @@ extern void set_TimeInternal(TimeInternal *t, Integer32 s, Integer32 ns); ...@@ -324,7 +327,8 @@ extern void set_TimeInternal(TimeInternal *t, Integer32 s, Integer32 ns);
/* Get and Set system timestamp */ /* Get and Set system timestamp */
extern void pp_get_tstamp(TimeInternal *t); extern void pp_get_tstamp(TimeInternal *t);
extern void pp_set_tstamp(TimeInternal *t); extern int32_t pp_set_tstamp(TimeInternal *t);
/* Virtualization of Linux adjtimex (or BSD adjtime) system clock time /* Virtualization of Linux adjtimex (or BSD adjtime) system clock time
* adjustment. Boolean: returns 1 in case of success and 0 if failure */ * adjustment. Boolean: returns 1 in case of success and 0 if failure */
......
...@@ -259,6 +259,7 @@ void pp_update_clock(struct pp_instance *ppi) ...@@ -259,6 +259,7 @@ void pp_update_clock(struct pp_instance *ppi)
{ {
Integer32 adj; Integer32 adj;
TimeInternal time_tmp; TimeInternal time_tmp;
uint32_t tstamp_diff;
/* FIXME diag DBGV("updateClock\n");*/ /* FIXME diag DBGV("updateClock\n");*/
...@@ -290,7 +291,8 @@ void pp_update_clock(struct pp_instance *ppi) ...@@ -290,7 +291,8 @@ void pp_update_clock(struct pp_instance *ppi)
pp_get_tstamp(&time_tmp); pp_get_tstamp(&time_tmp);
sub_TimeInternal(&time_tmp, &time_tmp, sub_TimeInternal(&time_tmp, &time_tmp,
&DSCUR(ppi)->offsetFromMaster); &DSCUR(ppi)->offsetFromMaster);
pp_set_tstamp(&time_tmp); tstamp_diff = pp_set_tstamp(&time_tmp);
pp_timer_adjust_all(ppi, tstamp_diff);
pp_init_clock(ppi); pp_init_clock(ppi);
} else { } else {
adj = DSCUR(ppi)->offsetFromMaster.nanoseconds adj = DSCUR(ppi)->offsetFromMaster.nanoseconds
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment