Miroslav Lichvar
2013-10-09 13:30:46 UTC
I was thinking about adding some code to the servo that would check if
the synchronized clock is advancing as expected by comparing it to the
system monotonic clock. It would allow us to see when something else
is touching the clock or the PHC/driver is broken.
We would probably need to move the clock_settime()/clock_adjtime()
calls to the servo first to allow the servo to make these assumptions,
but I was thinking something like this. What do you think?
diff --git a/servo.c b/servo.c
index 45c5832..f5daa9a 100644
--- a/servo.c
+++ b/servo.c
@@ -17,16 +17,26 @@
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <string.h>
+#include <time.h>
#include "pi.h"
+#include "print.h"
#include "servo_private.h"
struct servo *servo_create(enum servo_type type, int fadj, int max_ppb, int sw_ts)
{
+ struct servo *servo;
+
if (type == CLOCK_SERVO_PI) {
- return pi_servo_create(fadj, max_ppb, sw_ts);
- }
- return NULL;
+ servo = pi_servo_create(fadj, max_ppb, sw_ts);
+ } else
+ return NULL;
+
+ servo->last_mono_ts = 0;
+ servo->last_local_ts = 0;
+ servo->last_fadj = fadj;
+
+ return servo;
}
void servo_destroy(struct servo *servo)
@@ -39,7 +49,42 @@ double servo_sample(struct servo *servo,
uint64_t local_ts,
enum servo_state *state)
{
- return servo->sample(servo, offset, local_ts, state);
+ double fadj;
+ struct timespec now;
+ uint64_t mono_ts;
+
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ mono_ts = now.tv_sec * 1000000000 + now.tv_nsec;
+
+ if (servo->last_local_ts) {
+ if (servo->last_local_ts > local_ts) {
+ pr_warning("unexpected backward clock step!");
+ } else if ((mono_ts - servo->last_mono_ts) *
+ (1.1 - servo->last_fadj / 1e9) <
+ (local_ts - servo->last_local_ts)) {
+ pr_warning("clock running faster than expected!");
+ } else if ((mono_ts - servo->last_mono_ts) *
+ (0.9 + servo->last_fadj / 1e9) >
+ (local_ts - servo->last_local_ts)) {
+ pr_warning("clock running slower than expected!");
+ }
+ }
+
+ fadj = servo->sample(servo, offset, local_ts, state);
+
+ servo->last_mono_ts = mono_ts;
+ servo->last_local_ts = local_ts;
+ servo->last_fadj = fadj;
+
+ switch (*state) {
+ case SERVO_JUMP:
+ servo->last_local_ts -= offset;
+ break;
+ default:
+ break;
+ }
+
+ return fadj;
}
void servo_sync_interval(struct servo *servo, double interval)
diff --git a/servo_private.h b/servo_private.h
index 98c7db2..e6d3ae1 100644
--- a/servo_private.h
+++ b/servo_private.h
@@ -22,6 +22,9 @@
#include "contain.h"
struct servo {
+ uint64_t last_mono_ts;
+ uint64_t last_local_ts;
+ double last_fadj;
void (*destroy)(struct servo *servo);
the synchronized clock is advancing as expected by comparing it to the
system monotonic clock. It would allow us to see when something else
is touching the clock or the PHC/driver is broken.
We would probably need to move the clock_settime()/clock_adjtime()
calls to the servo first to allow the servo to make these assumptions,
but I was thinking something like this. What do you think?
diff --git a/servo.c b/servo.c
index 45c5832..f5daa9a 100644
--- a/servo.c
+++ b/servo.c
@@ -17,16 +17,26 @@
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <string.h>
+#include <time.h>
#include "pi.h"
+#include "print.h"
#include "servo_private.h"
struct servo *servo_create(enum servo_type type, int fadj, int max_ppb, int sw_ts)
{
+ struct servo *servo;
+
if (type == CLOCK_SERVO_PI) {
- return pi_servo_create(fadj, max_ppb, sw_ts);
- }
- return NULL;
+ servo = pi_servo_create(fadj, max_ppb, sw_ts);
+ } else
+ return NULL;
+
+ servo->last_mono_ts = 0;
+ servo->last_local_ts = 0;
+ servo->last_fadj = fadj;
+
+ return servo;
}
void servo_destroy(struct servo *servo)
@@ -39,7 +49,42 @@ double servo_sample(struct servo *servo,
uint64_t local_ts,
enum servo_state *state)
{
- return servo->sample(servo, offset, local_ts, state);
+ double fadj;
+ struct timespec now;
+ uint64_t mono_ts;
+
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ mono_ts = now.tv_sec * 1000000000 + now.tv_nsec;
+
+ if (servo->last_local_ts) {
+ if (servo->last_local_ts > local_ts) {
+ pr_warning("unexpected backward clock step!");
+ } else if ((mono_ts - servo->last_mono_ts) *
+ (1.1 - servo->last_fadj / 1e9) <
+ (local_ts - servo->last_local_ts)) {
+ pr_warning("clock running faster than expected!");
+ } else if ((mono_ts - servo->last_mono_ts) *
+ (0.9 + servo->last_fadj / 1e9) >
+ (local_ts - servo->last_local_ts)) {
+ pr_warning("clock running slower than expected!");
+ }
+ }
+
+ fadj = servo->sample(servo, offset, local_ts, state);
+
+ servo->last_mono_ts = mono_ts;
+ servo->last_local_ts = local_ts;
+ servo->last_fadj = fadj;
+
+ switch (*state) {
+ case SERVO_JUMP:
+ servo->last_local_ts -= offset;
+ break;
+ default:
+ break;
+ }
+
+ return fadj;
}
void servo_sync_interval(struct servo *servo, double interval)
diff --git a/servo_private.h b/servo_private.h
index 98c7db2..e6d3ae1 100644
--- a/servo_private.h
+++ b/servo_private.h
@@ -22,6 +22,9 @@
#include "contain.h"
struct servo {
+ uint64_t last_mono_ts;
+ uint64_t last_local_ts;
+ double last_fadj;
void (*destroy)(struct servo *servo);
--
Miroslav Lichvar
Miroslav Lichvar