/**

 * @file clock.c

 * @note Copyright (C) 2011 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 <errno.h>

#include <time.h>

#include <linux/net_tstamp.h>

#include <poll.h>

#include <stdlib.h>

#include <string.h>

#include <sys/queue.h>

#include "address.h"

#include "bmc.h"

#include "clock.h"

#include "clockadj.h"

#include "clockcheck.h"

#include "foreign.h"

#include "filter.h"

#include "missing.h"

#include "msg.h"

#include "phc.h"

#include "port.h"

#include "servo.h"

#include "stats.h"

#include "print.h"

#include "rtnl.h"

#include "tlv.h"

#include "tsproc.h"

#include "uds.h"

#include "util.h"

#define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer */

#define POW2_41 ((double)(1ULL << 41))

struct port {

	LIST_ENTRY(port) list;

};

struct freq_estimator {

	tmv_t origin1;

	tmv_t ingress1;

	unsigned int max_count;

	unsigned int count;

};

struct clock_stats {

	struct stats *offset;

	struct stats *freq;

	struct stats *delay;

	unsigned int max_count;

};

struct clock_subscriber {

	LIST_ENTRY(clock_subscriber) list;

	uint8_t events[EVENT_BITMASK_CNT];

	struct PortIdentity targetPortIdentity;

	struct address addr;

	UInteger16 sequenceId;

	time_t expiration;

};

struct clock {

	enum clock_type type;

	struct config *config;

	clockid_t clkid;

	struct servo *servo;

	enum servo_type servo_type;

	int (*dscmp)(struct dataset *a, struct dataset *b);

	struct defaultDS dds;

	struct dataset default_dataset;

	struct currentDS cur;

	struct parent_ds dad;

	struct timePropertiesDS tds;

	struct ClockIdentity ptl[PATH_TRACE_MAX];

	struct foreign_clock *best;

	struct ClockIdentity best_id;

	LIST_HEAD(ports_head, port) ports;

	struct port *uds_port;

	struct pollfd *pollfd;

	int pollfd_valid;

	int nports; /* does not include the UDS port */

	int last_port_number;

	int sde;

	int free_running;

	int freq_est_interval;

	int grand_master_capable; /* for 802.1AS only */

	int utc_timescale;

	int utc_offset_set;

	int leap_set;

	int kernel_leap;

	int utc_offset;

	int time_flags;  /* grand master role */

	int time_source; /* grand master role */

	enum servo_state servo_state;

	enum timestamp_type timestamping;

	tmv_t master_offset;

	tmv_t path_delay;

	tmv_t ingress_ts;

	tmv_t initial_delay;

	struct tsproc *tsproc;

	struct freq_estimator fest;

	struct time_status_np status;

	double master_local_rr; /* maintained when free_running */

	double nrr;

	struct clock_description desc;

	struct clock_stats stats;

	int stats_interval;

	struct clockcheck *sanity_check;

	struct interface uds_interface;

	LIST_HEAD(clock_subscribers_head, clock_subscriber) subscribers;

};

struct clock the_clock;

static void handle_state_decision_event(struct clock *c);

static int clock_resize_pollfd(struct clock *c, int new_nports);

static void clock_remove_port(struct clock *c, struct port *p);

static void remove_subscriber(struct clock_subscriber *s)

{

	LIST_REMOVE(s, list);

	free(s);

}

static void clock_update_subscription(struct clock *c, struct ptp_message *req,

				      uint8_t *bitmask, uint16_t duration)

{

	struct clock_subscriber *s;

	int i, remove = 1;

	struct timespec now;

	for (i = 0; i < EVENT_BITMASK_CNT; i++) {

		if (bitmask[i]) {

			remove = 0;

			break;

		}

	}

	LIST_FOREACH(s, &c->subscribers, list) {

		if (pid_eq(&s->targetPortIdentity,

			   &req->header.sourcePortIdentity)) {

			/* Found, update the transport address and event

			 * mask. */

			if (!remove) {

				s->addr = req->address;

				memcpy(s->events, bitmask, EVENT_BITMASK_CNT);

				clock_gettime(CLOCK_MONOTONIC, &now;;

				s->expiration = now.tv_sec + duration;

			} else {

				remove_subscriber(s);

			}

			return;

		}

	}

	if (remove)

		return;

	/* Not present yet, add the subscriber. */

	s = malloc(sizeof(*s));

	if (!s) {

		pr_err("failed to allocate memory for a subscriber");

		return;

	}

	s->targetPortIdentity = req->header.sourcePortIdentity;

	s->addr = req->address;

	memcpy(s->events, bitmask, EVENT_BITMASK_CNT);

	clock_gettime(CLOCK_MONOTONIC, &now;;

	s->expiration = now.tv_sec + duration;

	s->sequenceId = 0;

	LIST_INSERT_HEAD(&c->subscribers, s, list);

}

static void clock_get_subscription(struct clock *c, struct ptp_message *req,

				   uint8_t *bitmask, uint16_t *duration)

{

	struct clock_subscriber *s;

	struct timespec now;

	LIST_FOREACH(s, &c->subscribers, list) {

		if (pid_eq(&s->targetPortIdentity,

			   &req->header.sourcePortIdentity)) {

			memcpy(bitmask, s->events, EVENT_BITMASK_CNT);

			clock_gettime(CLOCK_MONOTONIC, &now;;

			if (s->expiration < now.tv_sec)

				*duration = 0;

			else

				*duration = s->expiration - now.tv_sec;

			return;

		}

	}

	/* A client without entry means the client has no subscriptions. */

	memset(bitmask, 0, EVENT_BITMASK_CNT);

	*duration = 0;

}

static void clock_flush_subscriptions(struct clock *c)

{

	struct clock_subscriber *s, *tmp;

	LIST_FOREACH_SAFE(s, &c->subscribers, list, tmp) {

		remove_subscriber(s);

	}

}

static void clock_prune_subscriptions(struct clock *c)

{

	struct clock_subscriber *s, *tmp;

	struct timespec now;

	clock_gettime(CLOCK_MONOTONIC, &now;;

	LIST_FOREACH_SAFE(s, &c->subscribers, list, tmp) {

		if (s->expiration <= now.tv_sec) {

			pr_info("subscriber %s timed out",

				pid2str(&s->targetPortIdentity));

			remove_subscriber(s);

		}

	}

}

void clock_send_notification(struct clock *c, struct ptp_message *msg,

			     enum notification event)

{

	unsigned int event_pos = event / 8;

	uint8_t mask = 1 << (event % 8);

	struct port *uds = c->uds_port;

	struct clock_subscriber *s;

	LIST_FOREACH(s, &c->subscribers, list) {

		if (!(s->events[event_pos] & mask))

			continue;

		/* send event */

		msg->header.sequenceId = htons(s->sequenceId);

		s->sequenceId++;

		msg->management.targetPortIdentity.clockIdentity =

			s->targetPortIdentity.clockIdentity;

		msg->management.targetPortIdentity.portNumber =

			htons(s->targetPortIdentity.portNumber);

		msg->address = s->addr;

		port_forward_to(uds, msg);

	}

}

void clock_destroy(struct clock *c)

{

	struct port *p, *tmp;

	clock_flush_subscriptions(c);

	LIST_FOREACH_SAFE(p, &c->ports, list, tmp) {

		clock_remove_port(c, p);

	}

	port_close(c->uds_port);

	free(c->pollfd);

	if (c->clkid != CLOCK_REALTIME) {

		phc_close(c->clkid);

	}

	servo_destroy(c->servo);

	tsproc_destroy(c->tsproc);

	stats_destroy(c->stats.offset);

	stats_destroy(c->stats.freq);

	stats_destroy(c->stats.delay);

	if (c->sanity_check) {

		clockcheck_destroy(c->sanity_check);

	}

	memset(c, 0, sizeof(*c));

	msg_cleanup();

	tc_cleanup();

}

static int clock_fault_timeout(struct port *port, int set)

{

	struct fault_interval i;

	if (!set) {

		pr_debug("clearing fault on port %d", port_number(port));

		return port_set_fault_timer_lin(port, 0);

	}

	fault_interval(port, last_fault_type(port), &i);

	if (i.type == FTMO_LINEAR_SECONDS) {

		pr_debug("waiting %d seconds to clear fault on port %d",

			 i.val, port_number(port));

		return port_set_fault_timer_lin(port, i.val);

	} else if (i.type == FTMO_LOG2_SECONDS) {

		pr_debug("waiting 2^{%d} seconds to clear fault on port %d",

			 i.val, port_number(port));

		return port_set_fault_timer_log(port, 1, i.val);

	}

	pr_err("Unsupported fault interval type %d", i.type);

	return -1;

}

static void clock_freq_est_reset(struct clock *c)

{

	c->fest.origin1 = tmv_zero();

	c->fest.ingress1 = tmv_zero();

	c->fest.count = 0;

}

static void clock_management_send_error(struct port *p,

					struct ptp_message *msg, int error_id)

{

	if (port_management_error(port_identity(p), p, msg, error_id))

		pr_err("failed to send management error status");

}

/* The 'p' and 'req' paremeters are needed for the GET actions that operate

 * on per-client datasets. If such actions do not apply to the caller, it is

 * allowed to pass both of them as NULL.

 */

static int clock_management_fill_response(struct clock *c, struct port *p,

					  struct ptp_message *req,

					  struct ptp_message *rsp, int id)

{

	struct grandmaster_settings_np *gsn;

	struct management_tlv_datum *mtd;

	struct subscribe_events_np *sen;

	struct management_tlv *tlv;

	struct time_status_np *tsn;

	struct tlv_extra *extra;

	struct PTPText *text;

	int datalen = 0;

	extra = tlv_extra_alloc();

	if (!extra) {

		pr_err("failed to allocate TLV descriptor");

		return 0;

	}

	extra->tlv = (struct TLV *) rsp->management.suffix;

	tlv = (struct management_tlv *) rsp->management.suffix;

	tlv->type = TLV_MANAGEMENT;

	tlv->id = id;

	switch (id) {

	case TLV_USER_DESCRIPTION:

		text = (struct PTPText *) tlv->data;

		text->length = c->desc.userDescription.length;

		memcpy(text->text, c->desc.userDescription.text, text->length);

		datalen = 1 + text->length;

		break;

	case TLV_DEFAULT_DATA_SET:

		memcpy(tlv->data, &c->dds, sizeof(c->dds));

		datalen = sizeof(c->dds);

		break;

	case TLV_CURRENT_DATA_SET:

		memcpy(tlv->data, &c->cur, sizeof(c->cur));

		datalen = sizeof(c->cur);

		break;

	case TLV_PARENT_DATA_SET:

		memcpy(tlv->data, &c->dad.pds, sizeof(c->dad.pds));

		datalen = sizeof(c->dad.pds);

		break;

	case TLV_TIME_PROPERTIES_DATA_SET:

		memcpy(tlv->data, &c->tds, sizeof(c->tds));

		datalen = sizeof(c->tds);

		break;

	case TLV_PRIORITY1:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->dds.priority1;

		datalen = sizeof(*mtd);

		break;

	case TLV_PRIORITY2:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->dds.priority2;

		datalen = sizeof(*mtd);

		break;

	case TLV_DOMAIN:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->dds.domainNumber;

		datalen = sizeof(*mtd);

		break;

	case TLV_SLAVE_ONLY:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->dds.flags & DDS_SLAVE_ONLY;

		datalen = sizeof(*mtd);

		break;

	case TLV_CLOCK_ACCURACY:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->dds.clockQuality.clockAccuracy;

		datalen = sizeof(*mtd);

		break;

	case TLV_TRACEABILITY_PROPERTIES:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->tds.flags & (TIME_TRACEABLE|FREQ_TRACEABLE);

		datalen = sizeof(*mtd);

		break;

	case TLV_TIMESCALE_PROPERTIES:

		mtd = (struct management_tlv_datum *) tlv->data;

		mtd->val = c->tds.flags & PTP_TIMESCALE;

		datalen = sizeof(*mtd);

		break;

	case TLV_TIME_STATUS_NP:

		tsn = (struct time_status_np *) tlv->data;

		tsn->master_offset = tmv_to_nanoseconds(c->master_offset);

		tsn->ingress_time = tmv_to_nanoseconds(c->ingress_ts);

		tsn->cumulativeScaledRateOffset =

			(Integer32) (c->status.cumulativeScaledRateOffset +

				      c->nrr * POW2_41 - POW2_41);

		tsn->scaledLastGmPhaseChange = c->status.scaledLastGmPhaseChange;

		tsn->gmTimeBaseIndicator = c->status.gmTimeBaseIndicator;

		tsn->lastGmPhaseChange = c->status.lastGmPhaseChange;

		if (cid_eq(&c->dad.pds.grandmasterIdentity, &c->dds.clockIdentity))

			tsn->gmPresent = 0;

		else

			tsn->gmPresent = 1;

		tsn->gmIdentity = c->dad.pds.grandmasterIdentity;

		datalen = sizeof(*tsn);

		break;

	case TLV_GRANDMASTER_SETTINGS_NP:

		gsn = (struct grandmaster_settings_np *) tlv->data;

		gsn->clockQuality = c->dds.clockQuality;

		gsn->utc_offset = c->utc_offset;

		gsn->time_flags = c->time_flags;

		gsn->time_source = c->time_source;

		datalen = sizeof(*gsn);

		break;

	case TLV_SUBSCRIBE_EVENTS_NP:

		if (p != c->uds_port) {

			/* Only the UDS port allowed. */

			break;

		}

		sen = (struct subscribe_events_np *)tlv->data;

		clock_get_subscription(c, req, sen->bitmask, &sen->duration);

		break;

	default:

		/* The caller should *not* respond to this message. */

		tlv_extra_recycle(extra);

		return 0;

	}

	if (datalen % 2) {

		tlv->data[datalen] = 0;

		datalen++;

	}

	tlv->length = sizeof(tlv->id) + datalen;

	rsp->header.messageLength += sizeof(*tlv) + datalen;

	msg_tlv_attach(rsp, extra);

	/* The caller can respond to this message. */

	return 1;

}

static int clock_management_get_response(struct clock *c, struct port *p,

					 int id, struct ptp_message *req)

{

	struct PortIdentity pid = port_identity(p);

	struct ptp_message *rsp;

	int respond;

	rsp = port_management_reply(pid, p, req);

	if (!rsp) {

		return 0;

	}

	respond = clock_management_fill_response(c, p, req, rsp, id);

	if (respond)

		port_prepare_and_send(p, rsp, TRANS_GENERAL);

	msg_put(rsp);

	return respond;

}

static int clock_management_set(struct clock *c, struct port *p,

				int id, struct ptp_message *req, int *changed)

{

	int respond = 0;

	struct management_tlv *tlv;

	struct management_tlv_datum *mtd;

	struct grandmaster_settings_np *gsn;

	struct subscribe_events_np *sen;

	tlv = (struct management_tlv *) req->management.suffix;

	switch (id) {

	case TLV_PRIORITY1:

		mtd = (struct management_tlv_datum *) tlv->data;

		c->dds.priority1 = mtd->val;

		*changed = 1;

		respond = 1;

		break;

	case TLV_PRIORITY2:

		mtd = (struct management_tlv_datum *) tlv->data;

		c->dds.priority2 = mtd->val;

		*changed = 1;

		respond = 1;

		break;

	case TLV_GRANDMASTER_SETTINGS_NP:

		gsn = (struct grandmaster_settings_np *) tlv->data;

		c->dds.clockQuality = gsn->clockQuality;

		c->utc_offset = gsn->utc_offset;

		c->time_flags = gsn->time_flags;

		c->time_source = gsn->time_source;

		*changed = 1;

		respond = 1;

		break;

	case TLV_SUBSCRIBE_EVENTS_NP:

		sen = (struct subscribe_events_np *)tlv->data;

		clock_update_subscription(c, req, sen->bitmask,

					  sen->duration);

		respond = 1;

		break;

	}

	if (respond && !clock_management_get_response(c, p, id, req))

		pr_err("failed to send management set response");

	return respond ? 1 : 0;

}

static void clock_stats_update(struct clock_stats *s,

			       double offset, double freq)

{

	struct stats_result offset_stats, freq_stats, delay_stats;

	stats_add_value(s->offset, offset);

	stats_add_value(s->freq, freq);

	if (stats_get_num_values(s->offset) < s->max_count)

		return;

	stats_get_result(s->offset, &offset_stats);

	stats_get_result(s->freq, &freq_stats);

	/* Path delay stats are updated separately, they may be empty. */

	if (!stats_get_result(s->delay, &delay_stats)) {

		pr_info("rms %4.0f max %4.0f "

			"freq %+6.0f +/- %3.0f "

			"delay %5.0f +/- %3.0f",

			offset_stats.rms, offset_stats.max_abs,

			freq_stats.mean, freq_stats.stddev,

			delay_stats.mean, delay_stats.stddev);

	} else {

		pr_info("rms %4.0f max %4.0f "

			"freq %+6.0f +/- %3.0f",

			offset_stats.rms, offset_stats.max_abs,

			freq_stats.mean, freq_stats.stddev);

	}

	stats_reset(s->offset);

	stats_reset(s->freq);

	stats_reset(s->delay);

}

static enum servo_state clock_no_adjust(struct clock *c, tmv_t ingress,

					tmv_t origin)

{

	double fui;

	double ratio, freq;

	struct freq_estimator *f = &c->fest;

	enum servo_state state = SERVO_UNLOCKED;

	/*

	 * The ratio of the local clock freqency to the master clock

	 * is estimated by:

	 *

	 *    (ingress_2 - ingress_1) / (origin_2 - origin_1)

	 *

	 * Both of the origin time estimates include the path delay,

	 * but we assume that the path delay is in fact constant.

	 * By leaving out the path delay altogther, we can avoid the

	 * error caused by our imperfect path delay measurement.

	 */

	if (tmv_is_zero(f->ingress1)) {

		f->ingress1 = ingress;

		f->origin1 = origin;

		return state;

	}

	f->count++;

	if (f->count < f->max_count) {

		return state;

	}

	if (tmv_cmp(ingress, f->ingress1) == 0) {

		pr_warning("bad timestamps in rate ratio calculation");

		return state;

	}

	ratio = tmv_dbl(tmv_sub(origin, f->origin1)) /

		tmv_dbl(tmv_sub(ingress, f->ingress1));

	freq = (1.0 - ratio) * 1e9;

	if (c->stats.max_count > 1) {

		clock_stats_update(&c->stats, tmv_dbl(c->master_offset), freq);

	} else {

		pr_info("master offset %10" PRId64 " s%d freq %+7.0f "

			"path delay %9" PRId64,

			tmv_to_nanoseconds(c->master_offset), state, freq,

			tmv_to_nanoseconds(c->path_delay));

	}

	fui = 1.0 + (c->status.cumulativeScaledRateOffset + 0.0) / POW2_41;

	pr_debug("peer/local    %.9f", c->nrr);

	pr_debug("fup_info      %.9f", fui);

	pr_debug("product       %.9f", fui * c->nrr);

	pr_debug("sum-1         %.9f", fui + c->nrr - 1.0);

	pr_debug("master/local  %.9f", ratio);

	pr_debug("diff         %+.9f", ratio - (fui + c->nrr - 1.0));

	f->ingress1 = ingress;

	f->origin1 = origin;

	f->count = 0;

	c->master_local_rr = ratio;

	return state;

}

static void clock_update_grandmaster(struct clock *c)

{

	struct parentDS *pds = &c->dad.pds;

	memset(&c->cur, 0, sizeof(c->cur));

	memset(c->ptl, 0, sizeof(c->ptl));

	pds->parentPortIdentity.clockIdentity   = c->dds.clockIdentity;

	pds->parentPortIdentity.portNumber      = 0;

	pds->grandmasterIdentity                = c->dds.clockIdentity;

	pds->grandmasterClockQuality            = c->dds.clockQuality;

	pds->grandmasterPriority1               = c->dds.priority1;

	pds->grandmasterPriority2               = c->dds.priority2;

	c->dad.path_length                      = 0;

	c->tds.currentUtcOffset                 = c->utc_offset;

	c->tds.flags                            = c->time_flags;

	c->tds.timeSource                       = c->time_source;

}

static void clock_update_slave(struct clock *c)

{

	struct parentDS *pds = &c->dad.pds;

	struct ptp_message *msg        = TAILQ_FIRST(&c->best->messages);

	c->cur.stepsRemoved            = 1 + c->best->dataset.stepsRemoved;

	pds->parentPortIdentity        = c->best->dataset.sender;

	pds->grandmasterIdentity       = msg->announce.grandmasterIdentity;

	pds->grandmasterClockQuality   = msg->announce.grandmasterClockQuality;

	pds->grandmasterPriority1      = msg->announce.grandmasterPriority1;

	pds->grandmasterPriority2      = msg->announce.grandmasterPriority2;

	c->tds.currentUtcOffset        = msg->announce.currentUtcOffset;

	c->tds.flags                   = msg->header.flagField[1];

	c->tds.timeSource              = msg->announce.timeSource;

	if (!(c->tds.flags & PTP_TIMESCALE)) {

		pr_warning("foreign master not using PTP timescale");

	}

	if (c->tds.currentUtcOffset < c->utc_offset) {

		pr_warning("running in a temporal vortex");

	}

	if ((c->tds.flags & UTC_OFF_VALID && c->tds.flags & TIME_TRACEABLE) ||

	    (c->tds.currentUtcOffset > c->utc_offset)) {

		pr_info("updating UTC offset to %d", c->tds.currentUtcOffset);

		c->utc_offset = c->tds.currentUtcOffset;

	}

}

static int clock_utc_correct(struct clock *c, tmv_t ingress)

{

	struct timespec offset;

	int utc_offset, leap, clock_leap;

	uint64_t ts;

	if (!c->utc_timescale)

		return 0;

	utc_offset = c->utc_offset;

	if (c->tds.flags & LEAP_61) {

		leap = 1;

	} else if (c->tds.flags & LEAP_59) {

		leap = -1;

	} else {

		leap = 0;

	}

	/* Handle leap seconds. */

	if ((leap || c->leap_set) && c->clkid == CLOCK_REALTIME) {

		/* If the clock will be stepped, the time stamp has to be the

		   target time. Ignore possible 1 second error in utc_offset. */

		if (c->servo_state == SERVO_UNLOCKED) {

			ts = tmv_to_nanoseconds(tmv_sub(ingress,

							c->master_offset));

			if (c->tds.flags & PTP_TIMESCALE)

				ts -= utc_offset * NS_PER_SEC;

		} else {

			ts = tmv_to_nanoseconds(ingress);

		}

		/* Suspend clock updates in the last second before midnight. */

		if (is_utc_ambiguous(ts)) {

			pr_info("clock update suspended due to leap second");

			return -1;

		}

		clock_leap = leap_second_status(ts, c->leap_set,

						&leap, &utc_offset);

		if (c->leap_set != clock_leap) {

			if (c->kernel_leap)

				sysclk_set_leap(clock_leap);

			else

				servo_leap(c->servo, clock_leap);

			c->leap_set = clock_leap;

		}

	}

	/* Update TAI-UTC offset of the system clock if valid and traceable. */

	if (c->tds.flags & UTC_OFF_VALID && c->tds.flags & TIME_TRACEABLE &&

	    c->utc_offset_set != utc_offset && c->clkid == CLOCK_REALTIME) {

		sysclk_set_tai_offset(utc_offset);

		c->utc_offset_set = utc_offset;

	}

	if (!(c->tds.flags & PTP_TIMESCALE))

		return 0;

	offset.tv_sec = utc_offset;

	offset.tv_nsec = 0;

	/* Local clock is UTC, but master is TAI. */

	c->master_offset = tmv_add(c->master_offset, timespec_to_tmv(offset));

	return 0;

}

static int forwarding(struct clock *c, struct port *p)

{

	enum port_state ps = port_state(p);

	switch (ps) {

	case PS_MASTER:

	case PS_GRAND_MASTER:

	case PS_SLAVE:

	case PS_UNCALIBRATED:

	case PS_PRE_MASTER:

		return 1;

	default:

		break;

	}

	if (p == c->uds_port && ps != PS_FAULTY) {

		return 1;

	}

	return 0;

}

/* public methods */

UInteger8 clock_class(struct clock *c)

{

	return c->dds.clockQuality.clockClass;

}

struct config *clock_config(struct clock *c)

{

	return c->config;

}

int (*clock_dscmp(struct clock *c))(struct dataset *a, struct dataset *b)

{

	return c->dscmp;

}

struct currentDS *clock_current_dataset(struct clock *c)

{

	return &c->cur;

}

static int clock_add_port(struct clock *c, int phc_index,

			  enum timestamp_type timestamping,

			  struct interface *iface)

{

	struct port *p, *piter, *lastp = NULL;

	if (clock_resize_pollfd(c, c->nports + 1)) {

		return -1;

	}

	p = port_open(phc_index, timestamping, ++c->last_port_number, iface, c);

	if (!p) {

		/* No need to shrink pollfd */

		return -1;

	}

	LIST_FOREACH(piter, &c->ports, list) {

		lastp = piter;

	}

	if (lastp) {

		LIST_INSERT_AFTER(lastp, p, list);

	} else {

		LIST_INSERT_HEAD(&c->ports, p, list);

	}

	c->nports++;

	clock_fda_changed(c);

	return 0;

}

static void clock_remove_port(struct clock *c, struct port *p)

{

	/* Do not call clock_resize_pollfd, it's pointless to shrink

	 * the allocated memory at this point, clock_destroy will free

	 * it all anyway. This function is usable from other parts of

	 * the code, but even then we don't mind if pollfd is larger

	 * than necessary. */

	LIST_REMOVE(p, list);

	c->nports--;

	clock_fda_changed(c);

	port_close(p);

}

int clock_required_modes(struct clock *c)

{

	int required_modes = 0;

	switch (c->timestamping) {

	case TS_SOFTWARE:

		required_modes |= SOF_TIMESTAMPING_TX_SOFTWARE |

			SOF_TIMESTAMPING_RX_SOFTWARE |

			SOF_TIMESTAMPING_SOFTWARE;

		break;

	case TS_LEGACY_HW:

		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |

			SOF_TIMESTAMPING_RX_HARDWARE |

			SOF_TIMESTAMPING_SYS_HARDWARE;

		break;

	case TS_HARDWARE:

	case TS_ONESTEP:

	case TS_P2P1STEP:

		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |

			SOF_TIMESTAMPING_RX_HARDWARE |

			SOF_TIMESTAMPING_RAW_HARDWARE;

		break;

	default:

		break;

	}

	return required_modes;

}

/*

 * If we do not have a slave or the rtnl query failed, then use our

 * own interface name as the time stamping interface name.

 */

static void ensure_ts_label(struct interface *iface)

{

	if (iface->ts_label[0] == '\0')

		strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE);

}

struct clock *clock_create(enum clock_type type, struct config *config,

			   const char *phc_device)

{

	enum servo_type servo = config_get_int(config, NULL, "clock_servo");

	enum timestamp_type timestamping;

	int fadj = 0, max_adj = 0, sw_ts;

	int phc_index, required_modes = 0;

	struct clock *c = &the_clock;

	struct port *p;

	unsigned char oui[OUI_LEN];

	char phc[32], *tmp;

	struct interface *iface, *udsif = &c->uds_interface;

	struct timespec ts;

	int sfl;

	clock_gettime(CLOCK_REALTIME, &ts);

	srandom(ts.tv_sec ^ ts.tv_nsec);

	if (c->nports) {

		clock_destroy(c);

	}

	switch (type) {

	case CLOCK_TYPE_ORDINARY:

	case CLOCK_TYPE_BOUNDARY:

	case CLOCK_TYPE_P2P:

	case CLOCK_TYPE_E2E:

		c->type = type;

		break;

	case CLOCK_TYPE_MANAGEMENT:

		return NULL;

	}