diff --git a/config.c b/config.c index 79341b6..f12ebc5 100644 --- a/config.c +++ b/config.c @@ -223,6 +223,11 @@ static void scan_global_line(const char *s, struct config *cfg) if (df > 0.0 && df < 1.0) *cfg->pi_integral_const = df; + } else if (1 == sscanf(s, " pi_offset_const %lf", &df)) { + + if (df > 0.0) + *cfg->pi_offset_const = df; + } else if (MAC_LEN == sscanf(s, " ptp_dst_mac %hhx:%hhx:%hhx:%hhx:%hhx:%hhx", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5])) { diff --git a/config.h b/config.h index 9691478..8840b41 100644 --- a/config.h +++ b/config.h @@ -66,6 +66,8 @@ struct config { double *pi_proportional_const; double *pi_integral_const; + double *pi_offset_const; + unsigned char *ptp_dst_mac; unsigned char *p2p_dst_mac; diff --git a/pi.c b/pi.c index d003587..cf83f9f 100644 --- a/pi.c +++ b/pi.c @@ -18,6 +18,7 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include +#include #include "pi.h" #include "servo_private.h" @@ -28,9 +29,12 @@ #define SWTS_KP 0.1 #define SWTS_KI 0.001 +#define NSEC_PER_SEC 1000000000 + /* These two take their values from the configuration file. (see ptp4l.c) */ double configured_pi_kp; double configured_pi_ki; +double configured_pi_offset; struct pi_servo { struct servo servo; @@ -40,6 +44,7 @@ struct pi_servo { double maxppb; double kp; double ki; + double max_offset; int count; }; @@ -81,6 +86,19 @@ static double pi_sample(struct servo *servo, s->count = 4; break; case 4: + /* + * reset the clock servo when offset is greater than the max + * offset value. Note that the clock jump will be performed in + * step 3, so it is not necessary to have clock jump + * immediately. This allows re-calculating drift as in initial + * clock startup. + */ + if (s->max_offset && (s->max_offset < fabs(offset))) { + *state = SERVO_UNLOCKED; + s->count = 0; + break; + } + ki_term = s->ki * offset; ppb = s->kp * offset + s->drift + ki_term; if (ppb < -s->maxppb) { @@ -121,5 +139,11 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) s->ki = HWTS_KI; } + if (configured_pi_offset > 0.0) { + s->max_offset = configured_pi_offset * NSEC_PER_SEC; + } else { + s->max_offset = 0.0; + } + return &s->servo; } diff --git a/pi.h b/pi.h index 2208832..eff90aa 100644 --- a/pi.h +++ b/pi.h @@ -33,6 +33,15 @@ extern double configured_pi_kp; */ extern double configured_pi_ki; +/** + * When set to a non-zero value, this variable controls the maximum allowed + * offset before a clock jump occurs instead of the default clock-slewing + * mechanism + * + * Note that this variable is measured in seconds, and allows fractional values. + */ +extern double configured_pi_offset; + struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts); #endif diff --git a/ptp4l.c b/ptp4l.c index 7a2ef61..9a9ba9e 100644 --- a/ptp4l.c +++ b/ptp4l.c @@ -70,6 +70,8 @@ static struct config cfg_settings = { .pi_proportional_const = &configured_pi_kp, .pi_integral_const = &configured_pi_ki, + .pi_offset_const = &configured_pi_offset, + .ptp_dst_mac = ptp_dst_mac, .p2p_dst_mac = p2p_dst_mac,