text: /**
   * @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 <linux/net_tstamp.h>
  #include <poll.h>
  #include <stdlib.h>
  #include <string.h>
  #include <time.h>
  #include <math.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 "hash.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 "sk.h"
  #include "tlv.h"
  #include "tsproc.h"
  #include "uds.h"
  #include "util.h"
  
  /* QoT Types Header */
  #include "../../../../../qot_types.h"
  
  /* New header for the Quartz Timeline Service */
  #include "../local_timeline.h"
  
  #ifdef PTP_QUARTZ
  /* Added PTP Timeline clock ops */
  #include "../qot_tlclockops.h"
  #endif
  
  #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;
  	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;
  	struct hash *index2port;
  	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;  /* grand master role */
  	int time_flags;  /* grand master role */
  	int time_source; /* grand master role */
  	enum servo_state servo_state;
  	tmv_t master_offset;
  	tmv_t path_delay;
  	tmv_t ingress_ts;
  	struct tsproc *tsproc;
  	struct freq_estimator fest;
  	struct time_status_np status;
  	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;
  	/* Added for the QoT Stack */
  	int timelineid;  					/* Timeline ID */
  	#ifndef PTP_QUARTZ
  	int *timelinesfd; 					/* Timeline File Descriptor */
  	int tml_clkid; 						/* Timeline clock id */
  	#else
  	tl_translation_t* tml_clk_params;	/* Timeline clock parameters */ 
  	#endif
  };
  
  /* Global Clock Structure */
  struct clock timeline_clocks[MAX_TIMELINES];
  
  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 int cid_eq(struct ClockIdentity *a, struct ClockIdentity *b)
  {
  	return 0 == memcmp(a, b, sizeof(*a));
  }
  
  #ifndef LIST_FOREACH_SAFE
  #define	LIST_FOREACH_SAFE(var, head, field, tvar)			\
  	for ((var) = LIST_FIRST((head));				\
  	    (var) && ((tvar) = LIST_NEXT((var), field), 1);		\
  	    (var) = (tvar))
  #endif
  
  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 (!memcmp(&s->targetPortIdentity, &req->header.sourcePortIdentity,
  		            sizeof(struct PortIdentity))) {
  			/* 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 (!memcmp(&s->targetPortIdentity, &req->header.sourcePortIdentity,
  			    sizeof(struct PortIdentity))) {
  			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,
  			     int msglen, 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);
  	}
  	if (c->pollfd[0].fd >= 0) {
  		rtnl_close(c->pollfd[0].fd);
  	}
  	port_close(c->uds_port);
  	free(c->pollfd);
  	hash_destroy(c->index2port, NULL);
  	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();
  }
  
  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_link_status(void *ctx, int index, int linkup)
  {
  	struct clock *c = ctx;
  	struct port *p;
  	char key[16];
  
  	snprintf(key, sizeof(key), "%d", index);
  	p = hash_lookup(c->index2port, key);
  	if (!p) {
  		return;
  	}
  	port_link_status_set(p, linkup);
  	if (linkup) {
  		port_dispatch(p, EV_FAULT_CLEARED, 0);
  	} else {
  		port_dispatch(p, EV_FAULT_DETECTED, 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)
  {
  	int datalen = 0, respond = 0;
  	struct management_tlv *tlv;
  	struct management_tlv_datum *mtd;
  	struct time_status_np *tsn;
  	struct grandmaster_settings_np *gsn;
  	struct subscribe_events_np *sen;
  	struct PTPText *text;
  
  	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;
  		respond = 1;
  		break;
  	case TLV_DEFAULT_DATA_SET:
  		memcpy(tlv->data, &c->dds, sizeof(c->dds));
  		datalen = sizeof(c->dds);
  		respond = 1;
  		break;
  	case TLV_CURRENT_DATA_SET:
  		memcpy(tlv->data, &c->cur, sizeof(c->cur));
  		datalen = sizeof(c->cur);
  		respond = 1;
  		break;
  	case TLV_PARENT_DATA_SET:
  		memcpy(tlv->data, &c->dad.pds, sizeof(c->dad.pds));
  		datalen = sizeof(c->dad.pds);
  		respond = 1;
  		break;
  	case TLV_TIME_PROPERTIES_DATA_SET:
  		memcpy(tlv->data, &c->tds, sizeof(c->tds));
  		datalen = sizeof(c->tds);
  		respond = 1;
  		break;
  	case TLV_PRIORITY1:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->dds.priority1;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_PRIORITY2:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->dds.priority2;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_DOMAIN:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->dds.domainNumber;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_SLAVE_ONLY:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->dds.flags & DDS_SLAVE_ONLY;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_CLOCK_ACCURACY:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->dds.clockQuality.clockAccuracy;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_TRACEABILITY_PROPERTIES:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->tds.flags & (TIME_TRACEABLE|FREQ_TRACEABLE);
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_TIMESCALE_PROPERTIES:
  		mtd = (struct management_tlv_datum *) tlv->data;
  		mtd->val = c->tds.flags & PTP_TIMESCALE;
  		datalen = sizeof(*mtd);
  		respond = 1;
  		break;
  	case TLV_TIME_STATUS_NP:
  		tsn = (struct time_status_np *) tlv->data;
  		tsn->master_offset = 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);
  		respond = 1;
  		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);
  		respond = 1;
  		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);
  		respond = 1;
  		break;
  	}
  	if (respond) {
  		if (datalen % 2) {
  			tlv->data[datalen] = 0;
  			datalen++;
  		}
  		tlv->length = sizeof(tlv->id) + datalen;
  		rsp->header.messageLength += sizeof(*tlv) + datalen;
  		rsp->tlv_count = 1;
  	}
  	return respond;
  }
  
  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, 0);
  	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,
  			       int64_t 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 (!f->ingress1) {
  		f->ingress1 = ingress;
  		f->origin1 = origin;
  		return state;
  	}
  	f->count++;
  	if (f->count < f->max_count) {
  		return state;
  	}
  	if (tmv_eq(ingress, f->ingress1)) {
  		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_to_nanoseconds(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;
  
  	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 < CURRENT_UTC_OFFSET) {
  		pr_warning("running in a temporal vortex");
  	}
  }
  
  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;
  
  	if (c->tds.flags & UTC_OFF_VALID && c->tds.flags & TIME_TRACEABLE) {
  		utc_offset = c->tds.currentUtcOffset;
  	} else if (c->tds.currentUtcOffset > CURRENT_UTC_OFFSET) {
  		utc_offset = c->tds.currentUtcOffset;
  	} else {
  		utc_offset = CURRENT_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;
  }
  
  static int clock_add_port(struct clock *c, int phc_index,
  			  enum timestamp_type timestamping,
  			  struct interface *iface)
  {
  	struct port *p, *piter, *lastp = NULL;
  	int fd, index;
  	char key[16];
  
  	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);
  
  	/* Remember the index to port mapping, for link status tracking. */
  	fd = sk_interface_fd();
  	if (fd < 0) {
  		return -1;
  	}
  	index = sk_interface_index(fd, iface->name);
  	if (index < 0) {
  		return -1;
  	}
  	snprintf(key, sizeof(key), "%d", index);
  	if (hash_insert(c->index2port, key, p)) {
  		pr_err("failed to add port with index %d twice!", index);
  		return -1;
  	}
  	close(fd);
  
  	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);
  }
  
  #ifdef PTP_QUARTZ
  // Flag indicating if the clock is the master for this timeline
  int timeline_master_flag[MAX_TIMELINES];
  #endif
  
  struct clock *clock_create(enum clock_type type, struct config *config,
  			   const char *phc_device, int timelineid, int *timelinesfd, void* clk_params)
  {
  	enum timestamp_type timestamping =
  		config_get_int(config, NULL, "time_stamping");
  	int fadj = 0, max_adj = 0, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0;
  	enum servo_type servo = config_get_int(config, NULL, "clock_servo");
  	int phc_index, required_modes = 0;
  	struct clock *c = &timeline_clocks[timelineid];//&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:
  		c->type = type;
  		break;
  	case CLOCK_TYPE_P2P:
  	case CLOCK_TYPE_E2E:
  	case CLOCK_TYPE_MANAGEMENT:
  		return NULL;
  	}
  
  	#ifdef PTP_QUARTZ
  	// Set the timeline node is master flag to 0
  	timeline_master_flag[c->timelineid] = 0;
  	#endif
  
  	/* Initialize the defaultDS. */
  	c->dds.clockQuality.clockClass =
  		config_get_int(config, NULL, "clockClass");
  	c->dds.clockQuality.clockAccuracy =
  		config_get_int(config, NULL, "clockAccuracy");
  	c->dds.clockQuality.offsetScaledLogVariance =
  		config_get_int(config, NULL, "offsetScaledLogVariance");
  
  	c->desc.productDescription.max_symbols = 64;
  	c->desc.revisionData.max_symbols = 32;
  	c->desc.userDescription.max_symbols = 128;
  
  	tmp = config_get_string(config, NULL, "productDescription");
  	if (count_char(tmp, ';') != 2 ||
  	    static_ptp_text_set(&c->desc.productDescription, tmp)) {
  		pr_err("invalid productDescription '%s'", tmp);
  		return NULL;
  	}
  	tmp = config_get_string(config, NULL, "revisionData");
  	if (count_char(tmp, ';') != 2 ||
  	    static_ptp_text_set(&c->desc.revisionData, tmp)) {
  		pr_err("invalid revisionData '%s'", tmp);
  		return NULL;
  	}
  	tmp = config_get_string(config, NULL, "userDescription");
  	if (static_ptp_text_set(&c->desc.userDescription, tmp)) {
  		pr_err("invalid userDescription '%s'", tmp);
  		return NULL;
  	}
  	tmp = config_get_string(config, NULL, "manufacturerIdentity");
  	if (OUI_LEN != sscanf(tmp, "%hhx:%hhx:%hhx", &oui[0], &oui[1], &oui[2])) {
  		pr_err("invalid manufacturerIdentity '%s'", tmp);
  		return NULL;
  	}
  	memcpy(c->desc.manufacturerIdentity, oui, OUI_LEN);
  
  	c->dds.domainNumber = config_get_int(config, NULL, "domainNumber");
  
  	printf("Clock PTP Domain is %d\n", c->dds.domainNumber);
  
  	if (config_get_int(config, NULL, "slaveOnly")) {
  		c->dds.flags |= DDS_SLAVE_ONLY;
  	}
  	if (config_get_int(config, NULL, "twoStepFlag")) {
  		c->dds.flags |= DDS_TWO_STEP_FLAG;
  	}
  	c->dds.priority1 = config_get_int(config, NULL, "priority1");
  	c->dds.priority2 = config_get_int(config, NULL, "priority2");
  
  	if (!config_get_int(config, NULL, "gmCapable") &&
  	    c->dds.flags & DDS_SLAVE_ONLY) {
  		pr_err("Cannot mix 1588 slaveOnly with 802.1AS !gmCapable");
  		return NULL;
  	}
  	if (!config_get_int(config, NULL, "gmCapable") ||
  	    c->dds.flags & DDS_SLAVE_ONLY) {
  		c->dds.clockQuality.clockClass = 255;
  	}
  
  	if (!(c->dds.flags & DDS_TWO_STEP_FLAG)) {
  		switch (timestamping) {
  		case TS_SOFTWARE:
  		case TS_LEGACY_HW:
  			pr_err("one step is only possible "
  			       "with hardware time stamping");
  			return NULL;
  		case TS_HARDWARE:
  			timestamping = TS_ONESTEP;
  			if (config_set_int(config, "time_stamping", TS_ONESTEP))
  				return NULL;
  			break;
  		case TS_ONESTEP:
  			break;
  		}
  	}
  
  	/* Check the time stamping mode on each interface. */
  	switch (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:
  		required_modes |= SOF_TIMESTAMPING_TX_HARDWARE |
  			SOF_TIMESTAMPING_RX_HARDWARE |
  			SOF_TIMESTAMPING_RAW_HARDWARE;
  		break;
  	}
  	STAILQ_FOREACH(iface, &config->interfaces, list) {
  		if (iface->ts_info.valid &&
  		    ((iface->ts_info.so_timestamping & required_modes) != required_modes)) {
  			pr_err("interface '%s' does not support "
  			       "requested timestamping mode", iface->name);
  		    /* Modified for the QoT Stack -> Use SW Timestamping if HW Timestamping not supported*/
  			if (config_set_int(config, "time_stamping", TS_SOFTWARE))
  			{
  				return NULL;
  			}
  		}
  	}
  
  	iface = STAILQ_FIRST(&config->interfaces);
  
  	/* determine PHC Clock index */
  	if (config_get_int(config, NULL, "free_running")) {
  		phc_index = -1;
  	} else if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) {
  		phc_index = -1;
  	} else if (phc_device) {
  		if (1 != sscanf(phc_device, "/dev/ptp%d", &phc_index)) {
  			pr_err("bad ptp device string");
  			return NULL;
  		}
  	} else if (iface->ts_info.valid) {
  		phc_index = iface->ts_info.phc_index;
  	} else {
  		pr_err("PTP device not specified and automatic determination"
  		       " is not supported. Please specify PTP device.");
  		return NULL;
  	}
  	if (phc_index >= 0) {
  		pr_info("selected /dev/ptp%d as PTP clock", phc_index);
  	}
  
  	if (generate_clock_identity(&c->dds.clockIdentity, iface->name)) {
  		pr_err("failed to generate a clock identity");
  		return NULL;
  	}
  
  	/* Configure the UDS. */
  	snprintf(udsif->name, sizeof(udsif->name), "%s",
  		 config_get_string(config, NULL, "uds_address"));
  	if (config_set_section_int(config, udsif->name,
  				   "announceReceiptTimeout", 0)) {
  		return NULL;
  	}
  	if (config_set_section_int(config, udsif->name,
  				    "delay_mechanism", DM_AUTO)) {
  		return NULL;
  	}
  	if (config_set_section_int(config, udsif->name,
  				    "network_transport", TRANS_UDS)) {
  		return NULL;
  	}
  	if (config_set_section_int(config, udsif->name, "delay_filter_length", 1)) {
  		return NULL;
  	}
  
  	c->config = config;
  	c->free_running = config_get_int(config, NULL, "free_running");
  	c->freq_est_interval = config_get_int(config, NULL, "freq_est_interval");
  	c->grand_master_capable = config_get_int(config, NULL, "gmCapable");
  	c->kernel_leap = config_get_int(config, NULL, "kernel_leap");
  	c->utc_offset = CURRENT_UTC_OFFSET;
  	c->time_source = config_get_int(config, NULL, "timeSource");
  
  	if (c->free_running) {
  		c->clkid = CLOCK_INVALID;
  		if (timestamping == TS_SOFTWARE || timestamping == TS_LEGACY_HW) {
  			c->utc_timescale = 1;
  		}
  	} else if (phc_index >= 0) {
  		snprintf(phc, 31, "/dev/ptp%d", phc_index);
  		c->clkid = phc_open(phc);
  		if (c->clkid == CLOCK_INVALID) {
  			pr_err("Failed to open %s: %m", phc);
  			return NULL;
  		}
  		max_adj = phc_max_adj(c->clkid);
  		if (!max_adj) {
  			pr_err("clock is not adjustable");
  			return NULL;
  		}
  		clockadj_init(c->clkid);
  	} else {
  		c->clkid = CLOCK_REALTIME;
  		c->utc_timescale = 1;
  		clockadj_init(c->clkid);
  		max_adj = sysclk_max_freq();
  		sysclk_set_leap(0);
  	}
  
  	/* Added for the QoT Stack */
  	#ifdef PTP_QUARTZ
  	/* Register the identity of the PHC clock with the timeline clock adjust subsystem */
  	qot_set_phc(c->clkid);
  	#endif
  	c->utc_offset_set = 0;
  	c->leap_set = 0;
  	c->time_flags = c->utc_timescale ? 0 : PTP_TIMESCALE;
  
  	if (c->clkid != CLOCK_INVALID) {
  		fadj = (int) clockadj_get_freq(c->clkid);
  		/* Due to a bug in older kernels, the reading may silently fail
  		   and return 0. Set the frequency back to make sure fadj is
  		   the actual frequency of the clock. */
  		clockadj_set_freq(c->clkid, fadj);
  	}
  	c->servo = servo_create(c->config, servo, -fadj, max_adj, sw_ts);
  	if (!c->servo) {
  		pr_err("Failed to create clock servo");
  		return NULL;
  	}
  	c->servo_state = SERVO_UNLOCKED;
  	c->servo_type = servo;
  	c->tsproc = tsproc_create(config_get_int(config, NULL, "tsproc_mode"),
  				  config_get_int(config, NULL, "delay_filter"),
  				  config_get_int(config, NULL, "delay_filter_length"));
  	if (!c->tsproc) {
  		pr_err("Failed to create time stamp processor");
  		return NULL;
  	}
  	c->nrr = 1.0;
  	c->stats_interval = config_get_int(config, NULL, "summary_interval");
  	c->stats.offset = stats_create();
  	c->stats.freq = stats_create();
  	c->stats.delay = stats_create();
  	if (!c->stats.offset || !c->stats.freq || !c->stats.delay) {
  		pr_err("failed to create stats");
  		return NULL;
  	}
  	sfl = config_get_int(config, NULL, "sanity_freq_limit");
  	if (sfl) {
  		c->sanity_check = clockcheck_create(sfl);
  		if (!c->sanity_check) {
  			pr_err("Failed to create clock sanity check");
  			return NULL;
  		}
  	}
  
  	/* Initialize the parentDS. */
  	clock_update_grandmaster(c);
  	c->dad.pds.parentStats                           = 0;
  	c->dad.pds.observedParentOffsetScaledLogVariance = 0xffff;
  	c->dad.pds.observedParentClockPhaseChangeRate    = 0x7fffffff;
  	c->dad.ptl = c->ptl;
  
  	clock_sync_interval(c, 0);
  
  	LIST_INIT(&c->subscribers);
  	LIST_INIT(&c->ports);
  	c->last_port_number = 0;
  
  	if (clock_resize_pollfd(c, 0)) {
  		pr_err("failed to allocate pollfd");
  		return NULL;
  	}
  
  	/* Open a RT netlink socket. */
  	c->pollfd[0].fd = rtnl_open();
  	c->pollfd[0].events = POLLIN|POLLPRI;
  
  	/* QoT Stack Related Timeline Metadata */
  	c->timelineid   = timelineid;
  	#ifdef PTP_QUARTZ
  	c->tml_clk_params = (tl_translation_t*) clk_params;
  	#else
  	c->timelinesfd  = timelinesfd; 
  	c->tml_clkid    = FD_TO_CLOCKID(timelinesfd[0]);
  	#endif
  
  	/* Create the UDS interface. */
  	c->uds_port = port_open(phc_index, timestamping, 0, udsif, c);
  	if (!c->uds_port) {
  		pr_err("failed to open the UDS port");
  		return NULL;
  	}
  	clock_fda_changed(c);
  
  	c->index2port = hash_create();
  	if (!c->index2port) {
  		pr_err("failed create index-to-port hash table");
  		return NULL;
  	}
  	/* Create the ports. */
  	STAILQ_FOREACH(iface, &config->interfaces, list) {
  		if (clock_add_port(c, phc_index, timestamping, iface)) {
  			pr_err("failed to open port %s", iface->name);
  			return NULL;
  		}
  	}
  
  	c->dds.numberPorts = c->nports;
  
  	LIST_FOREACH(p, &c->ports, list) {
  		port_dispatch(p, EV_INITIALIZE, 0);
  	}
  	port_dispatch(c->uds_port, EV_INITIALIZE, 0);
  	if (c->pollfd[0].fd >= 0) {
  		rtnl_link_query(c->pollfd[0].fd);
  	}
  	return c;
  }
  
  struct dataset *clock_best_foreign(struct clock *c)
  {
  	return c->best ? &c->best->dataset : NULL;
  }
  
  struct port *clock_best_port(struct clock *c)
  {
  	return c->best ? c->best->port : NULL;
  }
  
  struct dataset *clock_default_ds(struct clock *c)
  {
  	struct dataset *out = &c->default_dataset;
  	struct defaultDS *in = &c->dds;
  
  	out->priority1              = in->priority1;
  	out->identity               = in->clockIdentity;
  	out->quality                = in->clockQuality;
  	out->priority2              = in->priority2;
  	out->stepsRemoved           = 0;
  	out->sender.clockIdentity   = in->clockIdentity;
  	out->sender.portNumber      = 0;
  	out->receiver.clockIdentity = in->clockIdentity;
  	out->receiver.portNumber    = 0;
  
  	return out;
  }
  
  UInteger8 clock_domain_number(struct clock *c)
  {
  	return c->dds.domainNumber;
  }
  
  struct port *clock_first_port(struct clock *c)
  {
  	return LIST_FIRST(&c->ports);
  }
  
  void clock_follow_up_info(struct clock *c, struct follow_up_info_tlv *f)
  {
  	c->status.cumulativeScaledRateOffset = f->cumulativeScaledRateOffset;
  	c->status.scaledLastGmPhaseChange = f->scaledLastGmPhaseChange;
  	c->status.gmTimeBaseIndicator = f->gmTimeBaseIndicator;
  	memcpy(&c->status.lastGmPhaseChange, &f->lastGmPhaseChange,
  	       sizeof(c->status.lastGmPhaseChange));
  }
  
  int clock_gm_capable(struct clock *c)
  {
  	return c->grand_master_capable;
  }
  
  struct ClockIdentity clock_identity(struct clock *c)
  {
  	return c->dds.clockIdentity;
  }
  
  static int clock_resize_pollfd(struct clock *c, int new_nports)
  {
  	struct pollfd *new_pollfd;
  
  	/*
  	 * Need to allocate one descriptor for RT netlink and one
  	 * whole extra block of fds for UDS.
  	 */
  	new_pollfd = realloc(c->pollfd,
  			     (1 + (new_nports + 1) * N_CLOCK_PFD) *
  			     sizeof(struct pollfd));
  	if (!new_pollfd)
  		return -1;
  	c->pollfd = new_pollfd;
  	return 0;
  }
  
  static void clock_fill_pollfd(struct pollfd *dest, struct port *p)
  {
  	struct fdarray *fda;
  	int i;
  
  	fda = port_fda(p);
  	for (i = 0; i < N_POLLFD; i++) {
  		dest[i].fd = fda->fd[i];
  		dest[i].events = POLLIN|POLLPRI;
  	}
  	dest[i].fd = port_fault_fd(p);
  	dest[i].events = POLLIN|POLLPRI;
  }
  
  static void clock_check_pollfd(struct clock *c)
  {
  	struct port *p;
  	struct pollfd *dest = c->pollfd + 1;
  
  	if (c->pollfd_valid)
  		return;
  	LIST_FOREACH(p, &c->ports, list) {
  		clock_fill_pollfd(dest, p);
  		dest += N_CLOCK_PFD;
  	}
  	clock_fill_pollfd(dest, c->uds_port);
  	c->pollfd_valid = 1;
  }
  
  void clock_fda_changed(struct clock *c)
  {
  	c->pollfd_valid = 0;
  }
  
  static int clock_do_forward_mgmt(struct clock *c,
  				 struct port *in, struct port *out,
  				 struct ptp_message *msg, int *pre_sent)
  {
  	if (in == out || !forwarding(c, out))
  		return 0;
  	if (!*pre_sent) {
  		/* delay calling msg_pre_send until
  		 * actually forwarding */
  		msg_pre_send(msg);
  		*pre_sent = 1;
  	}
  	return port_forward(out, msg);
  }
  
  static void clock_forward_mgmt_msg(struct clock *c, struct port *p, struct ptp_message *msg)
  {
  	struct port *piter;
  	int pdulen = 0, msg_ready = 0;
  
  	if (forwarding(c, p) && msg->management.boundaryHops) {
  		pdulen = msg->header.messageLength;
  		msg->management.boundaryHops--;
  		LIST_FOREACH(piter, &c->ports, list) {
  			if (clock_do_forward_mgmt(c, p, piter, msg, &msg_ready))
  				pr_err("port %d: management forward failed",
  				       port_number(piter));
  		}
  		if (clock_do_forward_mgmt(c, p, c->uds_port, msg, &msg_ready))
  			pr_err("uds port: management forward failed");
  		if (msg_ready) {
  			msg_post_recv(msg, pdulen);
  			msg->management.boundaryHops++;
  		}
  	}
  }
  
  int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg)
  {
  	int changed = 0, res, answers;
  	struct port *piter;
  	struct management_tlv *mgt;
  	struct ClockIdentity *tcid, wildcard = {
  		{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}
  	};
  
  	/* Forward this message out all eligible ports. */
  	clock_forward_mgmt_msg(c, p, msg);
  
  	/* Apply this message to the local clock and ports. */
  	tcid = &msg->management.targetPortIdentity.clockIdentity;
  	if (!cid_eq(tcid, &wildcard) && !cid_eq(tcid, &c->dds.clockIdentity)) {
  		return changed;
  	}
  	if (msg->tlv_count != 1) {
  		return changed;
  	}
  	mgt = (struct management_tlv *) msg->management.suffix;
  
  	/*
  	  The correct length according to the management ID is checked
  	  in tlv.c, but management TLVs with empty bodies are also
  	  received successfully to support GETs and CMDs. At this
  	  point the TLV either has the correct length or length 2.
  	*/
  	switch (management_action(msg)) {
  	case GET:
  		if (clock_management_get_response(c, p, mgt->id, msg))
  			return changed;
  		break;
  	case SET:
  		if (mgt->length == 2 && mgt->id != TLV_NULL_MANAGEMENT) {
  			clock_management_send_error(p, msg, TLV_WRONG_LENGTH);
  			return changed;
  		}
  		if (p != c->uds_port) {
  			/* Sorry, only allowed on the UDS port. */
  			clock_management_send_error(p, msg, TLV_NOT_SUPPORTED);
  			return changed;
  		}
  		if (clock_management_set(c, p, mgt->id, msg, &changed))
  			return changed;
  		break;
  	case COMMAND:
  		break;
  	default:
  		return changed;
  	}
  
  	switch (mgt->id) {
  	case TLV_PORT_PROPERTIES_NP:
  		if (p != c->uds_port) {
  			/* Only the UDS port allowed. */
  			clock_management_send_error(p, msg, TLV_NOT_SUPPORTED);
  			return 0;
  		}
  	}
  
  	switch (mgt->id) {
  	case TLV_USER_DESCRIPTION:
  	case TLV_SAVE_IN_NON_VOLATILE_STORAGE:
  	case TLV_RESET_NON_VOLATILE_STORAGE:
  	case TLV_INITIALIZE:
  	case TLV_FAULT_LOG:
  	case TLV_FAULT_LOG_RESET:
  	case TLV_DEFAULT_DATA_SET:
  	case TLV_CURRENT_DATA_SET:
  	case TLV_PARENT_DATA_SET:
  	case TLV_TIME_PROPERTIES_DATA_SET:
  	case TLV_PRIORITY1:
  	case TLV_PRIORITY2:
  	case TLV_DOMAIN:
  	case TLV_SLAVE_ONLY:
  	case TLV_TIME:
  	case TLV_CLOCK_ACCURACY:
  	case TLV_UTC_PROPERTIES:
  	case TLV_TRACEABILITY_PROPERTIES:
  	case TLV_TIMESCALE_PROPERTIES:
  	case TLV_PATH_TRACE_LIST:
  	case TLV_PATH_TRACE_ENABLE:
  	case TLV_GRANDMASTER_CLUSTER_TABLE:
  	case TLV_ACCEPTABLE_MASTER_TABLE:
  	case TLV_ACCEPTABLE_MASTER_MAX_TABLE_SIZE:
  	case TLV_ALTERNATE_TIME_OFFSET_ENABLE:
  	case TLV_ALTERNATE_TIME_OFFSET_NAME:
  	case TLV_ALTERNATE_TIME_OFFSET_MAX_KEY:
  	case TLV_ALTERNATE_TIME_OFFSET_PROPERTIES:
  	case TLV_TRANSPARENT_CLOCK_DEFAULT_DATA_SET:
  	case TLV_PRIMARY_DOMAIN:
  	case TLV_TIME_STATUS_NP:
  	case TLV_GRANDMASTER_SETTINGS_NP:
  	case TLV_SUBSCRIBE_EVENTS_NP:
  		clock_management_send_error(p, msg, TLV_NOT_SUPPORTED);
  		break;
  	default:
  		answers = 0;
  		LIST_FOREACH(piter, &c->ports, list) {
  			res = port_manage(piter, p, msg);
  			if (res < 0)
  				return changed;
  			if (res > 0)
  				answers++;
  		}
  		if (!answers) {
  			/* IEEE 1588 Interpretation #21 suggests to use
  			 * TLV_WRONG_VALUE for ports that do not exist */
  			clock_management_send_error(p, msg, TLV_WRONG_VALUE);
  		}
  		break;
  	}
  	return changed;
  }
  
  void clock_notify_event(struct clock *c, enum notification event)
  {
  	struct port *uds = c->uds_port;
  	struct PortIdentity pid = port_identity(uds);
  	struct ptp_message *msg;
  	UInteger16 msg_len;
  	int id;
  
  	switch (event) {
  	/* set id */
  	default:
  		return;
  	}
  	/* targetPortIdentity and sequenceId will be filled by
  	 * clock_send_notification */
  	msg = port_management_notify(pid, uds);
  	if (!msg)
  		return;
  	if (!clock_management_fill_response(c, NULL, NULL, msg, id))
  		goto err;
  	msg_len = msg->header.messageLength;
  	if (msg_pre_send(msg))
  		goto err;
  	clock_send_notification(c, msg, msg_len, event);
  err:
  	msg_put(msg);
  }
  
  struct parent_ds *clock_parent_ds(struct clock *c)
  {
  	return &c->dad;
  }
  
  struct PortIdentity clock_parent_identity(struct clock *c)
  {
  	return c->dad.pds.parentPortIdentity;
  }
  
  int clock_poll(struct clock *c)
  {
  	int cnt, err, i, sde = 0;
  	enum fsm_event event;
  	struct pollfd *cur;
  	struct port *p;
  
  	clock_check_pollfd(c);
  	cnt = poll(c->pollfd, 1 + (c->nports + 1) * N_CLOCK_PFD, -1);
  	if (cnt < 0) {
  		if (EINTR == errno) {
  			return 0;
  		} else {
  			pr_emerg("poll failed");
  			return -1;
  		}
  	} else if (!cnt) {
  		return 0;
  	}
  
  	/* Check the RT netlink. */
  	cur = c->pollfd;
  	if (cur->revents & (POLLIN|POLLPRI)) {
  		rtnl_link_status(cur->fd, clock_link_status, c);
  	}
  
  	cur++;
  	LIST_FOREACH(p, &c->ports, list) {
  		/* Let the ports handle their events. */
  		for (i = err = 0; i < N_POLLFD && !err; i++) {
  			if (cur[i].revents & (POLLIN|POLLPRI)) {
  				event = port_event(p, i);
  				if (EV_STATE_DECISION_EVENT == event)
  					sde = 1;
  				if (EV_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES == event)
  					sde = 1;
  				err = port_dispatch(p, event, 0);
  				/* Clear any fault after a little while. */
  				if (PS_FAULTY == port_state(p)) {
  					clock_fault_timeout(p, 1);
  					break;
  				}
  			}
  		}
  
  		/*
  		 * When the fault timer expires we clear the fault,
  		 * but only if the link is up.
  		 */
  		if (cur[N_POLLFD].revents & (POLLIN|POLLPRI)) {
  			clock_fault_timeout(p, 0);
  			if (port_link_status_get(p)) {
  				port_dispatch(p, EV_FAULT_CLEARED, 0);
  			}
  		}
  
  		cur += N_CLOCK_PFD;
  	}
  
  	/* Check the UDS port. */
  	for (i = 0; i < N_POLLFD; i++) {
  		if (cur[i].revents & (POLLIN|POLLPRI)) {
  			event = port_event(c->uds_port, i);
  			if (EV_STATE_DECISION_EVENT == event)
  				sde = 1;
  		}
  	}
  
  	if (sde)
  		handle_state_decision_event(c);
  
  	clock_prune_subscriptions(c);
  	return 0;
  }
  
  void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx)
  {
  	tsproc_up_ts(c->tsproc, req, rx);
  
  	if (tsproc_update_delay(c->tsproc, &c->path_delay))
  		return;
  
  	c->cur.meanPathDelay = tmv_to_TimeInterval(c->path_delay);
  
  	if (c->stats.delay)
  		stats_add_value(c->stats.delay,
  				tmv_to_nanoseconds(c->path_delay));
  }
  
  void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx,
  		      double nrr)
  {
  	c->path_delay = ppd;
  	c->nrr = nrr;
  
  	tsproc_set_delay(c->tsproc, ppd);
  	tsproc_up_ts(c->tsproc, req, rx);
  
  	if (c->stats.delay)
  		stats_add_value(c->stats.delay, tmv_to_nanoseconds(ppd));
  }
  
  int clock_slave_only(struct clock *c)
  {
  	return c->dds.flags & DDS_SLAVE_ONLY;
  }
  
  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->config, 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;
  }
  
  // Global Variable for Sharing Computed Clock Statistic from Sync to Uncertainty Calculation
  qot_stat_t ptp_clocksync_data_point[MAX_TIMELINES];
  
  #ifdef PTP_QUARTZ
  /* Quartz function to project core time to timeline time */
  int clock_project_timeline(tl_translation_t* clk_params, tmv_t ts, struct timespec *tml_ts)
  {
  	utimepoint_t utp; 
  
  	TP_FROM_nSEC(utp.estimate, ts);
  
  	qot_loc2rem(&utp, 0, clk_params);
  	tml_ts->tv_sec = utp.estimate.sec;
      tml_ts->tv_nsec= utp.estimate.asec / NS_PER_SEC;
  	return 0;
  }
  
  /* Quartz function to set the timeline frequency */
  void clock_timeline_set_freq(tl_translation_t* clk_params, double freq)
  {
  	struct timex tx;
  	memset(&tx, 0, sizeof(tx));
  
  	tx.modes |= ADJ_FREQUENCY;
  	tx.freq = (long) (freq * 65.536);
  	if (qot_timeline_adjtime(&tx, clk_params) < 0)
  		pr_err("failed to adjust the clock: %m");
  }
  
  /* Quartz function to step the timeline clock */
  void clock_timeline_step(tl_translation_t* clk_params, 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 (qot_timeline_adjtime(&tx, clk_params) < 0)
  		pr_err("failed to step clock: %m");
  }
  
  #else
  /* QoT Stack function to project core time to timeline time */
  int clock_project_timeline(clockid_t clkid, tmv_t ts, struct timespec *tml_ts)
  {
  	stimepoint_t utp; 
  
  	TP_FROM_nSEC(utp.estimate, ts);
  
  	int fd = CLOCKID_TO_FD(clkid);
  
  	if(ioctl(fd, TIMELINE_CORE_TO_REMOTE, &utp)){
  		return -1;
  	}
  	tml_ts->tv_sec = utp.estimate.sec;
      tml_ts->tv_nsec= utp.estimate.asec / NS_PER_SEC;
  	return 0;
  }
  #endif
  
  enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
  {
  	double adj, weight;
  	enum servo_state state = SERVO_UNLOCKED;
  	struct timespec ingress_tml;             // Added for the QoT Stack
  	tmv_t ingress_tml_tmv;				     // Added for the QoT Stack
  
  	c->ingress_ts = ingress;
  
  	/* QoT Stack Project received timestamp to timeline reference */
  	#ifdef PTP_QUARTZ
  	if(clock_project_timeline(c->tml_clk_params, ingress, &ingress_tml)){
  		pr_warning("[T%i]: timeline projection failed", c->timelineid);
  		return SERVO_UNLOCKED;
  	}
  	#else
  	if(clock_project_timeline(c->tml_clkid, ingress, &ingress_tml)){
  		pr_warning("[T%i]: timeline projection failed", c->timelineid);
  		return SERVO_UNLOCKED;
  	}
  	#endif
  
  	/* Convert timeline timestamps to tmv */
  	ingress_tml_tmv = timespec_to_tmv(ingress_tml);
  
  	/* Enter in Values to the ts system*/
  	tsproc_down_ts(c->tsproc, origin, ingress_tml_tmv);
  
  	/* Compute offset */
  	if (tsproc_update_offset(c->tsproc, &c->master_offset, &weight))
  		return state;
  
  	/* Replace timestamps with original timestamps */
  	tsproc_down_ts(c->tsproc, origin, ingress);
  
  	if (clock_utc_correct(c, ingress_tml_tmv))
  		return c->servo_state;
  
  	c->cur.offsetFromMaster = tmv_to_TimeInterval(c->master_offset);
  
  	if (c->free_running)
  		return clock_no_adjust(c, ingress, origin);
  
  	adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset),
  			   tmv_to_nanoseconds(ingress_tml_tmv), weight, &state);
  	c->servo_state = state;
  
  	if (c->stats.max_count > 1) {
  		clock_stats_update(&c->stats,
  				   tmv_to_nanoseconds(c->master_offset), adj);
  	} else {
  		pr_info("[T%i]: master offset %10" PRId64 " s%d freq %+7.0f "
  			"path delay %9" PRId64, c->timelineid,
  			tmv_to_nanoseconds(c->master_offset), state, adj,
  			tmv_to_nanoseconds(c->path_delay));
  	}
  
  	tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c));
  
  	switch (state) {
  	case SERVO_UNLOCKED:
  		break;
  	case SERVO_JUMP:
  		/* Changes for QoT Stack */
  		#ifdef PTP_QUARTZ
  		clock_timeline_set_freq(c->tml_clk_params, -adj);
  		clock_timeline_step(c->tml_clk_params, -tmv_to_nanoseconds(c->master_offset));
  		#else
  		clockadj_set_freq(c->tml_clkid, -adj);
  		clockadj_step(c->tml_clkid, -tmv_to_nanoseconds(c->master_offset));
  		#endif
  		c->ingress_ts = tmv_zero();
  		// if (c->sanity_check) {
  		// 	clockcheck_set_freq(c->sanity_check, -adj);
  		// 	clockcheck_step(c->sanity_check,
  		// 			-tmv_to_nanoseconds(c->master_offset));
  		// }
  		tsproc_reset(c->tsproc, 0);
  		// Add Statistic for the QoT Uncertainty Service to process
  		ptp_clocksync_data_point[c->timelineid].offset = tmv_to_nanoseconds(c->master_offset);
  		ptp_clocksync_data_point[c->timelineid].drift = (int64_t)ceil(adj);
  		ptp_clocksync_data_point[c->timelineid].data_id++;
  		break;
  	case SERVO_LOCKED:
  		/* Changes for QoT Stack */
  		#ifdef PTP_QUARTZ
  		clock_timeline_set_freq(c->tml_clk_params, -adj);
  		#else
  		clockadj_set_freq(c->tml_clkid, -adj);
  		#endif
  		if (c->clkid == CLOCK_REALTIME)
  			sysclk_set_sync();
  		// if (c->sanity_check)
  		// 	clockcheck_set_freq(c->sanity_check, -adj);
  		// Add Statistic for the QoT Uncertainty Service to process
  		ptp_clocksync_data_point[c->timelineid].offset = tmv_to_nanoseconds(c->master_offset);
  		ptp_clocksync_data_point[c->timelineid].drift = (int64_t)ceil(adj);
  		ptp_clocksync_data_point[c->timelineid].data_id++;
  		break;
  	}
  	return state;
  }
  
  void clock_sync_interval(struct clock *c, int n)
  {
  	int shift;
  
  	shift = c->freq_est_interval - n;
  	if (shift < 0)
  		shift = 0;
  	else if (shift >= sizeof(int) * 8) {
  		shift = sizeof(int) * 8 - 1;
  		pr_warning("freq_est_interval is too long");
  	}
  	c->fest.max_count = (1 << shift);
  
  	shift = c->stats_interval - n;
  	if (shift < 0)
  		shift = 0;
  	else if (shift >= sizeof(int) * 8) {
  		shift = sizeof(int) * 8 - 1;
  		pr_warning("summary_interval is too long");
  	}
  	c->stats.max_count = (1 << shift);
  
  	servo_sync_interval(c->servo, n < 0 ? 1.0 / (1 << -n) : 1 << n);
  }
  
  struct timePropertiesDS *clock_time_properties(struct clock *c)
  {
  	return &c->tds;
  }
  
  void clock_update_time_properties(struct clock *c, struct timePropertiesDS tds)
  {
  	c->tds = tds;
  }
  
  static void handle_state_decision_event(struct clock *c)
  {
  	struct foreign_clock *best = NULL, *fc;
  	struct ClockIdentity best_id;
  	struct port *piter;
  	int fresh_best = 0;
  
  	LIST_FOREACH(piter, &c->ports, list) {
  		fc = port_compute_best(piter);
  		if (!fc)
  			continue;
  		if (!best || dscmp(&fc->dataset, &best->dataset) > 0)
  			best = fc;
  	}
  
  	if (best) {
  		best_id = best->dataset.identity;
  	} else {
  		best_id = c->dds.clockIdentity;
  	}
  
  	pr_notice("selected best master clock %s",
  		  cid2str(&best_id));
  
  	if (!cid_eq(&best_id, &c->best_id)) {
  		clock_freq_est_reset(c);
  		tsproc_reset(c->tsproc, 1);
  		c->ingress_ts = tmv_zero();
  		c->path_delay = 0;
  		c->nrr = 1.0;
  		fresh_best = 1;
  	}
  
  	c->best = best;
  	c->best_id = best_id;
  
  	LIST_FOREACH(piter, &c->ports, list) {
  		enum port_state ps;
  		enum fsm_event event;
  		ps = bmc_state_decision(c, piter);
  		switch (ps) {
  		case PS_LISTENING:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 0;
  			#endif
  			event = EV_NONE;
  			break;
  		case PS_GRAND_MASTER:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 1;
  			#endif
  			pr_notice("assuming the grand master role");
  			clock_update_grandmaster(c);
  			event = EV_RS_GRAND_MASTER;
  			break;
  		case PS_MASTER:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 1;
  			#endif
  			event = EV_RS_MASTER;
  			break;
  		case PS_PASSIVE:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 0;
  			#endif
  			event = EV_RS_PASSIVE;
  			break;
  		case PS_SLAVE:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 0;
  			#endif
  			clock_update_slave(c);
  			event = EV_RS_SLAVE;
  			break;
  		default:
  			#ifdef PTP_QUARTZ
  			timeline_master_flag[c->timelineid] = 0;
  			#endif
  			event = EV_FAULT_DETECTED;
  			break;
  		}
  		port_dispatch(piter, event, fresh_best);
  	}
  }
  
  struct clock_description *clock_description(struct clock *c)
  {
  	return &c->desc;
  }
  
  enum clock_type clock_type(struct clock *c)
  {
  	return c->type;
  }
  
  void clock_check_ts(struct clock *c, struct timespec ts)
  {
  	if (c->sanity_check &&
  	    clockcheck_sample(c->sanity_check,
  			      ts.tv_sec * NS_PER_SEC + ts.tv_nsec)) {
  		servo_reset(c->servo);
  	}
  }
  
  double clock_rate_ratio(struct clock *c)
  {
  	return servo_rate_ratio(c->servo);
  }
  