diff --git a/clock.c b/clock.c
index 0615187..6237bcf 100644
--- a/clock.c
+++ b/clock.c
@@ -19,6 +19,7 @@
#include <errno.h>
#include <time.h>
#include <linux/net_tstamp.h>
+#include <math.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
@@ -48,6 +49,8 @@
#define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer */
#define POW2_41 ((double)(1ULL << 41))
+#define OFFSETS_ARRAY_SIZE 30
+
struct interface {
STAILQ_ENTRY(interface) list;
};
@@ -114,6 +117,32 @@ struct clock {
int utc_offset;
int time_flags; /* grand master role */
int time_source; /* grand master role */
+
+ int64_t min_offset_locked;
+ int64_t max_freq_change;
+ int64_t max_offset_locked;
+ int max_offset_skipped_count;
+ int offset_skipped_count;
+ int64_t max_offset_locked_init;
+ int64_t offsets_array[OFFSETS_ARRAY_SIZE];
+ double freqs_array[OFFSETS_ARRAY_SIZE];
+ int offset_ptr;
+ int large_offset_ptr;
+ int offset_count;
+ int64_t offset_stdev;
+ int64_t offset_sigma_sq;
+ int offset_stdev_factor;
+ int64_t offset_mean; // this should be close to 0. We can raise warning if it is becoming too high.
+ int freq_ptr;
+ int freq_count;
+ double freq_stdev;
+ double freq_sigma_sq;
+ int freq_stdev_factor;
+ double freq_mean;
+ tmv_t last_correction;
+ int64_t min_offset_stddev;
+ double min_offset_freq_mean;
+
UInteger8 clock_class_threshold;
UInteger8 max_steps_removed;
enum servo_state servo_state;
@@ -145,6 +174,7 @@ static void handle_state_decision_event(struct clock *c);
static int clock_resize_pollfd(struct clock *c, int new_nports);
static void clock_remove_port(struct clock *c, struct port *p);
static void clock_stats_display(struct clock_stats *s);
+static void clock_synchronize_locked(struct clock *c, double adj);
static void remove_subscriber(struct clock_subscriber *s)
{
@@ -270,6 +300,10 @@ void clock_destroy(struct clock *c)
{
struct port *p, *tmp;
+ /* set last known mean freq on destroy */
+ if (c->min_offset_stddev != INT64_MAX)
+ clock_synchronize_locked(c, c->min_offset_freq_mean);
+
interface_destroy(c->uds_rw_if);
interface_destroy(c->uds_ro_if);
clock_flush_subscriptions(c);
@@ -1105,6 +1139,28 @@ struct clock *clock_create(enum clock_type type, struct config *config,
c->time_source = config_get_int(config, NULL, "timeSource");
c->step_window = config_get_int(config, NULL, "step_window");
+ c->min_offset_locked = config_get_int(config, NULL, "min_offset_locked");
+ c->max_freq_change = config_get_int(config, NULL, "max_freq_change");
+ c->max_offset_skipped_count = config_get_int(config, NULL, "max_offset_skipped_count");
+ c->max_offset_locked_init = config_get_int(config, NULL, "max_offset_locked_init");
+ c->offset_stdev_factor = config_get_int(config, NULL, "offset_stdev_factor");
+ c->freq_stdev_factor = config_get_int(config, NULL, "freq_stdev_factor");
+ c->offset_ptr = 0;
+ c->large_offset_ptr = 0;
+ c->offset_count = 0;
+ c->offset_stdev = 0;
+ c->offset_sigma_sq = 0;
+ c->offset_mean = 0;
+ c->freq_ptr = 0;
+ c->freq_count = 0;
+ c->freq_stdev = 0;
+ c->freq_sigma_sq = 0;
+ c->freq_mean = 0;
+ c->offset_skipped_count = 0;
+ c->last_correction = tmv_zero();
+ c->min_offset_freq_mean = 0;
+ c->min_offset_stddev = INT64_MAX;
+
if (c->free_running) {
c->clkid = CLOCK_INVALID;
if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) {
@@ -1798,11 +1854,77 @@ static void clock_synchronize_locked(struct clock *c, double adj)
}
}
+// update the sigma_sq and mean and pointer
+void init_clock_spike_filter(struct clock *c){
+ c->offset_ptr = 0;
+ c->offset_count = 0;
+ c->offset_stdev = 0;
+ c->offset_sigma_sq = 0;
+ c->offset_mean = 0;
+ c->freq_ptr = 0;
+ c->freq_count = 0;
+ c->freq_stdev = 0;
+ c->freq_sigma_sq = 0;
+ c->freq_mean = 0;
+ c->offset_skipped_count = 0;
+ pr_notice("Reset spike filter variables");
+}
+void update_clock_offset_stats(struct clock *c, int64_t offset)
+{
+ if (c->offset_count < OFFSETS_ARRAY_SIZE){
+ c->offset_mean = (c->offset_mean*c->offset_count+offset)/(c->offset_count+1);
+ c->offset_sigma_sq = c->offset_sigma_sq + pow(offset,2);
+ c->offset_stdev = sqrt(c->offset_sigma_sq/(c->offset_count+1));
+ } else{
+ c->offset_ptr = c->offset_ptr % OFFSETS_ARRAY_SIZE;
+ c->offset_mean = (c->offset_mean * OFFSETS_ARRAY_SIZE - c->offsets_array[c->offset_ptr]+offset)/OFFSETS_ARRAY_SIZE;
+ c->offset_sigma_sq = c->offset_sigma_sq - pow(c->offsets_array[c->offset_ptr],2) + pow(offset,2);
+ c->offset_stdev = sqrt(c->offset_sigma_sq/OFFSETS_ARRAY_SIZE);
+ if (c->offset_stdev > 0 && c->offset_stdev < c->min_offset_stddev)
+ c->min_offset_stddev = c->offset_stdev;
+ }
+ if (c->offset_stdev < 0) {
+ init_clock_spike_filter(c);
+ return;
+ }
+ c->offsets_array[c->offset_ptr] = offset;
+ c->offset_count+=1;
+ c->offset_ptr+=1;
+}
+
+void update_clock_freq_stats(struct clock *c, double freq)
+{
+ if (c->freq_count < OFFSETS_ARRAY_SIZE){
+ c->freq_mean = (c->freq_mean*c->freq_count+freq)/(c->freq_count+1);
+ c->freq_sigma_sq = c->freq_sigma_sq + pow(freq,2);
+ c->freq_stdev = sqrt(c->freq_sigma_sq/(c->freq_count+1) - pow(c->freq_mean,2));
+ } else{
+ c->freq_ptr = c->freq_ptr % OFFSETS_ARRAY_SIZE;
+ c->freq_mean = (c->freq_mean * OFFSETS_ARRAY_SIZE - c->freqs_array[c->freq_ptr]+freq)/OFFSETS_ARRAY_SIZE;
+ c->freq_sigma_sq = c->freq_sigma_sq - pow(c->freqs_array[c->freq_ptr],2) + pow(freq,2);
+ c->freq_stdev = sqrt(c->freq_sigma_sq/OFFSETS_ARRAY_SIZE - pow(c->freq_mean,2));
+ if (c->offset_stdev == c->min_offset_stddev) {
+ c->min_offset_freq_mean = c->freq_mean;
+ pr_notice("Best offset stddev = %ld, new mean freq = %lf", c->min_offset_stddev, c->min_offset_freq_mean);
+ }
+ }
+ c->freqs_array[c->freq_ptr] = freq;
+ c->freq_count+=1;
+ c->freq_ptr+=1;
+ c->last_correction = c->ingress_ts;
+}
+
+int64_t max_func(int64_t num1, int64_t num2)
+{
+ return (num1 > num2 ) ? num1 : num2;
+}
+
enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
{
enum servo_state state = SERVO_UNLOCKED;
double adj, weight;
- int64_t offset;
+ tmv_t master_offset;
+ int64_t offset, unsync_seconds;
if (c->step_window_counter) {
c->step_window_counter--;
@@ -1816,7 +1938,7 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
tsproc_down_ts(c->tsproc, origin, ingress);
- if (tsproc_update_offset(c->tsproc, &c->master_offset, &weight)) {
+ if (tsproc_update_offset(c->tsproc, &master_offset, &weight)) {
if (c->free_running) {
return clock_no_adjust(c, ingress, origin);
} else {
@@ -1824,6 +1946,60 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
}
}
+ offset = tmv_to_nanoseconds(master_offset);
+
+ if (c->servo_state == SERVO_LOCKED) {
+ pr_debug("mean freq: %lf", c->min_offset_freq_mean);
+ if (c->offset_count < OFFSETS_ARRAY_SIZE){
+ c->offset_skipped_count = 0;
+ // update the statistics of the clock
+ update_clock_offset_stats(c, offset);
+ } else {
+ // the last term is assuming that we have freq error RATE difference meanining that the freq is increasing max_freq_change every 1s.
+ // the middle term is assuming that at the time that we got bad ingress sync packet, we have a frequency error of (c->freq_stdev_factor*c->freq_stdev)
+ c->max_offset_locked = c->offset_stdev_factor * c->offset_stdev;
+ unsync_seconds = (tmv_to_nanoseconds(tmv_sub(c->ingress_ts, c->last_correction)) / NS_PER_SEC);
+ if (unsync_seconds > 5 || unsync_seconds < 0) {
+ pr_notice("seconds without sync: %ld", unsync_seconds);
+ }
+ c->max_offset_locked += unsync_seconds * c->freq_stdev_factor * ((int64_t) floor(c->freq_stdev)) + (c->max_freq_change/2) * pow(unsync_seconds,2);
+ // Overflow protection. Sometimes window grows too big resulting in ptp4l entering a limbo state
+ if (c->max_offset_locked < 0) {
+ pr_notice("max_offset_locked: %ld, offset_stdev_factor: %d, offset_stdev: %ld", c->max_offset_locked, c->offset_stdev_factor, c->offset_stdev);
+ pr_notice("unsync_seconds: %ld, freq_stdev_factor: %d, freq_stdev: %lf, max_freq_change: %ld", unsync_seconds, c->freq_stdev_factor, c->freq_stdev, c->max_freq_change);
+ c->servo_state = SERVO_UNLOCKED;
+ return c->servo_state;
+ }
+
+ bool is_spike = llabs(offset) > llabs(max_func(c->max_offset_locked, c->min_offset_locked));
+ if (is_spike) {
+ adj = c->min_offset_freq_mean;
+ c->master_offset = nanoseconds_to_tmv(c->max_offset_locked);
+ pr_notice("spike detected => max_offset_locked: %ld, setting offset to min_offset_freq_mean: %lf", c->max_offset_locked, adj);
+ clock_synchronize_locked(c, adj);
+ if (c->offset_skipped_count < c->max_offset_skipped_count) {
+ c->offset_skipped_count++;
+ pr_notice("skip %d/%d large offset (>%ld) %ld", c->offset_skipped_count,
+ c->max_offset_skipped_count, c->max_offset_locked, offset);
+ // we should consider changing freq to the best mean in case of spike
+ return c->servo_state;
+ } else {
+ // I am not totally sure if we should go to unlocked case or not. It may be better to just keep track of how many we missed.
+ c->servo_state = SERVO_UNLOCKED;
+ return c->servo_state;
+ }
+ } else {
+ pr_debug("NO spike detected => max_offset_locked: %ld", c->max_offset_locked);
+ c->offset_skipped_count = 0;
+ // update the statistics of the clock
+ update_clock_offset_stats(c, offset);
+ }
+ }
+ } else {
+ init_clock_spike_filter(c);
+ }
+ c->master_offset = master_offset;
+
if (clock_utc_correct(c, ingress)) {
return c->servo_state;
}
@@ -1836,7 +2012,6 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
return state;
}
- offset = tmv_to_nanoseconds(c->master_offset);
if (offset * tmv_sign(c->master_offset) > 10000) {
tsproc_dump_state(c->tsproc);
}
@@ -1863,6 +2038,7 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
break;
case SERVO_LOCKED:
clock_synchronize_locked(c, adj);
+ update_clock_freq_stats(c, adj);
break;
case SERVO_LOCKED_STABLE:
if (c->write_phase_mode) {
@@ -1871,6 +2047,7 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
} else {
clock_synchronize_locked(c, adj);
}
+ update_clock_freq_stats(c, adj);
break;
}
@@ -2025,6 +2202,10 @@ static void handle_state_decision_event(struct clock *c)
if (cid_eq(&best_id, &c->dds.clockIdentity)) {
pr_notice("selected local clock %s as best master",
cid2str(&best_id));
+ // let's set estimated mean freq while we are free running
+ if (c->min_offset_stddev != INT64_MAX) {
+ clockadj_set_freq(c->clkid, -c->min_offset_freq_mean);
+ }
} else {
pr_notice("selected best master clock %s",
cid2str(&best_id));
diff --git a/clock.h b/clock.h
index 17b2e3b..f86229b 100644
--- a/clock.h
+++ b/clock.h
@@ -361,6 +361,8 @@ struct timePropertiesDS clock_time_properties(struct clock *c);
*/
void clock_update_time_properties(struct clock *c, struct timePropertiesDS tds);
+void init_clock_spike_filter(struct clock *c);
+
/**
* Obtain a clock's description.
* @param c The clock instance.
diff --git a/config.c b/config.c
index 747a735..718d880 100644
--- a/config.c
+++ b/config.c
@@ -346,6 +346,13 @@ struct config_item config_tab[] = {
GLOB_ITEM_INT("utc_offset", CURRENT_UTC_OFFSET, 0, INT_MAX),
GLOB_ITEM_INT("verbose", 0, 0, 1),
GLOB_ITEM_INT("write_phase_mode", 0, 0, 1),
+
+ GLOB_ITEM_INT("max_freq_change", 20, 0, INT_MAX),
+ GLOB_ITEM_INT("max_offset_skipped_count", 15, 0, INT_MAX),
+ GLOB_ITEM_INT("max_offset_locked_init", 500000, 0, INT_MAX),
+ GLOB_ITEM_INT("offset_stdev_factor", 3, 0, INT_MAX),
+ GLOB_ITEM_INT("freq_stdev_factor", 3, 0, INT_MAX),
+ GLOB_ITEM_INT("min_offset_locked", 15000, 0, INT_MAX),
};
static struct unicast_master_table *current_uc_mtab;