diff --git a/clock.c b/clock.c index b928f56..a6e31e6 100644 --- a/clock.c +++ b/clock.c @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "address.h" @@ -102,6 +103,7 @@ struct clock { int sde; int free_running; int freq_est_interval; + int write_phase_mode; int grand_master_capable; /* for 802.1AS only */ int utc_timescale; int utc_offset_set; @@ -1028,6 +1030,7 @@ struct clock *clock_create(enum clock_type type, struct config *config, c->config = config; c->free_running = config_get_int(config, NULL, "free_running"); c->freq_est_interval = config_get_int(config, NULL, "freq_est_interval"); + c->write_phase_mode = config_get_int(config, NULL, "write_phase_mode"); c->grand_master_capable = config_get_int(config, NULL, "gmCapable"); c->kernel_leap = config_get_int(config, NULL, "kernel_leap"); c->utc_offset = config_get_int(config, NULL, "utc_offset"); @@ -1076,6 +1079,12 @@ struct clock *clock_create(enum clock_type type, struct config *config, and return 0. Set the frequency back to make sure fadj is the actual frequency of the clock. */ clockadj_set_freq(c->clkid, fadj); + + /* Disable write phase mode if not implemented by driver */ + if (c->write_phase_mode && !phc_has_writephase(c->clkid)) { + pr_err("clock does not support write phase mode"); + return NULL; + } } c->servo = servo_create(c->config, servo, -fadj, max_adj, sw_ts); if (!c->servo) { @@ -1632,10 +1641,22 @@ int clock_switch_phc(struct clock *c, int phc_index) return 0; } +static void clock_synchronize_locked(struct clock *c, double adj) +{ + clockadj_set_freq(c->clkid, -adj); + if (c->clkid == CLOCK_REALTIME) { + sysclk_set_sync(); + } + if (c->sanity_check) { + clockcheck_set_freq(c->sanity_check, -adj); + } +} + enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin) { - double adj, weight; enum servo_state state = SERVO_UNLOCKED; + double adj, weight; + int64_t offset; c->ingress_ts = ingress; @@ -1659,19 +1680,11 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin) return clock_no_adjust(c, ingress, origin); } - adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset), - tmv_to_nanoseconds(ingress), weight, &state); + offset = tmv_to_nanoseconds(c->master_offset); + adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ingress), + weight, &state); c->servo_state = state; - if (c->stats.max_count > 1) { - clock_stats_update(&c->stats, tmv_dbl(c->master_offset), adj); - } else { - pr_info("master offset %10" PRId64 " s%d freq %+7.0f " - "path delay %9" PRId64, - tmv_to_nanoseconds(c->master_offset), state, adj, - tmv_to_nanoseconds(c->path_delay)); - } - tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c)); switch (state) { @@ -1689,16 +1702,27 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin) tsproc_reset(c->tsproc, 0); break; case SERVO_LOCKED: + clock_synchronize_locked(c, adj); + break; case SERVO_LOCKED_STABLE: - clockadj_set_freq(c->clkid, -adj); - if (c->clkid == CLOCK_REALTIME) { - sysclk_set_sync(); - } - if (c->sanity_check) { - clockcheck_set_freq(c->sanity_check, -adj); + if (c->write_phase_mode) { + clockadj_set_phase(c->clkid, -offset); + adj = 0; + } else { + clock_synchronize_locked(c, adj); } break; } + + if (c->stats.max_count > 1) { + clock_stats_update(&c->stats, tmv_dbl(c->master_offset), adj); + } else { + pr_info("master offset %10" PRId64 " s%d freq %+7.0f " + "path delay %9" PRId64, + tmv_to_nanoseconds(c->master_offset), state, adj, + tmv_to_nanoseconds(c->path_delay)); + } + return state; } diff --git a/clockadj.c b/clockadj.c index 0485d8c..b5c78cd 100644 --- a/clockadj.c +++ b/clockadj.c @@ -79,6 +79,18 @@ double clockadj_get_freq(clockid_t clkid) return f; } +void clockadj_set_phase(clockid_t clkid, long offset) +{ + struct timex tx; + memset(&tx, 0, sizeof(tx)); + + tx.modes = ADJ_OFFSET | ADJ_NANO; + tx.offset = offset; + if (clock_adjtime(clkid, &tx) < 0) { + pr_err("failed to set the clock offset: %m"); + } +} + void clockadj_step(clockid_t clkid, int64_t step) { struct timex tx; diff --git a/clockadj.h b/clockadj.h index 4ea98c1..43325c8 100644 --- a/clockadj.h +++ b/clockadj.h @@ -43,6 +43,13 @@ void clockadj_set_freq(clockid_t clkid, double freq); */ double clockadj_get_freq(clockid_t clkid); +/** + * Set clock's phase offset. + * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME. + * @param offset The phase offset in nanoseconds. + */ +void clockadj_set_phase(clockid_t clkid, long offset); + /** * Step clock's time. * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME. diff --git a/config.c b/config.c index 0cbab4c..269183c 100644 --- a/config.c +++ b/config.c @@ -328,6 +328,7 @@ struct config_item config_tab[] = { GLOB_ITEM_STR("userDescription", ""), 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), }; static struct unicast_master_table *current_uc_mtab; diff --git a/configs/default.cfg b/configs/default.cfg index 91f6aaa..8c19129 100644 --- a/configs/default.cfg +++ b/configs/default.cfg @@ -80,6 +80,7 @@ ntpshm_segment 0 msg_interval_request 0 servo_num_offset_values 10 servo_offset_threshold 0 +write_phase_mode 0 # # Transport options # diff --git a/missing.h b/missing.h index 4726803..bc708cb 100644 --- a/missing.h +++ b/missing.h @@ -24,6 +24,7 @@ #define HAVE_MISSING_H #include +#include #include #include #include @@ -75,9 +76,9 @@ enum { #define PTP_PEROUT_REQUEST2 PTP_PEROUT_REQUEST #endif -#ifndef PTP_PIN_SETFUNC +#if LINUX_VERSION_CODE < KERNEL_VERSION(5,8,0) -/* from Linux kernel version 5.4 */ +/* from upcoming Linux kernel version 5.8 */ struct compat_ptp_clock_caps { int max_adj; /* Maximum frequency adjustment in parts per billon. */ int n_alarm; /* Number of programmable alarms. */ @@ -87,11 +88,17 @@ struct compat_ptp_clock_caps { int n_pins; /* Number of input/output pins. */ /* Whether the clock supports precise system-device cross timestamps */ int cross_timestamping; - int rsv[13]; /* Reserved for future use. */ + /* Whether the clock supports adjust phase */ + int adjust_phase; + int rsv[12]; /* Reserved for future use. */ }; #define ptp_clock_caps compat_ptp_clock_caps +#endif /*LINUX_VERSION_CODE < 5.8*/ + +#ifndef PTP_PIN_SETFUNC + enum ptp_pin_function { PTP_PF_NONE, PTP_PF_EXTTS, diff --git a/phc.c b/phc.c index 14132db..37f6b9f 100644 --- a/phc.c +++ b/phc.c @@ -127,3 +127,13 @@ int phc_has_pps(clockid_t clkid) return 0; return caps.pps; } + +int phc_has_writephase(clockid_t clkid) +{ + struct ptp_clock_caps caps; + + if (phc_get_caps(clkid, &caps)) { + return 0; + } + return caps.adjust_phase; +} diff --git a/phc.h b/phc.h index 4dbc374..c48f906 100644 --- a/phc.h +++ b/phc.h @@ -77,4 +77,14 @@ int phc_pin_setfunc(clockid_t clkid, struct ptp_pin_desc *desc); */ int phc_has_pps(clockid_t clkid); +/** + * Checks whether the given PTP hardware clock device supports write phase mode. + * + * @param clkid A clock ID obtained using phc_open(). + * + * @return Zero if write phase mode is not supported by the clock, non-zero + * otherwise. + */ +int phc_has_writephase(clockid_t clkid); + #endif diff --git a/ptp4l.8 b/ptp4l.8 index 79bc9d9..24ce10d 100644 --- a/ptp4l.8 +++ b/ptp4l.8 @@ -763,6 +763,12 @@ The offset threshold used in order to transition from the SERVO_LOCKED to the SERVO_LOCKED_STABLE state. The transition occurs once the last 'servo_num_offset_values' offsets are all below the threshold value. The default value of offset_threshold is 0 (disabled). +.TP +.B write_phase_mode +This option enables using the "write phase" feature of a PTP Hardware +Clock. If supported by the device, this mode uses the hardware's +built in phase offset control instead of frequency offset control. +The default value is 0 (disabled). .SH UNICAST DISCOVERY OPTIONS