linuxptp/phc_ctl.c

/*
 * @file phc_ctl.c
 * @brief Utility program to directly control and debug a PHC device.
 * @note Copyright (C) 2014 Jacob Keller <jacob.keller@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 <errno.h>
#include <fcntl.h>
#include <float.h>
#include <signal.h>
#include <inttypes.h>
#include <limits.h>
#include <net/if.h>
#include <poll.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/queue.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <math.h>

#include <linux/pps.h>
#include <linux/ptp_clock.h>

#include "clockadj.h"
#include "missing.h"
#include "phc.h"
#include "print.h"
#include "sk.h"
#include "sysoff.h"
#include "util.h"
#include "version.h"

#define NSEC2SEC 1000000000.0

/* trap the alarm signal so that pause() will wake up on receipt */
static void handle_alarm(int s)
{
	return;
}

static void double_to_timespec(double d, struct timespec *ts)
{
	double fraction, whole;

	fraction = modf(d, &whole);

	/* cast the whole value to a time_t to store as seconds */
	ts->tv_sec = (time_t)whole;
	/* tv_nsec is a long, so we multiply the nanoseconds per second double
	 * value by our fractional component. This results in a correct
	 * timespec from the double representing seconds.
	 */
	ts->tv_nsec = (long)(NSEC2SEC * fraction);
}

static int install_handler(int signum, void(*handler)(int))
{
	struct sigaction action;
	sigset_t mask;

	/* Unblock the signal */
	sigemptyset(&mask);
	sigaddset(&mask, signum);
	sigprocmask(SIG_UNBLOCK, &mask, NULL);

	/* Install the signal handler */
	action.sa_handler = handler;
	action.sa_flags = 0;
	sigemptyset(&action.sa_mask);
	sigaction(signum, &action, NULL);

	return 0;
}

static int64_t calculate_offset(struct timespec *ts1,
				      struct timespec *rt,
				      struct timespec *ts2)
{
	int64_t interval;
	int64_t offset;

#define NSEC_PER_SEC 1000000000ULL
	/* calculate interval between clock realtime */
	interval = (ts2->tv_sec - ts1->tv_sec) * NSEC_PER_SEC;
	interval += ts2->tv_nsec - ts1->tv_nsec;

	/* assume PHC read occured half way between CLOCK_REALTIME reads */

	offset = (rt->tv_sec - ts1->tv_sec) * NSEC_PER_SEC;
	offset += (rt->tv_nsec - ts1->tv_nsec) - (interval / 2);

	return offset;
}

static clockid_t clock_open(char *device)
{
	struct sk_ts_info ts_info;
	char phc_device[19];
	int clkid;

	/* check if device is CLOCK_REALTIME */
	if (!strcasecmp(device, "CLOCK_REALTIME"))
		return CLOCK_REALTIME;

	/* check if device is valid phc device */
	clkid = phc_open(device);
	if (clkid != CLOCK_INVALID)
		return clkid;

	/* check if device is a valid ethernet device */
	if (sk_get_ts_info(device, &ts_info) || !ts_info.valid) {
		pr_err("unknown clock %s: %m", device);
		return CLOCK_INVALID;
	}

	if (ts_info.phc_index < 0) {
		pr_err("interface %s does not have a PHC", device);
		return CLOCK_INVALID;
	}

	sprintf(phc_device, "/dev/ptp%d", ts_info.phc_index);
	clkid = phc_open(phc_device);
	if (clkid == CLOCK_INVALID)
		pr_err("cannot open %s for %s: %m", phc_device, device);
	return clkid;
}

static void usage(const char *progname)
{
	fprintf(stderr,
		"\n"
		"usage: %s [options] <device> -- [command]\n\n"
		" device         ethernet or ptp clock device"
		"\n"
		" options\n"
		" -l [num]       set the logging level to 'num'\n"
		" -q             do not print messages to the syslog\n"
		" -Q             do not print messages to stdout\n"
		" -v             prints the software version and exits\n"
		" -h             prints this message and exits\n"
		"\n"
		" commands\n"
		" specify commands with arguments. Can specify multiple\n"
		" commands to be executed in order. Seconds are read as\n"
		" double precision floating point values.\n"
		"  set  [seconds]  set PHC time (defaults to time on CLOCK_REALTIME)\n"
		"  get             get PHC time\n"
		"  adj  <seconds>  adjust PHC time by offset\n"
		"  freq [ppb]      adjust PHC frequency (default returns current offset)\n"
		"  cmp             compare PHC offset to CLOCK_REALTIME\n"
		"  caps            display device capabilities (default if no command given)\n"
		"  wait <seconds>  pause between commands\n"
		"\n",
		progname);
}

typedef int (*cmd_func_t)(clockid_t, int, char *[]);
struct cmd_t {
	const char *name;
	const cmd_func_t function;
};

static cmd_func_t get_command_function(const char *name);
static inline int name_is_a_command(const char *name);

static int do_set(clockid_t clkid, int cmdc, char *cmdv[])
{
	struct timespec ts;
	double time_arg = 0;
	int args_to_eat = 0;

	enum parser_result r;

	memset(&ts, 0, sizeof(ts));

	/* if we have no more arguments, or the next argument is the ";"
	 * separator, then we run set as default parameter mode */
	if (cmdc < 1 || name_is_a_command(cmdv[0])) {
		clock_gettime(CLOCK_REALTIME, &ts);

		/* since we aren't using the options, we can simply ensure
		 * that we don't eat any arguments
		 */
		args_to_eat = 0;
	} else {
		/* parse the double */
		r = get_ranged_double(cmdv[0], &time_arg, 0.0, DBL_MAX);
		switch (r) {
		case PARSED_OK:
			break;
		case MALFORMED:
			pr_err("set: '%s' is not a valid double", cmdv[0]);
			return -2;
		case OUT_OF_RANGE:
			pr_err("set: '%s' is out of range", cmdv[0]);
			return -2;
		default:
			pr_err("set: couldn't process '%s'", cmdv[0]);
			return -2;
		}

		double_to_timespec(time_arg, &ts);

		/* in order for processing to work, we need to ensure the
		 * run_cmds loop eats the optional set argument
		 */
		args_to_eat = 1;
	}

	if (clock_settime(clkid, &ts)) {
		pr_err("set: failed to set clock time: %s",
			strerror(errno));
		return -1;
	} else {
		pr_notice("set clock time to %ld.%09ld or %s",
			ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec));
	}

	return args_to_eat;
}

static int do_get(clockid_t clkid, int cmdc, char *cmdv[])
{
	struct timespec ts;

	memset(&ts, 0, sizeof(ts));
	if (clock_gettime(clkid, &ts)) {
		pr_err("get: failed to get clock time: %s",
			strerror(errno));

		return -1;
	} else {
		pr_notice("clock time is %ld.%09lu or %s",
			ts.tv_sec, ts.tv_nsec, ctime(&ts.tv_sec));
	}

	/* get operation does not require any arguments */
	return 0;
}

static int do_adj(clockid_t clkid, int cmdc, char *cmdv[])
{
	double time_arg;
	int64_t nsecs;
	enum parser_result r;

	if (cmdc < 1 || name_is_a_command(cmdv[0])) {
		pr_err("adj: missing required time argument");
		return -2;
	}

	/* parse the double time offset argument */
	r = get_ranged_double(cmdv[0], &time_arg, -DBL_MAX, DBL_MAX);
	switch (r) {
	case PARSED_OK:
		break;
	case MALFORMED:
		pr_err("adj: '%s' is not a valid double", cmdv[0]);
		return -2;
	case OUT_OF_RANGE:
		pr_err("adj: '%s' is out of range.", cmdv[0]);
		return -2;
	default:
		pr_err("adj: couldn't process '%s'", cmdv[0]);
		return -2;
	}

	nsecs = (int64_t)(NSEC2SEC * time_arg);

	clockadj_init(clkid);
	clockadj_step(clkid, nsecs);

	pr_notice("adjusted clock by %lf seconds", time_arg);

	/* adjustment always consumes one argument */
	return 1;
}

static int do_freq(clockid_t clkid, int cmdc, char *cmdv[])
{
	double ppb;
	enum parser_result r;

	clockadj_init(clkid);

	if (cmdc < 1 || name_is_a_command(cmdv[0])) {
		ppb = clockadj_get_freq(clkid);
		pr_err("clock frequency offset is %lfppb", ppb);

		/* no argument was used */
		return 0;
	}

	/* parse the double ppb argument */
	r = get_ranged_double(cmdv[0], &ppb, -NSEC2SEC, NSEC2SEC);
	switch (r) {
	case PARSED_OK:
		break;
	case MALFORMED:
		pr_err("freq: '%s' is not a valid double", cmdv[0]);
		return -2;
	case OUT_OF_RANGE:
		pr_err("freq: '%s' is out of range.", cmdv[0]);
		return -2;
	default:
		pr_err("freq: couldn't process '%s'", cmdv[0]);
		return -2;
	}

	clockadj_set_freq(clkid, ppb);
	pr_err("adjusted clock frequency offset to %lfppb", ppb);

	/* consumed one argument to determine the frequency adjustment value */
	return 1;
}

static int do_caps(clockid_t clkid, int cmdc, char *cmdv[])
{
	struct ptp_clock_caps caps;

	if (clkid == CLOCK_REALTIME) {
		pr_warning("CLOCK_REALTIME is not a PHC device.");
		return 0;
	}

	if (ioctl(CLOCKID_TO_FD(clkid), PTP_CLOCK_GETCAPS, &caps)) {
		pr_err("get capabilities failed: %s",
			strerror(errno));
		return -1;
	}

	pr_notice("\n"
		"capabilities:\n"
		"  %d maximum frequency adjustment (ppb)\n"
		"  %d programable alarms\n"
		"  %d external time stamp channels\n"
		"  %d programmable periodic signals\n"
		"  %s pulse per second support",
		caps.max_adj,
		caps.n_alarm,
		caps.n_ext_ts,
		caps.n_per_out,
		caps.pps ? "has" : "doesn't have");
	return 0;
}

static int do_cmp(clockid_t clkid, int cmdc, char *cmdv[])
{
	struct timespec ts, rta, rtb;
	int64_t sys_offset, delay = 0, offset;
	uint64_t sys_ts;

	if (SYSOFF_SUPPORTED ==
	    sysoff_measure(CLOCKID_TO_FD(clkid),
			   9, &sys_offset, &sys_ts, &delay)) {
		pr_notice( "offset from CLOCK_REALTIME is %"PRId64"ns\n",
			sys_offset);
		return 0;
	}

	memset(&ts, 0, sizeof(ts));
	memset(&ts, 0, sizeof(rta));
	memset(&ts, 0, sizeof(rtb));
	if (clock_gettime(CLOCK_REALTIME, &rta) ||
	    clock_gettime(clkid, &ts) ||
	    clock_gettime(CLOCK_REALTIME, &rtb)) {
		pr_err("cmp: failed clock reads: %s\n",
			strerror(errno));
		return -1;
	}

	offset = calculate_offset(&rta, &ts, &rtb);
	pr_notice( "offset from CLOCK_REALTIME is approximately %"PRId64"ns\n",
		offset);

	return 0;
}

static int do_wait(clockid_t clkid, int cmdc, char *cmdv[])
{
	double time_arg;
	struct timespec ts;
	struct itimerval timer;
	enum parser_result r;

	if (cmdc < 1 || name_is_a_command(cmdv[0])) {
		pr_err("wait: requires sleep duration argument\n");
		return -2;
	}

	memset(&timer, 0, sizeof(timer));

	/* parse the double time offset argument */
	r = get_ranged_double(cmdv[0], &time_arg, 0.0, DBL_MAX);
	switch (r) {
	case PARSED_OK:
		break;
	case MALFORMED:
		pr_err("wait: '%s' is not a valid double", cmdv[0]);
		return -2;
	case OUT_OF_RANGE:
		pr_err("wait: '%s' is out of range.", cmdv[0]);
		return -2;
	default:
		pr_err("wait: couldn't process '%s'", cmdv[0]);
		return -2;
	}

	double_to_timespec(time_arg, &ts);
	timer.it_value.tv_sec = ts.tv_sec;
	timer.it_value.tv_usec = ts.tv_nsec / 1000;
	setitimer(ITIMER_REAL, &timer, NULL);
	pause();

	/* the SIGALRM is already trapped during initialization, so we will
	 * wake up here once the alarm is handled.
	 */
	pr_notice( "process slept for %lf seconds\n", time_arg);

	return 1;
}

static const struct cmd_t all_commands[] = {
	{ "set", &do_set },
	{ "get", &do_get },
	{ "adj", &do_adj },
	{ "freq", &do_freq },
	{ "cmp", &do_cmp },
	{ "caps", &do_caps },
	{ "wait", &do_wait },
	{ 0, 0 }
};

static cmd_func_t get_command_function(const char *name)
{
	int i;
	cmd_func_t cmd = NULL;

	for (i = 0; all_commands[i].name != NULL; i++) {
		if (!strncmp(name,
			     all_commands[i].name,
			     strlen(all_commands[i].name)))
			cmd = all_commands[i].function;
	}

	return cmd;
}

static inline int name_is_a_command(const char *name)
{
	return get_command_function(name) != NULL;
}

static int run_cmds(clockid_t clkid, int cmdc, char *cmdv[])
{
	int i = 0, result = 0;
	cmd_func_t action = NULL;

	while (i < cmdc) {
		char *arg = cmdv[i];

		/* increment now to remove the command argument */
		i++;

		action = get_command_function(arg);
		if (action)
			result = action(clkid, cmdc - i, &cmdv[i]);
		else
			pr_err("unknown command %s.", arg);

		/* result is how many arguments were used up by the command,
		 * not including the ";". We will increment the loop counter
		 * to avoid processing the arguments as commands.
		 */
		if (result < 0)
			return result;
		else
			i += result;
	}

	return 0;
}

int main(int argc, char *argv[])
{
	const char *progname;
	char **cmdv, *default_cmdv[] = { "caps" };
	int c, result, cmdc;
	int print_level = LOG_INFO, verbose = 1, use_syslog = 1;
	clockid_t clkid;

	install_handler(SIGALRM, handle_alarm);

	/* Process the command line arguments. */
	progname = strrchr(argv[0], '/');
	progname = progname ? 1+progname : argv[0];
	while (EOF != (c = getopt(argc, argv,
				  "l:qQvh"))) {
		switch (c) {
		case 'l':
			if (get_arg_val_i(c, optarg, &print_level,
					  PRINT_LEVEL_MIN, PRINT_LEVEL_MAX))
				return -1;
			break;
		case 'q':
			use_syslog = 0;
			break;
		case 'Q':
			verbose = 0;
			break;
		case 'v':
			version_show(stdout);
			return 0;
		case 'h':
			usage(progname);
			return 0;
		default:
			usage(progname);
			return -1;
		}
	}

	print_set_progname(progname);
	print_set_verbose(verbose);
	print_set_syslog(use_syslog);
	print_set_level(print_level);

	if ((argc - optind) < 1) {
		usage(progname);
		return -1;
	}

	if ((argc - optind) == 1) {
		cmdv = default_cmdv;
		cmdc = 1;
	} else {
		cmdv = &argv[optind+1];
		cmdc = argc - optind - 1;
	}

	clkid = clock_open(argv[optind]);
	if (clkid == CLOCK_INVALID)
		return -1;

	/* pass the remaining arguments to the run_cmds loop */
	result = run_cmds(clkid, cmdc, cmdv);
	if (result < -1) {
		/* show usage when command fails */
		usage(progname);
		return result;
	}

	return 0;
}