diff --git a/clock.c b/clock.c index 835d2c3..b841e81 100644 --- a/clock.c +++ b/clock.c @@ -74,6 +74,7 @@ struct clock_subscriber { struct clock { clockid_t clkid; struct servo *servo; + enum servo_type servo_type; struct defaultDS dds; struct dataset default_dataset; struct currentDS cur; @@ -862,6 +863,7 @@ struct clock *clock_create(int phc_index, struct interfaces_head *ifaces, return NULL; } c->servo_state = SERVO_UNLOCKED; + c->servo_type = servo; c->delay_filter = filter_create(dds->delay_filter, dds->delay_filter_length); if (!c->delay_filter) { @@ -1358,6 +1360,41 @@ UInteger16 clock_steps_removed(struct clock *c) return c->cur.stepsRemoved; } +int clock_switch_phc(struct clock *c, int phc_index) +{ + struct servo *servo; + int fadj, max_adj; + clockid_t clkid; + char phc[32]; + + snprintf(phc, 31, "/dev/ptp%d", phc_index); + clkid = phc_open(phc); + if (clkid == CLOCK_INVALID) { + pr_err("Switching PHC, failed to open %s: %m", phc); + return -1; + } + max_adj = phc_max_adj(clkid); + if (!max_adj) { + pr_err("Switching PHC, clock is not adjustable"); + phc_close(clkid); + return -1; + } + fadj = (int) clockadj_get_freq(clkid); + clockadj_set_freq(clkid, fadj); + servo = servo_create(c->servo_type, -fadj, max_adj, 0); + if (!servo) { + pr_err("Switching PHC, failed to create clock servo"); + phc_close(clkid); + return -1; + } + phc_close(c->clkid); + servo_destroy(c->servo); + c->clkid = clkid; + c->servo = servo; + c->servo_state = SERVO_UNLOCKED; + return 0; +} + enum servo_state clock_synchronize(struct clock *c, struct timespec ingress_ts, struct timestamp origin_ts, diff --git a/clock.h b/clock.h index a2b46be..4834464 100644 --- a/clock.h +++ b/clock.h @@ -204,6 +204,14 @@ int clock_slave_only(struct clock *c); */ UInteger16 clock_steps_removed(struct clock *c); +/** + * Switch to a new PTP Hardware Clock, for use with the "jbod" mode. + * @param c The clock instance. + * @param phc_index The index of the PHC device to use. + * @return Zero on success, non-zero otherwise. + */ +int clock_switch_phc(struct clock *c, int phc_index); + /** * Provide a data point to synchronize the clock. * @param c The clock instance to synchronize.