diff --git a/clock.c b/clock.c index 96be630..71c4791 100644 --- a/clock.c +++ b/clock.c @@ -24,6 +24,7 @@ #include "bmc.h" #include "clock.h" +#include "clockadj.h" #include "foreign.h" #include "mave.h" #include "missing.h" @@ -404,52 +405,6 @@ static enum servo_state clock_no_adjust(struct clock *c) return state; } -static void clock_ppb(clockid_t clkid, double ppb) -{ - struct timex tx; - memset(&tx, 0, sizeof(tx)); - tx.modes = ADJ_FREQUENCY; - tx.freq = (long) (ppb * 65.536); - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to adjust the clock: %m"); -} - -static double clock_ppb_read(clockid_t clkid) -{ - double f = 0.0; - struct timex tx; - memset(&tx, 0, sizeof(tx)); - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to read out the clock frequency adjustment: %m"); - else - f = tx.freq / 65.536; - return f; -} - -static void clock_step(clockid_t clkid, int64_t ns) -{ - struct timex tx; - int sign = 1; - if (ns < 0) { - sign = -1; - ns *= -1; - } - memset(&tx, 0, sizeof(tx)); - tx.modes = ADJ_SETOFFSET | ADJ_NANO; - tx.time.tv_sec = sign * (ns / NS_PER_SEC); - tx.time.tv_usec = sign * (ns % NS_PER_SEC); - /* - * The value of a timeval is the sum of its fields, but the - * field tv_usec must always be non-negative. - */ - if (tx.time.tv_usec < 0) { - tx.time.tv_sec -= 1; - tx.time.tv_usec += 1000000000; - } - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to step clock: %m"); -} - static void clock_update_grandmaster(struct clock *c) { struct parentDS *pds = &c->dad.pds; @@ -580,7 +535,7 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count, } if (c->clkid != CLOCK_INVALID) { - fadj = (int) clock_ppb_read(c->clkid); + fadj = (int) clockadj_get_freq(c->clkid); } c->servo = servo_create(servo, -fadj, max_adj, sw_ts); if (!c->servo) { @@ -1035,13 +990,13 @@ enum servo_state clock_synchronize(struct clock *c, case SERVO_UNLOCKED: break; case SERVO_JUMP: - clock_ppb(c->clkid, -adj); - clock_step(c->clkid, -c->master_offset); + clockadj_set_freq(c->clkid, -adj); + clockadj_step(c->clkid, -c->master_offset); c->t1 = tmv_zero(); c->t2 = tmv_zero(); break; case SERVO_LOCKED: - clock_ppb(c->clkid, -adj); + clockadj_set_freq(c->clkid, -adj); break; } return state; diff --git a/clockadj.c b/clockadj.c new file mode 100644 index 0000000..a2b55fd --- /dev/null +++ b/clockadj.c @@ -0,0 +1,72 @@ +/** + * @file clockadj.c + * @note Copyright (C) 2013 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include + +#include "clockadj.h" +#include "missing.h" +#include "print.h" + +#define NS_PER_SEC 1000000000LL + +void clockadj_set_freq(clockid_t clkid, double freq) +{ + struct timex tx; + memset(&tx, 0, sizeof(tx)); + tx.modes = ADJ_FREQUENCY; + tx.freq = (long) (freq * 65.536); + if (clock_adjtime(clkid, &tx) < 0) + pr_err("failed to adjust the clock: %m"); +} + +double clockadj_get_freq(clockid_t clkid) +{ + double f = 0.0; + struct timex tx; + memset(&tx, 0, sizeof(tx)); + if (clock_adjtime(clkid, &tx) < 0) + pr_err("failed to read out the clock frequency adjustment: %m"); + else + f = tx.freq / 65.536; + return f; +} + +void clockadj_step(clockid_t clkid, int64_t step) +{ + struct timex tx; + int sign = 1; + if (step < 0) { + sign = -1; + step *= -1; + } + memset(&tx, 0, sizeof(tx)); + tx.modes = ADJ_SETOFFSET | ADJ_NANO; + tx.time.tv_sec = sign * (step / NS_PER_SEC); + tx.time.tv_usec = sign * (step % NS_PER_SEC); + /* + * The value of a timeval is the sum of its fields, but the + * field tv_usec must always be non-negative. + */ + if (tx.time.tv_usec < 0) { + tx.time.tv_sec -= 1; + tx.time.tv_usec += 1000000000; + } + if (clock_adjtime(clkid, &tx) < 0) + pr_err("failed to step clock: %m"); +} diff --git a/clockadj.h b/clockadj.h new file mode 100644 index 0000000..b40d7a2 --- /dev/null +++ b/clockadj.h @@ -0,0 +1,47 @@ +/** + * @file clockadj.h + * @brief Wraps clock_adjtime functionality. + * @note Copyright (C) 2013 Miroslav Lichvar + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#ifndef HAVE_CLOCKADJ_H +#define HAVE_CLOCKADJ_H + +#include +#include + +/* + * Set clock's frequency offset. + * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME. + * @param freq The frequency offset in parts per billion (ppb). + */ +void clockadj_set_freq(clockid_t clkid, double freq); + +/* + * Read clock's frequency offset. + * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME. + * @return The frequency offset in parts per billion (ppb). + */ +double clockadj_get_freq(clockid_t clkid); + +/* + * Step clock's time. + * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME. + * @param step The time step in nanoseconds. + */ +void clockadj_step(clockid_t clkid, int64_t step); + +#endif diff --git a/makefile b/makefile index 671de48..e9ecb03 100644 --- a/makefile +++ b/makefile @@ -29,9 +29,9 @@ VER = -DVER=$(version) CFLAGS = -Wall $(VER) $(INC) $(DEBUG) $(FEAT_CFLAGS) $(EXTRA_CFLAGS) LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) PRG = ptp4l pmc phc2sys hwstamp_ctl -OBJ = bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \ - print.o raw.o servo.o sk.o stats.o tlv.o tmtab.o transport.o udp.o udp6.o \ - uds.o util.o version.o +OBJ = bmc.o clock.o clockadj.o config.o fsm.o ptp4l.o mave.o msg.o phc.o \ + pi.o port.o print.o raw.o servo.o sk.o stats.o tlv.o tmtab.o transport.o \ + udp.o udp6.o uds.o util.o version.o OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o SRC = $(OBJECTS:.o=.c) @@ -52,8 +52,8 @@ ptp4l: $(OBJ) pmc: msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o transport.o udp.o \ udp6.o uds.o util.o version.o -phc2sys: msg.o phc2sys.o pmc_common.o print.o pi.o servo.o raw.o sk.o stats.o \ - sysoff.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o +phc2sys: clockadj.o msg.o phc2sys.o pmc_common.o print.o pi.o servo.o raw.o \ + sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o hwstamp_ctl: hwstamp_ctl.o version.o diff --git a/phc2sys.c b/phc2sys.c index a600709..ac036dd 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -27,13 +27,13 @@ #include #include #include -#include #include #include #include #include +#include "clockadj.h" #include "ds.h" #include "fsm.h" #include "missing.h" @@ -78,52 +78,6 @@ static clockid_t clock_open(char *device) return FD_TO_CLOCKID(fd); } -static void clock_ppb(clockid_t clkid, double ppb) -{ - struct timex tx; - memset(&tx, 0, sizeof(tx)); - tx.modes = ADJ_FREQUENCY; - tx.freq = (long) (ppb * 65.536); - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to adjust the clock: %m"); -} - -static double clock_ppb_read(clockid_t clkid) -{ - double f = 0.0; - struct timex tx; - memset(&tx, 0, sizeof(tx)); - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to read out the clock frequency adjustment: %m"); - else - f = tx.freq / 65.536; - return f; -} - -static void clock_step(clockid_t clkid, int64_t ns) -{ - struct timex tx; - int sign = 1; - if (ns < 0) { - sign = -1; - ns *= -1; - } - memset(&tx, 0, sizeof(tx)); - tx.modes = ADJ_SETOFFSET | ADJ_NANO; - tx.time.tv_sec = sign * (ns / NS_PER_SEC); - tx.time.tv_usec = sign * (ns % NS_PER_SEC); - /* - * The value of a timeval is the sum of its fields, but the - * field tv_usec must always be non-negative. - */ - if (tx.time.tv_usec < 0) { - tx.time.tv_sec -= 1; - tx.time.tv_usec += 1000000000; - } - if (clock_adjtime(clkid, &tx) < 0) - pr_err("failed to step clock: %m"); -} - static int read_phc(clockid_t clkid, clockid_t sysclk, int readings, int64_t *offset, uint64_t *ts, int64_t *delay) { @@ -224,10 +178,10 @@ static void update_clock(struct clock *clock, case SERVO_UNLOCKED: break; case SERVO_JUMP: - clock_step(clock->clkid, -offset); + clockadj_step(clock->clkid, -offset); /* Fall through. */ case SERVO_LOCKED: - clock_ppb(clock->clkid, -ppb); + clockadj_set_freq(clock->clkid, -ppb); break; } @@ -669,10 +623,10 @@ int main(int argc, char *argv[]) close_pmc(&dst_clock); } - ppb = clock_ppb_read(dst_clock.clkid); + ppb = clockadj_get_freq(dst_clock.clkid); /* The reading may silently fail and return 0, reset the frequency to make sure ppb is the actual frequency of the clock. */ - clock_ppb(dst_clock.clkid, ppb); + clockadj_set_freq(dst_clock.clkid, ppb); dst_clock.servo = servo_create(CLOCK_SERVO_PI, -ppb, max_ppb, 0);