linuxptp/clockadj.c

178 lines
4.7 KiB
C
Raw Normal View History

/**
* @file clockadj.c
* @note Copyright (C) 2013 Richard Cochran <richardcochran@gmail.com>
*
* 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 <math.h>
#include <string.h>
#include <unistd.h>
#include "clockadj.h"
#include "missing.h"
#include "print.h"
#define NS_PER_SEC 1000000000LL
static int realtime_leap_bit;
static long realtime_hz;
static long realtime_nominal_tick;
void clockadj_init(clockid_t clkid)
{
#ifdef _SC_CLK_TCK
if (clkid == CLOCK_REALTIME) {
/* This is USER_HZ in the kernel. */
realtime_hz = sysconf(_SC_CLK_TCK);
if (realtime_hz > 0) {
/* This is TICK_USEC in the kernel. */
realtime_nominal_tick =
(1000000 + realtime_hz / 2) / realtime_hz;
}
}
#endif
}
void clockadj_set_freq(clockid_t clkid, double freq)
{
struct timex tx;
memset(&tx, 0, sizeof(tx));
/* With system clock set also the tick length. */
if (clkid == CLOCK_REALTIME && realtime_nominal_tick) {
tx.modes |= ADJ_TICK;
tx.tick = round(freq / 1e3 / realtime_hz) + realtime_nominal_tick;
freq -= 1e3 * realtime_hz * (tx.tick - realtime_nominal_tick);
}
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;
if (clkid == CLOCK_REALTIME && realtime_nominal_tick)
f += 1e3 * realtime_hz * (tx.tick - realtime_nominal_tick);
}
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");
}
void sysclk_set_leap(int leap)
{
clockid_t clkid = CLOCK_REALTIME;
struct timex tx;
const char *m = NULL;
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_STATUS;
switch (leap) {
case -1:
tx.status = STA_DEL;
m = "clock set to delete leap second at midnight (UTC)";
break;
case 1:
tx.status = STA_INS;
m = "clock set to insert leap second at midnight (UTC)";
break;
default:
tx.status = 0;
}
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to set the clock status: %m");
else if (m)
pr_notice("%s", m);
realtime_leap_bit = tx.status;
}
void sysclk_set_tai_offset(int offset)
{
clockid_t clkid = CLOCK_REALTIME;
struct timex tx;
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_TAI;
tx.constant = offset;
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to set TAI offset: %m");
}
int sysclk_max_freq(void)
{
clockid_t clkid = CLOCK_REALTIME;
int f = 0;
struct timex tx;
memset(&tx, 0, sizeof(tx));
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to read out the clock maximum adjustment: %m");
else
f = tx.tolerance / 65.536;
if (!f)
f = 500000;
/* The kernel allows the tick length to be adjusted up to 10%. But use
it only if the overall frequency of the clock can be adjusted
continuously with the tick and freq fields (i.e. hz <= 1000). */
if (realtime_nominal_tick && 2 * f >= 1000 * realtime_hz)
f = realtime_nominal_tick / 10 * 1000 * realtime_hz;
return f;
}
void sysclk_set_sync(void)
{
clockid_t clkid = CLOCK_REALTIME;
struct timex tx;
memset(&tx, 0, sizeof(tx));
/* Clear the STA_UNSYNC flag from the status and keep the maxerror
value (which is increased automatically by 500 ppm) below 16 seconds
to avoid getting the STA_UNSYNC flag back. */
tx.modes = ADJ_STATUS | ADJ_MAXERROR;
tx.status = realtime_leap_bit;
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to set clock status and maximum error: %m");
}