subprojects/gstreamer/libs/gst/net/gstptpclock.c

   1 /* GStreamer
   2  * Copyright (C) 2015 Sebastian Dröge <sebastian@centricular.com>
   3  *
   4  *
   5  * This library is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Library General Public
   7  * License as published by the Free Software Foundation; either
   8  * version 2 of the License, or (at your option) any later version.
   9  *
  10  * This library is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Library General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Library General Public
  16  * License along with this library; if not, write to the
  17  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
  18  * Boston, MA 02110-1301, USA.
  19  */
  20 /**
  21  * SECTION:gstptpclock
  22  * @title: GstPtpClock
  23  * @short_description: Special clock that synchronizes to a remote time
  24  *                     provider via PTP (IEEE1588:2008).
  25  * @see_also: #GstClock, #GstNetClientClock, #GstPipeline
  26  *
  27  * GstPtpClock implements a PTP (IEEE1588:2008) ordinary clock in slave-only
  28  * mode, that allows a GStreamer pipeline to synchronize to a PTP network
  29  * clock in some specific domain.
  30  *
  31  * The PTP subsystem can be initialized with gst_ptp_init(), which then starts
  32  * a helper process to do the actual communication via the PTP ports. This is
  33  * required as PTP listens on ports < 1024 and thus requires special
  34  * privileges. Once this helper process is started, the main process will
  35  * synchronize to all PTP domains that are detected on the selected
  36  * interfaces.
  37  *
  38  * gst_ptp_clock_new() then allows to create a GstClock that provides the PTP
  39  * time from a master clock inside a specific PTP domain. This clock will only
  40  * return valid timestamps once the timestamps in the PTP domain are known. To
  41  * check this, you can use gst_clock_wait_for_sync(), the GstClock::synced
  42  * signal and gst_clock_is_synced().
  43  *
  44  * To gather statistics about the PTP clock synchronization,
  45  * gst_ptp_statistics_callback_add() can be used. This gives the application
  46  * the possibility to collect all kinds of statistics from the clock
  47  * synchronization.
  48  *
  49  * Since: 1.6
  50  *
  51  */
  52 #ifdef HAVE_CONFIG_H
  53 #include "config.h"
  54 #endif
  55
  56 #include "gstptpclock.h"
  57
  58 #include "gstptp_private.h"
  59
  60 #ifdef HAVE_SYS_WAIT_H
  61 #include <sys/wait.h>
  62 #endif
  63 #ifdef G_OS_WIN32
  64 #define WIN32_LEAN_AND_MEAN
  65 #include <windows.h>
  66 #endif
  67 #include <sys/types.h>
  68
  69 #ifdef HAVE_UNISTD_H
  70 #include <unistd.h>
  71 #elif defined(G_OS_WIN32)
  72 #include <io.h>
  73 #endif
  74
  75 #include <gst/base/base.h>
  76
  77 GST_DEBUG_CATEGORY_STATIC (ptp_debug);
  78 #define GST_CAT_DEFAULT (ptp_debug)
  79
  80 /* IEEE 1588 7.7.3.1 */
  81 #define PTP_ANNOUNCE_RECEIPT_TIMEOUT 4
  82
  83 /* Use a running average for calculating the mean path delay instead
  84  * of just using the last measurement. Enabling this helps in unreliable
  85  * networks, like wifi, with often changing delays
  86  *
  87  * Undef for following IEEE1588-2008 by the letter
  88  */
  89 #define USE_RUNNING_AVERAGE_DELAY 1
  90
  91 /* Filter out any measurements that are above a certain threshold compared to
  92  * previous measurements. Enabling this helps filtering out outliers that
  93  * happen fairly often in unreliable networks, like wifi.
  94  *
  95  * Undef for following IEEE1588-2008 by the letter
  96  */
  97 #define USE_MEASUREMENT_FILTERING 1
  98
  99 /* Select the first clock from which we capture a SYNC message as the master
 100  * clock of the domain until we are ready to run the best master clock
 101  * algorithm. This allows faster syncing but might mean a change of the master
 102  * clock in the beginning. As all clocks in a domain are supposed to use the
 103  * same time, this shouldn't be much of a problem.
 104  *
 105  * Undef for following IEEE1588-2008 by the letter
 106  */
 107 #define USE_OPPORTUNISTIC_CLOCK_SELECTION 1
 108
 109 /* Only consider SYNC messages for which we are allowed to send a DELAY_REQ
 110  * afterwards. This allows better synchronization in networks with varying
 111  * delays, as for every other SYNC message we would have to assume that it's
 112  * the average of what we saw before. But that might be completely off
 113  */
 114 #define USE_ONLY_SYNC_WITH_DELAY 1
 115
 116 /* Filter out delay measurements that are too far away from the median of the
 117  * last delay measurements, currently those that are more than 2 times as big.
 118  * This increases accuracy a lot on wifi.
 119  */
 120 #define USE_MEDIAN_PRE_FILTERING 1
 121 #define MEDIAN_PRE_FILTERING_WINDOW 9
 122
 123 /* How many updates should be skipped at maximum when using USE_MEASUREMENT_FILTERING */
 124 #define MAX_SKIPPED_UPDATES 5
 125
 126 typedef enum
 127 {
 128   PTP_MESSAGE_TYPE_SYNC = 0x0,
 129   PTP_MESSAGE_TYPE_DELAY_REQ = 0x1,
 130   PTP_MESSAGE_TYPE_PDELAY_REQ = 0x2,
 131   PTP_MESSAGE_TYPE_PDELAY_RESP = 0x3,
 132   PTP_MESSAGE_TYPE_FOLLOW_UP = 0x8,
 133   PTP_MESSAGE_TYPE_DELAY_RESP = 0x9,
 134   PTP_MESSAGE_TYPE_PDELAY_RESP_FOLLOW_UP = 0xA,
 135   PTP_MESSAGE_TYPE_ANNOUNCE = 0xB,
 136   PTP_MESSAGE_TYPE_SIGNALING = 0xC,
 137   PTP_MESSAGE_TYPE_MANAGEMENT = 0xD
 138 } PtpMessageType;
 139
 140 typedef struct
 141 {
 142   guint64 seconds_field;        /* 48 bits valid */
 143   guint32 nanoseconds_field;
 144 } PtpTimestamp;
 145
 146 #define PTP_TIMESTAMP_TO_GST_CLOCK_TIME(ptp) (ptp.seconds_field * GST_SECOND + ptp.nanoseconds_field)
 147 #define GST_CLOCK_TIME_TO_PTP_TIMESTAMP_SECONDS(gst) (((GstClockTime) gst) / GST_SECOND)
 148 #define GST_CLOCK_TIME_TO_PTP_TIMESTAMP_NANOSECONDS(gst) (((GstClockTime) gst) % GST_SECOND)
 149
 150 typedef struct
 151 {
 152   guint64 clock_identity;
 153   guint16 port_number;
 154 } PtpClockIdentity;
 155
 156 static gint
 157 compare_clock_identity (const PtpClockIdentity * a, const PtpClockIdentity * b)
 158 {
 159   if (a->clock_identity < b->clock_identity)
 160     return -1;
 161   else if (a->clock_identity > b->clock_identity)
 162     return 1;
 163
 164   if (a->port_number < b->port_number)
 165     return -1;
 166   else if (a->port_number > b->port_number)
 167     return 1;
 168
 169   return 0;
 170 }
 171
 172 typedef struct
 173 {
 174   guint8 clock_class;
 175   guint8 clock_accuracy;
 176   guint16 offset_scaled_log_variance;
 177 } PtpClockQuality;
 178
 179 typedef struct
 180 {
 181   guint8 transport_specific;
 182   PtpMessageType message_type;
 183   /* guint8 reserved; */
 184   guint8 version_ptp;
 185   guint16 message_length;
 186   guint8 domain_number;
 187   /* guint8 reserved; */
 188   guint16 flag_field;
 189   gint64 correction_field;      /* 48.16 fixed point nanoseconds */
 190   /* guint32 reserved; */
 191   PtpClockIdentity source_port_identity;
 192   guint16 sequence_id;
 193   guint8 control_field;
 194   gint8 log_message_interval;
 195
 196   union
 197   {
 198     struct
 199     {
 200       PtpTimestamp origin_timestamp;
 201       gint16 current_utc_offset;
 202       /* guint8 reserved; */
 203       guint8 grandmaster_priority_1;
 204       PtpClockQuality grandmaster_clock_quality;
 205       guint8 grandmaster_priority_2;
 206       guint64 grandmaster_identity;
 207       guint16 steps_removed;
 208       guint8 time_source;
 209     } announce;
 210
 211     struct
 212     {
 213       PtpTimestamp origin_timestamp;
 214     } sync;
 215
 216     struct
 217     {
 218       PtpTimestamp precise_origin_timestamp;
 219     } follow_up;
 220
 221     struct
 222     {
 223       PtpTimestamp origin_timestamp;
 224     } delay_req;
 225
 226     struct
 227     {
 228       PtpTimestamp receive_timestamp;
 229       PtpClockIdentity requesting_port_identity;
 230     } delay_resp;
 231
 232   } message_specific;
 233 } PtpMessage;
 234
 235 static GMutex ptp_lock;
 236 static GCond ptp_cond;
 237 static gboolean initted = FALSE;
 238 #ifdef HAVE_PTP
 239 static gboolean supported = TRUE;
 240 #else
 241 static gboolean supported = FALSE;
 242 #endif
 243 static GPid ptp_helper_pid;
 244 static GThread *ptp_helper_thread;
 245 static GMainContext *main_context;
 246 static GMainLoop *main_loop;
 247 static GIOChannel *stdin_channel, *stdout_channel;
 248 static GRand *delay_req_rand;
 249 static GstClock *observation_system_clock;
 250 static PtpClockIdentity ptp_clock_id = { GST_PTP_CLOCK_ID_NONE, 0 };
 251
 252 typedef struct
 253 {
 254   GstClockTime receive_time;
 255
 256   PtpClockIdentity master_clock_identity;
 257
 258   guint8 grandmaster_priority_1;
 259   PtpClockQuality grandmaster_clock_quality;
 260   guint8 grandmaster_priority_2;
 261   guint64 grandmaster_identity;
 262   guint16 steps_removed;
 263   guint8 time_source;
 264
 265   guint16 sequence_id;
 266 } PtpAnnounceMessage;
 267
 268 typedef struct
 269 {
 270   PtpClockIdentity master_clock_identity;
 271
 272   GstClockTime announce_interval;       /* last interval we received */
 273   GQueue announce_messages;
 274 } PtpAnnounceSender;
 275
 276 typedef struct
 277 {
 278   guint domain;
 279   PtpClockIdentity master_clock_identity;
 280
 281   guint16 sync_seqnum;
 282   GstClockTime sync_recv_time_local;    /* t2 */
 283   GstClockTime sync_send_time_remote;   /* t1, might be -1 if FOLLOW_UP pending */
 284   GstClockTime follow_up_recv_time_local;
 285
 286   GSource *timeout_source;
 287   guint16 delay_req_seqnum;
 288   GstClockTime delay_req_send_time_local;       /* t3, -1 if we wait for FOLLOW_UP */
 289   GstClockTime delay_req_recv_time_remote;      /* t4, -1 if we wait */
 290   GstClockTime delay_resp_recv_time_local;
 291
 292   gint64 correction_field_sync; /* sum of the correction fields of SYNC/FOLLOW_UP */
 293   gint64 correction_field_delay;        /* sum of the correction fields of DELAY_RESP */
 294 } PtpPendingSync;
 295
 296 static void
 297 ptp_pending_sync_free (PtpPendingSync * sync)
 298 {
 299   if (sync->timeout_source) {
 300     g_source_destroy (sync->timeout_source);
 301     g_source_unref (sync->timeout_source);
 302   }
 303   g_free (sync);
 304 }
 305
 306 typedef struct
 307 {
 308   guint domain;
 309
 310   GstClockTime last_ptp_time;
 311   GstClockTime last_local_time;
 312   gint skipped_updates;
 313
 314   /* Used for selecting the master/grandmaster */
 315   GList *announce_senders;
 316
 317   /* Last selected master clock */
 318   gboolean have_master_clock;
 319   PtpClockIdentity master_clock_identity;
 320   guint64 grandmaster_identity;
 321
 322   /* Last SYNC or FOLLOW_UP timestamp we received */
 323   GstClockTime last_ptp_sync_time;
 324   GstClockTime sync_interval;
 325
 326   GstClockTime mean_path_delay;
 327   GstClockTime last_delay_req, min_delay_req_interval;
 328   guint16 last_delay_req_seqnum;
 329
 330   GstClockTime last_path_delays[MEDIAN_PRE_FILTERING_WINDOW];
 331   gint last_path_delays_missing;
 332
 333   GQueue pending_syncs;
 334
 335   GstClock *domain_clock;
 336 } PtpDomainData;
 337
 338 static GList *domain_data;
 339 static GMutex domain_clocks_lock;
 340 static GList *domain_clocks;
 341
 342 /* Protected by PTP lock */
 343 static void emit_ptp_statistics (guint8 domain, const GstStructure * stats);
 344 static GHookList domain_stats_hooks;
 345 static gint domain_stats_n_hooks;
 346 static gboolean domain_stats_hooks_initted = FALSE;
 347
 348 /* Converts log2 seconds to GstClockTime */
 349 static GstClockTime
 350 log2_to_clock_time (gint l)
 351 {
 352   if (l < 0)
 353     return GST_SECOND >> (-l);
 354   else
 355     return GST_SECOND << l;
 356 }
 357
 358 static void
 359 dump_ptp_message (PtpMessage * msg)
 360 {
 361   GST_TRACE ("PTP message:");
 362   GST_TRACE ("\ttransport_specific: %u", msg->transport_specific);
 363   GST_TRACE ("\tmessage_type: 0x%01x", msg->message_type);
 364   GST_TRACE ("\tversion_ptp: %u", msg->version_ptp);
 365   GST_TRACE ("\tmessage_length: %u", msg->message_length);
 366   GST_TRACE ("\tdomain_number: %u", msg->domain_number);
 367   GST_TRACE ("\tflag_field: 0x%04x", msg->flag_field);
 368   GST_TRACE ("\tcorrection_field: %" G_GINT64_FORMAT ".%03u",
 369       (msg->correction_field / 65536),
 370       (guint) ((msg->correction_field & 0xffff) * 1000) / 65536);
 371   GST_TRACE ("\tsource_port_identity: 0x%016" G_GINT64_MODIFIER "x %u",
 372       msg->source_port_identity.clock_identity,
 373       msg->source_port_identity.port_number);
 374   GST_TRACE ("\tsequence_id: %u", msg->sequence_id);
 375   GST_TRACE ("\tcontrol_field: 0x%02x", msg->control_field);
 376   GST_TRACE ("\tmessage_interval: %" GST_TIME_FORMAT,
 377       GST_TIME_ARGS (log2_to_clock_time (msg->log_message_interval)));
 378
 379   switch (msg->message_type) {
 380     case PTP_MESSAGE_TYPE_ANNOUNCE:
 381       GST_TRACE ("\tANNOUNCE:");
 382       GST_TRACE ("\t\torigin_timestamp: %" G_GUINT64_FORMAT ".%09u",
 383           msg->message_specific.announce.origin_timestamp.seconds_field,
 384           msg->message_specific.announce.origin_timestamp.nanoseconds_field);
 385       GST_TRACE ("\t\tcurrent_utc_offset: %d",
 386           msg->message_specific.announce.current_utc_offset);
 387       GST_TRACE ("\t\tgrandmaster_priority_1: %u",
 388           msg->message_specific.announce.grandmaster_priority_1);
 389       GST_TRACE ("\t\tgrandmaster_clock_quality: 0x%02x 0x%02x %u",
 390           msg->message_specific.announce.grandmaster_clock_quality.clock_class,
 391           msg->message_specific.announce.
 392           grandmaster_clock_quality.clock_accuracy,
 393           msg->message_specific.announce.
 394           grandmaster_clock_quality.offset_scaled_log_variance);
 395       GST_TRACE ("\t\tgrandmaster_priority_2: %u",
 396           msg->message_specific.announce.grandmaster_priority_2);
 397       GST_TRACE ("\t\tgrandmaster_identity: 0x%016" G_GINT64_MODIFIER "x",
 398           msg->message_specific.announce.grandmaster_identity);
 399       GST_TRACE ("\t\tsteps_removed: %u",
 400           msg->message_specific.announce.steps_removed);
 401       GST_TRACE ("\t\ttime_source: 0x%02x",
 402           msg->message_specific.announce.time_source);
 403       break;
 404     case PTP_MESSAGE_TYPE_SYNC:
 405       GST_TRACE ("\tSYNC:");
 406       GST_TRACE ("\t\torigin_timestamp: %" G_GUINT64_FORMAT ".%09u",
 407           msg->message_specific.sync.origin_timestamp.seconds_field,
 408           msg->message_specific.sync.origin_timestamp.nanoseconds_field);
 409       break;
 410     case PTP_MESSAGE_TYPE_FOLLOW_UP:
 411       GST_TRACE ("\tFOLLOW_UP:");
 412       GST_TRACE ("\t\tprecise_origin_timestamp: %" G_GUINT64_FORMAT ".%09u",
 413           msg->message_specific.follow_up.
 414           precise_origin_timestamp.seconds_field,
 415           msg->message_specific.follow_up.
 416           precise_origin_timestamp.nanoseconds_field);
 417       break;
 418     case PTP_MESSAGE_TYPE_DELAY_REQ:
 419       GST_TRACE ("\tDELAY_REQ:");
 420       GST_TRACE ("\t\torigin_timestamp: %" G_GUINT64_FORMAT ".%09u",
 421           msg->message_specific.delay_req.origin_timestamp.seconds_field,
 422           msg->message_specific.delay_req.origin_timestamp.nanoseconds_field);
 423       break;
 424     case PTP_MESSAGE_TYPE_DELAY_RESP:
 425       GST_TRACE ("\tDELAY_RESP:");
 426       GST_TRACE ("\t\treceive_timestamp: %" G_GUINT64_FORMAT ".%09u",
 427           msg->message_specific.delay_resp.receive_timestamp.seconds_field,
 428           msg->message_specific.delay_resp.receive_timestamp.nanoseconds_field);
 429       GST_TRACE ("\t\trequesting_port_identity: 0x%016" G_GINT64_MODIFIER
 430           "x %u",
 431           msg->message_specific.delay_resp.
 432           requesting_port_identity.clock_identity,
 433           msg->message_specific.delay_resp.
 434           requesting_port_identity.port_number);
 435       break;
 436     default:
 437       break;
 438   }
 439   GST_TRACE (" ");
 440 }
 441
 442 /* IEEE 1588-2008 5.3.3 */
 443 static gboolean
 444 parse_ptp_timestamp (PtpTimestamp * timestamp, GstByteReader * reader)
 445 {
 446   g_return_val_if_fail (gst_byte_reader_get_remaining (reader) >= 10, FALSE);
 447
 448   timestamp->seconds_field =
 449       (((guint64) gst_byte_reader_get_uint32_be_unchecked (reader)) << 16) |
 450       gst_byte_reader_get_uint16_be_unchecked (reader);
 451   timestamp->nanoseconds_field =
 452       gst_byte_reader_get_uint32_be_unchecked (reader);
 453
 454   if (timestamp->nanoseconds_field >= 1000000000)
 455     return FALSE;
 456
 457   return TRUE;
 458 }
 459
 460 /* IEEE 1588-2008 13.3 */
 461 static gboolean
 462 parse_ptp_message_header (PtpMessage * msg, GstByteReader * reader)
 463 {
 464   guint8 b;
 465
 466   g_return_val_if_fail (gst_byte_reader_get_remaining (reader) >= 34, FALSE);
 467
 468   b = gst_byte_reader_get_uint8_unchecked (reader);
 469   msg->transport_specific = b >> 4;
 470   msg->message_type = b & 0x0f;
 471
 472   b = gst_byte_reader_get_uint8_unchecked (reader);
 473   msg->version_ptp = b & 0x0f;
 474   if (msg->version_ptp != 2) {
 475     GST_WARNING ("Unsupported PTP message version (%u != 2)", msg->version_ptp);
 476     return FALSE;
 477   }
 478
 479   msg->message_length = gst_byte_reader_get_uint16_be_unchecked (reader);
 480   if (gst_byte_reader_get_remaining (reader) + 4 < msg->message_length) {
 481     GST_WARNING ("Not enough data (%u < %u)",
 482         gst_byte_reader_get_remaining (reader) + 4, msg->message_length);
 483     return FALSE;
 484   }
 485
 486   msg->domain_number = gst_byte_reader_get_uint8_unchecked (reader);
 487   gst_byte_reader_skip_unchecked (reader, 1);
 488
 489   msg->flag_field = gst_byte_reader_get_uint16_be_unchecked (reader);
 490   msg->correction_field = gst_byte_reader_get_uint64_be_unchecked (reader);
 491   gst_byte_reader_skip_unchecked (reader, 4);
 492
 493   msg->source_port_identity.clock_identity =
 494       gst_byte_reader_get_uint64_be_unchecked (reader);
 495   msg->source_port_identity.port_number =
 496       gst_byte_reader_get_uint16_be_unchecked (reader);
 497
 498   msg->sequence_id = gst_byte_reader_get_uint16_be_unchecked (reader);
 499   msg->control_field = gst_byte_reader_get_uint8_unchecked (reader);
 500   msg->log_message_interval = gst_byte_reader_get_uint8_unchecked (reader);
 501
 502   return TRUE;
 503 }
 504
 505 /* IEEE 1588-2008 13.5 */
 506 static gboolean
 507 parse_ptp_message_announce (PtpMessage * msg, GstByteReader * reader)
 508 {
 509   g_return_val_if_fail (msg->message_type == PTP_MESSAGE_TYPE_ANNOUNCE, FALSE);
 510
 511   if (gst_byte_reader_get_remaining (reader) < 20)
 512     return FALSE;
 513
 514   if (!parse_ptp_timestamp (&msg->message_specific.announce.origin_timestamp,
 515           reader))
 516     return FALSE;
 517
 518   msg->message_specific.announce.current_utc_offset =
 519       gst_byte_reader_get_uint16_be_unchecked (reader);
 520   gst_byte_reader_skip_unchecked (reader, 1);
 521
 522   msg->message_specific.announce.grandmaster_priority_1 =
 523       gst_byte_reader_get_uint8_unchecked (reader);
 524   msg->message_specific.announce.grandmaster_clock_quality.clock_class =
 525       gst_byte_reader_get_uint8_unchecked (reader);
 526   msg->message_specific.announce.grandmaster_clock_quality.clock_accuracy =
 527       gst_byte_reader_get_uint8_unchecked (reader);
 528   msg->message_specific.announce.
 529       grandmaster_clock_quality.offset_scaled_log_variance =
 530       gst_byte_reader_get_uint16_be_unchecked (reader);
 531   msg->message_specific.announce.grandmaster_priority_2 =
 532       gst_byte_reader_get_uint8_unchecked (reader);
 533   msg->message_specific.announce.grandmaster_identity =
 534       gst_byte_reader_get_uint64_be_unchecked (reader);
 535   msg->message_specific.announce.steps_removed =
 536       gst_byte_reader_get_uint16_be_unchecked (reader);
 537   msg->message_specific.announce.time_source =
 538       gst_byte_reader_get_uint8_unchecked (reader);
 539
 540   return TRUE;
 541 }
 542
 543 /* IEEE 1588-2008 13.6 */
 544 static gboolean
 545 parse_ptp_message_sync (PtpMessage * msg, GstByteReader * reader)
 546 {
 547   g_return_val_if_fail (msg->message_type == PTP_MESSAGE_TYPE_SYNC, FALSE);
 548
 549   if (gst_byte_reader_get_remaining (reader) < 10)
 550     return FALSE;
 551
 552   if (!parse_ptp_timestamp (&msg->message_specific.sync.origin_timestamp,
 553           reader))
 554     return FALSE;
 555
 556   return TRUE;
 557 }
 558
 559 /* IEEE 1588-2008 13.6 */
 560 static gboolean
 561 parse_ptp_message_delay_req (PtpMessage * msg, GstByteReader * reader)
 562 {
 563   g_return_val_if_fail (msg->message_type == PTP_MESSAGE_TYPE_DELAY_REQ, FALSE);
 564
 565   if (gst_byte_reader_get_remaining (reader) < 10)
 566     return FALSE;
 567
 568   if (!parse_ptp_timestamp (&msg->message_specific.delay_req.origin_timestamp,
 569           reader))
 570     return FALSE;
 571
 572   return TRUE;
 573 }
 574
 575 /* IEEE 1588-2008 13.7 */
 576 static gboolean
 577 parse_ptp_message_follow_up (PtpMessage * msg, GstByteReader * reader)
 578 {
 579   g_return_val_if_fail (msg->message_type == PTP_MESSAGE_TYPE_FOLLOW_UP, FALSE);
 580
 581   if (gst_byte_reader_get_remaining (reader) < 10)
 582     return FALSE;
 583
 584   if (!parse_ptp_timestamp (&msg->message_specific.
 585           follow_up.precise_origin_timestamp, reader))
 586     return FALSE;
 587
 588   return TRUE;
 589 }
 590
 591 /* IEEE 1588-2008 13.8 */
 592 static gboolean
 593 parse_ptp_message_delay_resp (PtpMessage * msg, GstByteReader * reader)
 594 {
 595   g_return_val_if_fail (msg->message_type == PTP_MESSAGE_TYPE_DELAY_RESP,
 596       FALSE);
 597
 598   if (gst_byte_reader_get_remaining (reader) < 20)
 599     return FALSE;
 600
 601   if (!parse_ptp_timestamp (&msg->message_specific.delay_resp.receive_timestamp,
 602           reader))
 603     return FALSE;
 604
 605   msg->message_specific.delay_resp.requesting_port_identity.clock_identity =
 606       gst_byte_reader_get_uint64_be_unchecked (reader);
 607   msg->message_specific.delay_resp.requesting_port_identity.port_number =
 608       gst_byte_reader_get_uint16_be_unchecked (reader);
 609
 610   return TRUE;
 611 }
 612
 613 static gboolean
 614 parse_ptp_message (PtpMessage * msg, const guint8 * data, gsize size)
 615 {
 616   GstByteReader reader;
 617   gboolean ret = FALSE;
 618
 619   gst_byte_reader_init (&reader, data, size);
 620
 621   if (!parse_ptp_message_header (msg, &reader)) {
 622     GST_WARNING ("Failed to parse PTP message header");
 623     return FALSE;
 624   }
 625
 626   switch (msg->message_type) {
 627     case PTP_MESSAGE_TYPE_SYNC:
 628       ret = parse_ptp_message_sync (msg, &reader);
 629       break;
 630     case PTP_MESSAGE_TYPE_FOLLOW_UP:
 631       ret = parse_ptp_message_follow_up (msg, &reader);
 632       break;
 633     case PTP_MESSAGE_TYPE_DELAY_REQ:
 634       ret = parse_ptp_message_delay_req (msg, &reader);
 635       break;
 636     case PTP_MESSAGE_TYPE_DELAY_RESP:
 637       ret = parse_ptp_message_delay_resp (msg, &reader);
 638       break;
 639     case PTP_MESSAGE_TYPE_ANNOUNCE:
 640       ret = parse_ptp_message_announce (msg, &reader);
 641       break;
 642     default:
 643       /* ignore for now */
 644       break;
 645   }
 646
 647   return ret;
 648 }
 649
 650 static gint
 651 compare_announce_message (const PtpAnnounceMessage * a,
 652     const PtpAnnounceMessage * b)
 653 {
 654   /* IEEE 1588 Figure 27 */
 655   if (a->grandmaster_identity == b->grandmaster_identity) {
 656     if (a->steps_removed + 1 < b->steps_removed)
 657       return -1;
 658     else if (a->steps_removed > b->steps_removed + 1)
 659       return 1;
 660
 661     /* Error cases are filtered out earlier */
 662     if (a->steps_removed < b->steps_removed)
 663       return -1;
 664     else if (a->steps_removed > b->steps_removed)
 665       return 1;
 666
 667     /* Error cases are filtered out earlier */
 668     if (a->master_clock_identity.clock_identity <
 669         b->master_clock_identity.clock_identity)
 670       return -1;
 671     else if (a->master_clock_identity.clock_identity >
 672         b->master_clock_identity.clock_identity)
 673       return 1;
 674
 675     /* Error cases are filtered out earlier */
 676     if (a->master_clock_identity.port_number <
 677         b->master_clock_identity.port_number)
 678       return -1;
 679     else if (a->master_clock_identity.port_number >
 680         b->master_clock_identity.port_number)
 681       return 1;
 682     else
 683       g_assert_not_reached ();
 684
 685     return 0;
 686   }
 687
 688   if (a->grandmaster_priority_1 < b->grandmaster_priority_1)
 689     return -1;
 690   else if (a->grandmaster_priority_1 > b->grandmaster_priority_1)
 691     return 1;
 692
 693   if (a->grandmaster_clock_quality.clock_class <
 694       b->grandmaster_clock_quality.clock_class)
 695     return -1;
 696   else if (a->grandmaster_clock_quality.clock_class >
 697       b->grandmaster_clock_quality.clock_class)
 698     return 1;
 699
 700   if (a->grandmaster_clock_quality.clock_accuracy <
 701       b->grandmaster_clock_quality.clock_accuracy)
 702     return -1;
 703   else if (a->grandmaster_clock_quality.clock_accuracy >
 704       b->grandmaster_clock_quality.clock_accuracy)
 705     return 1;
 706
 707   if (a->grandmaster_clock_quality.offset_scaled_log_variance <
 708       b->grandmaster_clock_quality.offset_scaled_log_variance)
 709     return -1;
 710   else if (a->grandmaster_clock_quality.offset_scaled_log_variance >
 711       b->grandmaster_clock_quality.offset_scaled_log_variance)
 712     return 1;
 713
 714   if (a->grandmaster_priority_2 < b->grandmaster_priority_2)
 715     return -1;
 716   else if (a->grandmaster_priority_2 > b->grandmaster_priority_2)
 717     return 1;
 718
 719   if (a->grandmaster_identity < b->grandmaster_identity)
 720     return -1;
 721   else if (a->grandmaster_identity > b->grandmaster_identity)
 722     return 1;
 723   else
 724     g_assert_not_reached ();
 725
 726   return 0;
 727 }
 728
 729 static void
 730 select_best_master_clock (PtpDomainData * domain, GstClockTime now)
 731 {
 732   GList *qualified_messages = NULL;
 733   GList *l, *m;
 734   PtpAnnounceMessage *best = NULL;
 735
 736   /* IEEE 1588 9.3.2.5 */
 737   for (l = domain->announce_senders; l; l = l->next) {
 738     PtpAnnounceSender *sender = l->data;
 739     GstClockTime window = 4 * sender->announce_interval;
 740     gint count = 0;
 741
 742     for (m = sender->announce_messages.head; m; m = m->next) {
 743       PtpAnnounceMessage *msg = m->data;
 744
 745       if (now - msg->receive_time <= window)
 746         count++;
 747     }
 748
 749     /* Only include the newest message of announce senders that had at least 2
 750      * announce messages in the last 4 announce intervals. Which also means
 751      * that we wait at least 4 announce intervals before we select a master
 752      * clock. Until then we just report based on the newest SYNC we received
 753      */
 754     if (count >= 2) {
 755       qualified_messages =
 756           g_list_prepend (qualified_messages,
 757           g_queue_peek_tail (&sender->announce_messages));
 758     }
 759   }
 760
 761   if (!qualified_messages) {
 762     GST_DEBUG
 763         ("No qualified announce messages for domain %u, can't select a master clock",
 764         domain->domain);
 765     domain->have_master_clock = FALSE;
 766     return;
 767   }
 768
 769   for (l = qualified_messages; l; l = l->next) {
 770     PtpAnnounceMessage *msg = l->data;
 771
 772     if (!best || compare_announce_message (msg, best) < 0)
 773       best = msg;
 774   }
 775   g_clear_pointer (&qualified_messages, g_list_free);
 776
 777   if (domain->have_master_clock
 778       && compare_clock_identity (&domain->master_clock_identity,
 779           &best->master_clock_identity) == 0) {
 780     GST_DEBUG ("Master clock in domain %u did not change", domain->domain);
 781   } else {
 782     GST_DEBUG ("Selected master clock for domain %u: 0x%016" G_GINT64_MODIFIER
 783         "x %u with grandmaster clock 0x%016" G_GINT64_MODIFIER "x",
 784         domain->domain, best->master_clock_identity.clock_identity,
 785         best->master_clock_identity.port_number, best->grandmaster_identity);
 786
 787     domain->have_master_clock = TRUE;
 788     domain->grandmaster_identity = best->grandmaster_identity;
 789
 790     /* Opportunistic master clock selection likely gave us the same master
 791      * clock before, no need to reset all statistics */
 792     if (compare_clock_identity (&domain->master_clock_identity,
 793             &best->master_clock_identity) != 0) {
 794       memcpy (&domain->master_clock_identity, &best->master_clock_identity,
 795           sizeof (PtpClockIdentity));
 796       domain->mean_path_delay = 0;
 797       domain->last_delay_req = 0;
 798       domain->last_path_delays_missing = 9;
 799       domain->min_delay_req_interval = 0;
 800       domain->sync_interval = 0;
 801       domain->last_ptp_sync_time = 0;
 802       domain->skipped_updates = 0;
 803       g_queue_foreach (&domain->pending_syncs, (GFunc) ptp_pending_sync_free,
 804           NULL);
 805       g_queue_clear (&domain->pending_syncs);
 806     }
 807
 808     if (g_atomic_int_get (&domain_stats_n_hooks)) {
 809       GstStructure *stats =
 810           gst_structure_new (GST_PTP_STATISTICS_BEST_MASTER_CLOCK_SELECTED,
 811           "domain", G_TYPE_UINT, domain->domain,
 812           "master-clock-id", G_TYPE_UINT64,
 813           domain->master_clock_identity.clock_identity,
 814           "master-clock-port", G_TYPE_UINT,
 815           domain->master_clock_identity.port_number,
 816           "grandmaster-clock-id", G_TYPE_UINT64, domain->grandmaster_identity,
 817           NULL);
 818       emit_ptp_statistics (domain->domain, stats);
 819       gst_structure_free (stats);
 820     }
 821   }
 822 }
 823
 824 static void
 825 handle_announce_message (PtpMessage * msg, GstClockTime receive_time)
 826 {
 827   GList *l;
 828   PtpDomainData *domain = NULL;
 829   PtpAnnounceSender *sender = NULL;
 830   PtpAnnounceMessage *announce;
 831
 832   /* IEEE1588 9.3.2.2 e)
 833    * Don't consider messages with the alternate master flag set
 834    */
 835   if ((msg->flag_field & 0x0100))
 836     return;
 837
 838   /* IEEE 1588 9.3.2.5 d)
 839    * Don't consider announce messages with steps_removed>=255
 840    */
 841   if (msg->message_specific.announce.steps_removed >= 255)
 842     return;
 843
 844   for (l = domain_data; l; l = l->next) {
 845     PtpDomainData *tmp = l->data;
 846
 847     if (tmp->domain == msg->domain_number) {
 848       domain = tmp;
 849       break;
 850     }
 851   }
 852
 853   if (!domain) {
 854     gchar *clock_name;
 855
 856     domain = g_new0 (PtpDomainData, 1);
 857     domain->domain = msg->domain_number;
 858     clock_name = g_strdup_printf ("ptp-clock-%u", domain->domain);
 859     domain->domain_clock =
 860         g_object_new (GST_TYPE_SYSTEM_CLOCK, "name", clock_name, NULL);
 861     gst_object_ref_sink (domain->domain_clock);
 862     g_free (clock_name);
 863     g_queue_init (&domain->pending_syncs);
 864     domain->last_path_delays_missing = 9;
 865     domain_data = g_list_prepend (domain_data, domain);
 866
 867     g_mutex_lock (&domain_clocks_lock);
 868     domain_clocks = g_list_prepend (domain_clocks, domain);
 869     g_mutex_unlock (&domain_clocks_lock);
 870
 871     if (g_atomic_int_get (&domain_stats_n_hooks)) {
 872       GstStructure *stats =
 873           gst_structure_new (GST_PTP_STATISTICS_NEW_DOMAIN_FOUND, "domain",
 874           G_TYPE_UINT, domain->domain, "clock", GST_TYPE_CLOCK,
 875           domain->domain_clock, NULL);
 876       emit_ptp_statistics (domain->domain, stats);
 877       gst_structure_free (stats);
 878     }
 879   }
 880
 881   for (l = domain->announce_senders; l; l = l->next) {
 882     PtpAnnounceSender *tmp = l->data;
 883
 884     if (compare_clock_identity (&tmp->master_clock_identity,
 885             &msg->source_port_identity) == 0) {
 886       sender = tmp;
 887       break;
 888     }
 889   }
 890
 891   if (!sender) {
 892     sender = g_new0 (PtpAnnounceSender, 1);
 893
 894     memcpy (&sender->master_clock_identity, &msg->source_port_identity,
 895         sizeof (PtpClockIdentity));
 896     g_queue_init (&sender->announce_messages);
 897     domain->announce_senders =
 898         g_list_prepend (domain->announce_senders, sender);
 899   }
 900
 901   for (l = sender->announce_messages.head; l; l = l->next) {
 902     PtpAnnounceMessage *tmp = l->data;
 903
 904     /* IEEE 1588 9.3.2.5 c)
 905      * Don't consider identical messages, i.e. duplicates
 906      */
 907     if (tmp->sequence_id == msg->sequence_id)
 908       return;
 909   }
 910
 911   sender->announce_interval = log2_to_clock_time (msg->log_message_interval);
 912
 913   announce = g_new0 (PtpAnnounceMessage, 1);
 914   announce->receive_time = receive_time;
 915   announce->sequence_id = msg->sequence_id;
 916   memcpy (&announce->master_clock_identity, &msg->source_port_identity,
 917       sizeof (PtpClockIdentity));
 918   announce->grandmaster_identity =
 919       msg->message_specific.announce.grandmaster_identity;
 920   announce->grandmaster_priority_1 =
 921       msg->message_specific.announce.grandmaster_priority_1;
 922   announce->grandmaster_clock_quality.clock_class =
 923       msg->message_specific.announce.grandmaster_clock_quality.clock_class;
 924   announce->grandmaster_clock_quality.clock_accuracy =
 925       msg->message_specific.announce.grandmaster_clock_quality.clock_accuracy;
 926   announce->grandmaster_clock_quality.offset_scaled_log_variance =
 927       msg->message_specific.announce.
 928       grandmaster_clock_quality.offset_scaled_log_variance;
 929   announce->grandmaster_priority_2 =
 930       msg->message_specific.announce.grandmaster_priority_2;
 931   announce->steps_removed = msg->message_specific.announce.steps_removed;
 932   announce->time_source = msg->message_specific.announce.time_source;
 933   g_queue_push_tail (&sender->announce_messages, announce);
 934
 935   select_best_master_clock (domain, receive_time);
 936 }
 937
 938 static gboolean
 939 send_delay_req_timeout (PtpPendingSync * sync)
 940 {
 941   StdIOHeader header = { 0, };
 942   guint8 delay_req[44];
 943   GstByteWriter writer;
 944   GIOStatus status;
 945   gsize written;
 946   GError *err = NULL;
 947
 948   header.type = TYPE_EVENT;
 949   header.size = 44;
 950
 951   GST_TRACE ("Sending delay_req to domain %u", sync->domain);
 952
 953   gst_byte_writer_init_with_data (&writer, delay_req, 44, FALSE);
 954   gst_byte_writer_put_uint8_unchecked (&writer, PTP_MESSAGE_TYPE_DELAY_REQ);
 955   gst_byte_writer_put_uint8_unchecked (&writer, 2);
 956   gst_byte_writer_put_uint16_be_unchecked (&writer, 44);
 957   gst_byte_writer_put_uint8_unchecked (&writer, sync->domain);
 958   gst_byte_writer_put_uint8_unchecked (&writer, 0);
 959   gst_byte_writer_put_uint16_be_unchecked (&writer, 0);
 960   gst_byte_writer_put_uint64_be_unchecked (&writer, 0);
 961   gst_byte_writer_put_uint32_be_unchecked (&writer, 0);
 962   gst_byte_writer_put_uint64_be_unchecked (&writer,
 963       ptp_clock_id.clock_identity);
 964   gst_byte_writer_put_uint16_be_unchecked (&writer, ptp_clock_id.port_number);
 965   gst_byte_writer_put_uint16_be_unchecked (&writer, sync->delay_req_seqnum);
 966   gst_byte_writer_put_uint8_unchecked (&writer, 0x01);
 967   gst_byte_writer_put_uint8_unchecked (&writer, 0x7f);
 968   gst_byte_writer_put_uint64_be_unchecked (&writer, 0);
 969   gst_byte_writer_put_uint16_be_unchecked (&writer, 0);
 970
 971   status =
 972       g_io_channel_write_chars (stdout_channel, (gchar *) & header,
 973       sizeof (header), &written, &err);
 974   if (status == G_IO_STATUS_ERROR) {
 975     g_warning ("Failed to write to stdout: %s", err->message);
 976     g_clear_error (&err);
 977     return G_SOURCE_REMOVE;
 978   } else if (status == G_IO_STATUS_EOF) {
 979     g_message ("EOF on stdout");
 980     g_main_loop_quit (main_loop);
 981     return G_SOURCE_REMOVE;
 982   } else if (status != G_IO_STATUS_NORMAL) {
 983     g_warning ("Unexpected stdout write status: %d", status);
 984     g_main_loop_quit (main_loop);
 985     return G_SOURCE_REMOVE;
 986   } else if (written != sizeof (header)) {
 987     g_warning ("Unexpected write size: %" G_GSIZE_FORMAT, written);
 988     g_main_loop_quit (main_loop);
 989     return G_SOURCE_REMOVE;
 990   }
 991
 992   sync->delay_req_send_time_local =
 993       gst_clock_get_time (observation_system_clock);
 994
 995   status =
 996       g_io_channel_write_chars (stdout_channel,
 997       (const gchar *) delay_req, 44, &written, &err);
 998   if (status == G_IO_STATUS_ERROR) {
 999     g_warning ("Failed to write to stdout: %s", err->message);
1000     g_clear_error (&err);
1001     g_main_loop_quit (main_loop);
1002     return G_SOURCE_REMOVE;
1003   } else if (status == G_IO_STATUS_EOF) {
1004     g_message ("EOF on stdout");
1005     g_main_loop_quit (main_loop);
1006     return G_SOURCE_REMOVE;
1007   } else if (status != G_IO_STATUS_NORMAL) {
1008     g_warning ("Unexpected stdout write status: %d", status);
1009     g_main_loop_quit (main_loop);
1010     return G_SOURCE_REMOVE;
1011   } else if (written != 44) {
1012     g_warning ("Unexpected write size: %" G_GSIZE_FORMAT, written);
1013     g_main_loop_quit (main_loop);
1014     return G_SOURCE_REMOVE;
1015   }
1016
1017   return G_SOURCE_REMOVE;
1018 }
1019
1020 static gboolean
1021 send_delay_req (PtpDomainData * domain, PtpPendingSync * sync)
1022 {
1023   GstClockTime now = gst_clock_get_time (observation_system_clock);
1024   guint timeout;
1025   GSource *timeout_source;
1026
1027   if (domain->last_delay_req != 0
1028       && domain->last_delay_req + domain->min_delay_req_interval > now) {
1029     GST_TRACE ("Too soon to send new DELAY_REQ");
1030     return FALSE;
1031   }
1032
1033   domain->last_delay_req = now;
1034   sync->delay_req_seqnum = domain->last_delay_req_seqnum++;
1035
1036   /* IEEE 1588 9.5.11.2 */
1037   if (domain->last_delay_req == 0 || domain->min_delay_req_interval == 0)
1038     timeout = 0;
1039   else
1040     timeout =
1041         g_rand_int_range (delay_req_rand, 0,
1042         (domain->min_delay_req_interval * 2) / GST_MSECOND);
1043
1044   sync->timeout_source = timeout_source = g_timeout_source_new (timeout);
1045   g_source_set_priority (timeout_source, G_PRIORITY_DEFAULT);
1046   g_source_set_callback (timeout_source, (GSourceFunc) send_delay_req_timeout,
1047       sync, NULL);
1048   g_source_attach (timeout_source, main_context);
1049
1050   return TRUE;
1051 }
1052
1053 /* Filtering of outliers for RTT and time calculations inspired
1054  * by the code from gstnetclientclock.c
1055  */
1056 static void
1057 update_ptp_time (PtpDomainData * domain, PtpPendingSync * sync)
1058 {
1059   GstClockTime internal_time, external_time, rate_num, rate_den;
1060   GstClockTime corrected_ptp_time, corrected_local_time;
1061   gdouble r_squared = 0.0;
1062   gboolean synced;
1063   GstClockTimeDiff discont = 0;
1064   GstClockTime estimated_ptp_time = GST_CLOCK_TIME_NONE;
1065 #ifdef USE_MEASUREMENT_FILTERING
1066   GstClockTime orig_internal_time, orig_external_time, orig_rate_num,
1067       orig_rate_den;
1068   GstClockTime new_estimated_ptp_time;
1069   GstClockTime max_discont, estimated_ptp_time_min, estimated_ptp_time_max;
1070   gboolean now_synced;
1071 #endif
1072 #ifdef USE_ONLY_SYNC_WITH_DELAY
1073   GstClockTime mean_path_delay;
1074 #endif
1075
1076   GST_TRACE ("Updating PTP time");
1077
1078 #ifdef USE_ONLY_SYNC_WITH_DELAY
1079   if (sync->delay_req_send_time_local == GST_CLOCK_TIME_NONE) {
1080     GST_TRACE ("Not updating - no delay_req sent");
1081     return;
1082   }
1083
1084   /* IEEE 1588 11.3 */
1085   mean_path_delay =
1086       (sync->delay_req_recv_time_remote - sync->sync_send_time_remote +
1087       sync->sync_recv_time_local - sync->delay_req_send_time_local -
1088       (sync->correction_field_sync + sync->correction_field_delay +
1089           32768) / 65536) / 2;
1090 #endif
1091
1092   /* IEEE 1588 11.2 */
1093   corrected_ptp_time =
1094       sync->sync_send_time_remote +
1095       (sync->correction_field_sync + 32768) / 65536;
1096
1097 #ifdef USE_ONLY_SYNC_WITH_DELAY
1098   corrected_local_time = sync->sync_recv_time_local - mean_path_delay;
1099 #else
1100   corrected_local_time = sync->sync_recv_time_local - domain->mean_path_delay;
1101 #endif
1102
1103 #ifdef USE_MEASUREMENT_FILTERING
1104   /* We check this here and when updating the mean path delay, because
1105    * we can get here without a delay response too. The tolerance on
1106    * accepting follow-up after a sync is high, because a PTP server
1107    * doesn't have to prioritise sending FOLLOW_UP - its purpose is
1108    * just to give us the accurate timestamp of the preceding SYNC */
1109   if (sync->follow_up_recv_time_local != GST_CLOCK_TIME_NONE
1110       && sync->follow_up_recv_time_local >
1111       sync->sync_recv_time_local + 20 * domain->mean_path_delay) {
1112     GstClockTimeDiff delay =
1113         sync->follow_up_recv_time_local - sync->sync_recv_time_local;
1114     GST_WARNING ("Sync-follow-up delay for domain %u too big: %"
1115         GST_STIME_FORMAT " > 20 * %" GST_TIME_FORMAT, domain->domain,
1116         GST_STIME_ARGS (delay), GST_TIME_ARGS (domain->mean_path_delay));
1117     synced = FALSE;
1118     gst_clock_get_calibration (GST_CLOCK_CAST (domain->domain_clock),
1119         &internal_time, &external_time, &rate_num, &rate_den);
1120     goto out;
1121   }
1122 #endif
1123
1124   /* Set an initial local-remote relation */
1125   if (domain->last_ptp_time == 0)
1126     gst_clock_set_calibration (domain->domain_clock, corrected_local_time,
1127         corrected_ptp_time, 1, 1);
1128
1129 #ifdef USE_MEASUREMENT_FILTERING
1130   /* Check if the corrected PTP time is +/- 3/4 RTT around what we would
1131    * estimate with our present knowledge about the clock
1132    */
1133   /* Store what the clock produced as 'now' before this update */
1134   gst_clock_get_calibration (GST_CLOCK_CAST (domain->domain_clock),
1135       &orig_internal_time, &orig_external_time, &orig_rate_num, &orig_rate_den);
1136   internal_time = orig_internal_time;
1137   external_time = orig_external_time;
1138   rate_num = orig_rate_num;
1139   rate_den = orig_rate_den;
1140
1141   /* 3/4 RTT window around the estimation */
1142   max_discont = domain->mean_path_delay * 3 / 2;
1143
1144   /* Check if the estimated sync time is inside our window */
1145   estimated_ptp_time_min = corrected_local_time - max_discont;
1146   estimated_ptp_time_min =
1147       gst_clock_adjust_with_calibration (GST_CLOCK_CAST (domain->domain_clock),
1148       estimated_ptp_time_min, internal_time, external_time, rate_num, rate_den);
1149   estimated_ptp_time_max = corrected_local_time + max_discont;
1150   estimated_ptp_time_max =
1151       gst_clock_adjust_with_calibration (GST_CLOCK_CAST (domain->domain_clock),
1152       estimated_ptp_time_max, internal_time, external_time, rate_num, rate_den);
1153
1154   synced = (estimated_ptp_time_min < corrected_ptp_time
1155       && corrected_ptp_time < estimated_ptp_time_max);
1156
1157   GST_DEBUG ("Adding observation for domain %u: %" GST_TIME_FORMAT " - %"
1158       GST_TIME_FORMAT, domain->domain,
1159       GST_TIME_ARGS (corrected_ptp_time), GST_TIME_ARGS (corrected_local_time));
1160
1161   GST_DEBUG ("Synced %d: %" GST_TIME_FORMAT " < %" GST_TIME_FORMAT " < %"
1162       GST_TIME_FORMAT, synced, GST_TIME_ARGS (estimated_ptp_time_min),
1163       GST_TIME_ARGS (corrected_ptp_time),
1164       GST_TIME_ARGS (estimated_ptp_time_max));
1165
1166   if (gst_clock_add_observation_unapplied (domain->domain_clock,
1167           corrected_local_time, corrected_ptp_time, &r_squared,
1168           &internal_time, &external_time, &rate_num, &rate_den)) {
1169     GST_DEBUG ("Regression gave r_squared: %f", r_squared);
1170
1171     /* Old estimated PTP time based on receive time and path delay */
1172     estimated_ptp_time = corrected_local_time;
1173     estimated_ptp_time =
1174         gst_clock_adjust_with_calibration (GST_CLOCK_CAST
1175         (domain->domain_clock), estimated_ptp_time, orig_internal_time,
1176         orig_external_time, orig_rate_num, orig_rate_den);
1177
1178     /* New estimated PTP time based on receive time and path delay */
1179     new_estimated_ptp_time = corrected_local_time;
1180     new_estimated_ptp_time =
1181         gst_clock_adjust_with_calibration (GST_CLOCK_CAST
1182         (domain->domain_clock), new_estimated_ptp_time, internal_time,
1183         external_time, rate_num, rate_den);
1184
1185     discont = GST_CLOCK_DIFF (estimated_ptp_time, new_estimated_ptp_time);
1186     if (synced && ABS (discont) > max_discont) {
1187       GstClockTimeDiff offset;
1188       GST_DEBUG ("Too large a discont %s%" GST_TIME_FORMAT
1189           ", clamping to 1/4 average RTT = %" GST_TIME_FORMAT,
1190           (discont < 0 ? "-" : ""), GST_TIME_ARGS (ABS (discont)),
1191           GST_TIME_ARGS (max_discont));
1192       if (discont > 0) {        /* Too large a forward step - add a -ve offset */
1193         offset = max_discont - discont;
1194         if (-offset > external_time)
1195           external_time = 0;
1196         else
1197           external_time += offset;
1198       } else {                  /* Too large a backward step - add a +ve offset */
1199         offset = -(max_discont + discont);
1200         external_time += offset;
1201       }
1202
1203       discont += offset;
1204     } else {
1205       GST_DEBUG ("Discont %s%" GST_TIME_FORMAT " (max: %" GST_TIME_FORMAT ")",
1206           (discont < 0 ? "-" : ""), GST_TIME_ARGS (ABS (discont)),
1207           GST_TIME_ARGS (max_discont));
1208     }
1209
1210     /* Check if the estimated sync time is now (still) inside our window */
1211     estimated_ptp_time_min = corrected_local_time - max_discont;
1212     estimated_ptp_time_min =
1213         gst_clock_adjust_with_calibration (GST_CLOCK_CAST
1214         (domain->domain_clock), estimated_ptp_time_min, internal_time,
1215         external_time, rate_num, rate_den);
1216     estimated_ptp_time_max = corrected_local_time + max_discont;
1217     estimated_ptp_time_max =
1218         gst_clock_adjust_with_calibration (GST_CLOCK_CAST
1219         (domain->domain_clock), estimated_ptp_time_max, internal_time,
1220         external_time, rate_num, rate_den);
1221
1222     now_synced = (estimated_ptp_time_min < corrected_ptp_time
1223         && corrected_ptp_time < estimated_ptp_time_max);
1224
1225     GST_DEBUG ("Now synced %d: %" GST_TIME_FORMAT " < %" GST_TIME_FORMAT " < %"
1226         GST_TIME_FORMAT, now_synced, GST_TIME_ARGS (estimated_ptp_time_min),
1227         GST_TIME_ARGS (corrected_ptp_time),
1228         GST_TIME_ARGS (estimated_ptp_time_max));
1229
1230     if (synced || now_synced || domain->skipped_updates > MAX_SKIPPED_UPDATES) {
1231       gst_clock_set_calibration (GST_CLOCK_CAST (domain->domain_clock),
1232           internal_time, external_time, rate_num, rate_den);
1233       domain->skipped_updates = 0;
1234
1235       domain->last_ptp_time = corrected_ptp_time;
1236       domain->last_local_time = corrected_local_time;
1237     } else {
1238       domain->skipped_updates++;
1239     }
1240   } else {
1241     domain->last_ptp_time = corrected_ptp_time;
1242     domain->last_local_time = corrected_local_time;
1243   }
1244
1245 #else
1246   GST_DEBUG ("Adding observation for domain %u: %" GST_TIME_FORMAT " - %"
1247       GST_TIME_FORMAT, domain->domain,
1248       GST_TIME_ARGS (corrected_ptp_time), GST_TIME_ARGS (corrected_local_time));
1249
1250   gst_clock_get_calibration (GST_CLOCK_CAST (domain->domain_clock),
1251       &internal_time, &external_time, &rate_num, &rate_den);
1252
1253   estimated_ptp_time = corrected_local_time;
1254   estimated_ptp_time =
1255       gst_clock_adjust_with_calibration (GST_CLOCK_CAST
1256       (domain->domain_clock), estimated_ptp_time, internal_time,
1257       external_time, rate_num, rate_den);
1258
1259   gst_clock_add_observation (domain->domain_clock,
1260       corrected_local_time, corrected_ptp_time, &r_squared);
1261
1262   gst_clock_get_calibration (GST_CLOCK_CAST (domain->domain_clock),
1263       &internal_time, &external_time, &rate_num, &rate_den);
1264
1265   synced = TRUE;
1266   domain->last_ptp_time = corrected_ptp_time;
1267   domain->last_local_time = corrected_local_time;
1268 #endif
1269
1270 #ifdef USE_MEASUREMENT_FILTERING
1271 out:
1272 #endif
1273   if (g_atomic_int_get (&domain_stats_n_hooks)) {
1274     GstStructure *stats = gst_structure_new (GST_PTP_STATISTICS_TIME_UPDATED,
1275         "domain", G_TYPE_UINT, domain->domain,
1276         "mean-path-delay-avg", GST_TYPE_CLOCK_TIME, domain->mean_path_delay,
1277         "local-time", GST_TYPE_CLOCK_TIME, corrected_local_time,
1278         "ptp-time", GST_TYPE_CLOCK_TIME, corrected_ptp_time,
1279         "estimated-ptp-time", GST_TYPE_CLOCK_TIME, estimated_ptp_time,
1280         "discontinuity", G_TYPE_INT64, discont,
1281         "synced", G_TYPE_BOOLEAN, synced,
1282         "r-squared", G_TYPE_DOUBLE, r_squared,
1283         "internal-time", GST_TYPE_CLOCK_TIME, internal_time,
1284         "external-time", GST_TYPE_CLOCK_TIME, external_time,
1285         "rate-num", G_TYPE_UINT64, rate_num,
1286         "rate-den", G_TYPE_UINT64, rate_den,
1287         "rate", G_TYPE_DOUBLE, (gdouble) (rate_num) / rate_den,
1288         NULL);
1289     emit_ptp_statistics (domain->domain, stats);
1290     gst_structure_free (stats);
1291   }
1292
1293 }
1294
1295 #ifdef USE_MEDIAN_PRE_FILTERING
1296 static gint
1297 compare_clock_time (const GstClockTime * a, const GstClockTime * b)
1298 {
1299   if (*a < *b)
1300     return -1;
1301   else if (*a > *b)
1302     return 1;
1303   return 0;
1304 }
1305 #endif
1306
1307 static gboolean
1308 update_mean_path_delay (PtpDomainData * domain, PtpPendingSync * sync)
1309 {
1310 #ifdef USE_MEDIAN_PRE_FILTERING
1311   GstClockTime last_path_delays[MEDIAN_PRE_FILTERING_WINDOW];
1312   GstClockTime median;
1313   gint i;
1314 #endif
1315
1316   GstClockTime mean_path_delay, delay_req_delay = 0;
1317   gboolean ret;
1318
1319   /* IEEE 1588 11.3 */
1320   mean_path_delay =
1321       (sync->delay_req_recv_time_remote - sync->sync_send_time_remote +
1322       sync->sync_recv_time_local - sync->delay_req_send_time_local -
1323       (sync->correction_field_sync + sync->correction_field_delay +
1324           32768) / 65536) / 2;
1325
1326 #ifdef USE_MEDIAN_PRE_FILTERING
1327   for (i = 1; i < MEDIAN_PRE_FILTERING_WINDOW; i++)
1328     domain->last_path_delays[i - 1] = domain->last_path_delays[i];
1329   domain->last_path_delays[i - 1] = mean_path_delay;
1330
1331   if (domain->last_path_delays_missing) {
1332     domain->last_path_delays_missing--;
1333   } else {
1334     memcpy (&last_path_delays, &domain->last_path_delays,
1335         sizeof (last_path_delays));
1336     g_qsort_with_data (&last_path_delays,
1337         MEDIAN_PRE_FILTERING_WINDOW, sizeof (GstClockTime),
1338         (GCompareDataFunc) compare_clock_time, NULL);
1339
1340     median = last_path_delays[MEDIAN_PRE_FILTERING_WINDOW / 2];
1341
1342     /* FIXME: We might want to use something else here, like only allowing
1343      * things in the interquartile range, or also filtering away delays that
1344      * are too small compared to the median. This here worked well enough
1345      * in tests so far.
1346      */
1347     if (mean_path_delay > 2 * median) {
1348       GST_WARNING ("Path delay for domain %u too big compared to median: %"
1349           GST_TIME_FORMAT " > 2 * %" GST_TIME_FORMAT, domain->domain,
1350           GST_TIME_ARGS (mean_path_delay), GST_TIME_ARGS (median));
1351       ret = FALSE;
1352       goto out;
1353     }
1354   }
1355 #endif
1356
1357 #ifdef USE_RUNNING_AVERAGE_DELAY
1358   /* Track an average round trip time, for a bit of smoothing */
1359   /* Always update before discarding a sample, so genuine changes in
1360    * the network get picked up, eventually */
1361   if (domain->mean_path_delay == 0)
1362     domain->mean_path_delay = mean_path_delay;
1363   else if (mean_path_delay < domain->mean_path_delay)   /* Shorter RTTs carry more weight than longer */
1364     domain->mean_path_delay =
1365         (3 * domain->mean_path_delay + mean_path_delay) / 4;
1366   else
1367     domain->mean_path_delay =
1368         (15 * domain->mean_path_delay + mean_path_delay) / 16;
1369 #else
1370   domain->mean_path_delay = mean_path_delay;
1371 #endif
1372
1373 #ifdef USE_MEASUREMENT_FILTERING
1374   /* The tolerance on accepting follow-up after a sync is high, because
1375    * a PTP server doesn't have to prioritise sending FOLLOW_UP - its purpose is
1376    * just to give us the accurate timestamp of the preceding SYNC */
1377   if (sync->follow_up_recv_time_local != GST_CLOCK_TIME_NONE &&
1378       domain->mean_path_delay != 0
1379       && sync->follow_up_recv_time_local >
1380       sync->sync_recv_time_local + 20 * domain->mean_path_delay) {
1381     GST_WARNING ("Sync-follow-up delay for domain %u too big: %" GST_TIME_FORMAT
1382         " > 20 * %" GST_TIME_FORMAT, domain->domain,
1383         GST_TIME_ARGS (sync->follow_up_recv_time_local -
1384             sync->sync_recv_time_local),
1385         GST_TIME_ARGS (domain->mean_path_delay));
1386     ret = FALSE;
1387     goto out;
1388   }
1389
1390   if (mean_path_delay > 2 * domain->mean_path_delay) {
1391     GST_WARNING ("Mean path delay for domain %u too big: %" GST_TIME_FORMAT
1392         " > 2 * %" GST_TIME_FORMAT, domain->domain,
1393         GST_TIME_ARGS (mean_path_delay),
1394         GST_TIME_ARGS (domain->mean_path_delay));
1395     ret = FALSE;
1396     goto out;
1397   }
1398 #endif
1399
1400   delay_req_delay =
1401       sync->delay_resp_recv_time_local - sync->delay_req_send_time_local;
1402
1403 #ifdef USE_MEASUREMENT_FILTERING
1404   /* delay_req_delay is a RTT, so 2 times the path delay is what we'd
1405    * hope for, but some PTP systems don't prioritise sending DELAY_RESP,
1406    * but they must still have placed an accurate reception timestamp.
1407    * That means we should be quite tolerant about late DELAY_RESP, and
1408    * mostly rely on filtering out jumps in the mean-path-delay elsewhere  */
1409   if (delay_req_delay > 20 * domain->mean_path_delay) {
1410     GST_WARNING ("Delay-request-response delay for domain %u too big: %"
1411         GST_TIME_FORMAT " > 20 * %" GST_TIME_FORMAT, domain->domain,
1412         GST_TIME_ARGS (delay_req_delay),
1413         GST_TIME_ARGS (domain->mean_path_delay));
1414     ret = FALSE;
1415     goto out;
1416   }
1417 #endif
1418
1419   ret = TRUE;
1420
1421   GST_DEBUG ("Got mean path delay for domain %u: %" GST_TIME_FORMAT " (new: %"
1422       GST_TIME_FORMAT ")", domain->domain,
1423       GST_TIME_ARGS (domain->mean_path_delay), GST_TIME_ARGS (mean_path_delay));
1424   GST_DEBUG ("Delay request delay for domain %u: %" GST_TIME_FORMAT,
1425       domain->domain, GST_TIME_ARGS (delay_req_delay));
1426
1427 #if defined(USE_MEASUREMENT_FILTERING) || defined(USE_MEDIAN_PRE_FILTERING)
1428 out:
1429 #endif
1430   if (g_atomic_int_get (&domain_stats_n_hooks)) {
1431     GstStructure *stats =
1432         gst_structure_new (GST_PTP_STATISTICS_PATH_DELAY_MEASURED,
1433         "domain", G_TYPE_UINT, domain->domain,
1434         "mean-path-delay-avg", GST_TYPE_CLOCK_TIME, domain->mean_path_delay,
1435         "mean-path-delay", GST_TYPE_CLOCK_TIME, mean_path_delay,
1436         "delay-request-delay", GST_TYPE_CLOCK_TIME, delay_req_delay, NULL);
1437     emit_ptp_statistics (domain->domain, stats);
1438     gst_structure_free (stats);
1439   }
1440
1441   return ret;
1442 }
1443
1444 static void
1445 handle_sync_message (PtpMessage * msg, GstClockTime receive_time)
1446 {
1447   GList *l;
1448   PtpDomainData *domain = NULL;
1449   PtpPendingSync *sync = NULL;
1450
1451   /* Don't consider messages with the alternate master flag set */
1452   if ((msg->flag_field & 0x0100)) {
1453     GST_TRACE ("Ignoring sync message with alternate-master flag");
1454     return;
1455   }
1456
1457   for (l = domain_data; l; l = l->next) {
1458     PtpDomainData *tmp = l->data;
1459
1460     if (msg->domain_number == tmp->domain) {
1461       domain = tmp;
1462       break;
1463     }
1464   }
1465
1466   if (!domain) {
1467     gchar *clock_name;
1468
1469     domain = g_new0 (PtpDomainData, 1);
1470     domain->domain = msg->domain_number;
1471     clock_name = g_strdup_printf ("ptp-clock-%u", domain->domain);
1472     domain->domain_clock =
1473         g_object_new (GST_TYPE_SYSTEM_CLOCK, "name", clock_name, NULL);
1474     gst_object_ref_sink (domain->domain_clock);
1475     g_free (clock_name);
1476     g_queue_init (&domain->pending_syncs);
1477     domain->last_path_delays_missing = 9;
1478     domain_data = g_list_prepend (domain_data, domain);
1479
1480     g_mutex_lock (&domain_clocks_lock);
1481     domain_clocks = g_list_prepend (domain_clocks, domain);
1482     g_mutex_unlock (&domain_clocks_lock);
1483   }
1484
1485   /* If we have a master clock, ignore this message if it's not coming from there */
1486   if (domain->have_master_clock
1487       && compare_clock_identity (&domain->master_clock_identity,
1488           &msg->source_port_identity) != 0)
1489     return;
1490
1491 #ifdef USE_OPPORTUNISTIC_CLOCK_SELECTION
1492   /* Opportunistic selection of master clock */
1493   if (!domain->have_master_clock)
1494     memcpy (&domain->master_clock_identity, &msg->source_port_identity,
1495         sizeof (PtpClockIdentity));
1496 #else
1497   if (!domain->have_master_clock)
1498     return;
1499 #endif
1500
1501   domain->sync_interval = log2_to_clock_time (msg->log_message_interval);
1502
1503   /* Check if duplicated */
1504   for (l = domain->pending_syncs.head; l; l = l->next) {
1505     PtpPendingSync *tmp = l->data;
1506
1507     if (tmp->sync_seqnum == msg->sequence_id)
1508       return;
1509   }
1510
1511   if (msg->message_specific.sync.origin_timestamp.seconds_field >
1512       GST_CLOCK_TIME_NONE / GST_SECOND) {
1513     GST_FIXME ("Unsupported sync message seconds field value: %"
1514         G_GUINT64_FORMAT " > %" G_GUINT64_FORMAT,
1515         msg->message_specific.sync.origin_timestamp.seconds_field,
1516         GST_CLOCK_TIME_NONE / GST_SECOND);
1517     return;
1518   }
1519
1520   sync = g_new0 (PtpPendingSync, 1);
1521   sync->domain = domain->domain;
1522   sync->sync_seqnum = msg->sequence_id;
1523   sync->sync_recv_time_local = receive_time;
1524   sync->sync_send_time_remote = GST_CLOCK_TIME_NONE;
1525   sync->follow_up_recv_time_local = GST_CLOCK_TIME_NONE;
1526   sync->delay_req_send_time_local = GST_CLOCK_TIME_NONE;
1527   sync->delay_req_recv_time_remote = GST_CLOCK_TIME_NONE;
1528   sync->delay_resp_recv_time_local = GST_CLOCK_TIME_NONE;
1529
1530   /* 0.5 correction factor for division later */
1531   sync->correction_field_sync = msg->correction_field;
1532
1533   if ((msg->flag_field & 0x0200)) {
1534     /* Wait for FOLLOW_UP */
1535     GST_TRACE ("Waiting for FOLLOW_UP msg");
1536   } else {
1537     sync->sync_send_time_remote =
1538         PTP_TIMESTAMP_TO_GST_CLOCK_TIME (msg->message_specific.
1539         sync.origin_timestamp);
1540
1541     if (domain->last_ptp_sync_time != 0
1542         && domain->last_ptp_sync_time >= sync->sync_send_time_remote) {
1543       GST_WARNING ("Backwards PTP times in domain %u: %" GST_TIME_FORMAT " >= %"
1544           GST_TIME_FORMAT, domain->domain,
1545           GST_TIME_ARGS (domain->last_ptp_sync_time),
1546           GST_TIME_ARGS (sync->sync_send_time_remote));
1547       ptp_pending_sync_free (sync);
1548       sync = NULL;
1549       return;
1550     }
1551     domain->last_ptp_sync_time = sync->sync_send_time_remote;
1552
1553     if (send_delay_req (domain, sync)) {
1554       /* Sent delay request */
1555     } else {
1556       update_ptp_time (domain, sync);
1557       ptp_pending_sync_free (sync);
1558       sync = NULL;
1559     }
1560   }
1561
1562   if (sync)
1563     g_queue_push_tail (&domain->pending_syncs, sync);
1564 }
1565
1566 static void
1567 handle_follow_up_message (PtpMessage * msg, GstClockTime receive_time)
1568 {
1569   GList *l;
1570   PtpDomainData *domain = NULL;
1571   PtpPendingSync *sync = NULL;
1572
1573   GST_TRACE ("Processing FOLLOW_UP message");
1574
1575   /* Don't consider messages with the alternate master flag set */
1576   if ((msg->flag_field & 0x0100)) {
1577     GST_TRACE ("Ignoring FOLLOW_UP with alternate-master flag");
1578     return;
1579   }
1580
1581   for (l = domain_data; l; l = l->next) {
1582     PtpDomainData *tmp = l->data;
1583
1584     if (msg->domain_number == tmp->domain) {
1585       domain = tmp;
1586       break;
1587     }
1588   }
1589
1590   if (!domain) {
1591     GST_TRACE ("No domain match for FOLLOW_UP msg");
1592     return;
1593   }
1594
1595   /* If we have a master clock, ignore this message if it's not coming from there */
1596   if (domain->have_master_clock
1597       && compare_clock_identity (&domain->master_clock_identity,
1598           &msg->source_port_identity) != 0) {
1599     GST_TRACE ("FOLLOW_UP msg not from current clock master. Ignoring");
1600     return;
1601   }
1602
1603   /* Check if we know about this one */
1604   for (l = domain->pending_syncs.head; l; l = l->next) {
1605     PtpPendingSync *tmp = l->data;
1606
1607     if (tmp->sync_seqnum == msg->sequence_id) {
1608       sync = tmp;
1609       break;
1610     }
1611   }
1612
1613   if (!sync) {
1614     GST_TRACE ("Ignoring FOLLOW_UP with no pending SYNC");
1615     return;
1616   }
1617
1618   /* Got a FOLLOW_UP for this already */
1619   if (sync->sync_send_time_remote != GST_CLOCK_TIME_NONE) {
1620     GST_TRACE ("Got repeat FOLLOW_UP. Ignoring");
1621     return;
1622   }
1623
1624   if (sync->sync_recv_time_local >= receive_time) {
1625     GST_ERROR ("Got bogus follow up in domain %u: %" GST_TIME_FORMAT " > %"
1626         GST_TIME_FORMAT, domain->domain,
1627         GST_TIME_ARGS (sync->sync_recv_time_local),
1628         GST_TIME_ARGS (receive_time));
1629     g_queue_remove (&domain->pending_syncs, sync);
1630     ptp_pending_sync_free (sync);
1631     return;
1632   }
1633
1634   sync->correction_field_sync += msg->correction_field;
1635   sync->sync_send_time_remote =
1636       PTP_TIMESTAMP_TO_GST_CLOCK_TIME (msg->message_specific.
1637       follow_up.precise_origin_timestamp);
1638   sync->follow_up_recv_time_local = receive_time;
1639
1640   if (domain->last_ptp_sync_time >= sync->sync_send_time_remote) {
1641     GST_WARNING ("Backwards PTP times in domain %u: %" GST_TIME_FORMAT " >= %"
1642         GST_TIME_FORMAT, domain->domain,
1643         GST_TIME_ARGS (domain->last_ptp_sync_time),
1644         GST_TIME_ARGS (sync->sync_send_time_remote));
1645     g_queue_remove (&domain->pending_syncs, sync);
1646     ptp_pending_sync_free (sync);
1647     sync = NULL;
1648     return;
1649   }
1650   domain->last_ptp_sync_time = sync->sync_send_time_remote;
1651
1652   if (send_delay_req (domain, sync)) {
1653     /* Sent delay request */
1654   } else {
1655     update_ptp_time (domain, sync);
1656     g_queue_remove (&domain->pending_syncs, sync);
1657     ptp_pending_sync_free (sync);
1658     sync = NULL;
1659   }
1660 }
1661
1662 static void
1663 handle_delay_resp_message (PtpMessage * msg, GstClockTime receive_time)
1664 {
1665   GList *l;
1666   PtpDomainData *domain = NULL;
1667   PtpPendingSync *sync = NULL;
1668
1669   /* Don't consider messages with the alternate master flag set */
1670   if ((msg->flag_field & 0x0100))
1671     return;
1672
1673   for (l = domain_data; l; l = l->next) {
1674     PtpDomainData *tmp = l->data;
1675
1676     if (msg->domain_number == tmp->domain) {
1677       domain = tmp;
1678       break;
1679     }
1680   }
1681
1682   if (!domain)
1683     return;
1684
1685   /* If we have a master clock, ignore this message if it's not coming from there */
1686   if (domain->have_master_clock
1687       && compare_clock_identity (&domain->master_clock_identity,
1688           &msg->source_port_identity) != 0)
1689     return;
1690
1691   /* Not for us */
1692   if (msg->message_specific.delay_resp.
1693       requesting_port_identity.clock_identity != ptp_clock_id.clock_identity
1694       || msg->message_specific.delay_resp.
1695       requesting_port_identity.port_number != ptp_clock_id.port_number)
1696     return;
1697
1698   domain->min_delay_req_interval =
1699       log2_to_clock_time (msg->log_message_interval);
1700
1701   /* Check if we know about this one */
1702   for (l = domain->pending_syncs.head; l; l = l->next) {
1703     PtpPendingSync *tmp = l->data;
1704
1705     if (tmp->delay_req_seqnum == msg->sequence_id) {
1706       sync = tmp;
1707       break;
1708     }
1709   }
1710
1711   if (!sync)
1712     return;
1713
1714   /* Got a DELAY_RESP for this already */
1715   if (sync->delay_req_recv_time_remote != GST_CLOCK_TIME_NONE)
1716     return;
1717
1718   if (sync->delay_req_send_time_local > receive_time) {
1719     GST_ERROR ("Got bogus delay response in domain %u: %" GST_TIME_FORMAT " > %"
1720         GST_TIME_FORMAT, domain->domain,
1721         GST_TIME_ARGS (sync->delay_req_send_time_local),
1722         GST_TIME_ARGS (receive_time));
1723     g_queue_remove (&domain->pending_syncs, sync);
1724     ptp_pending_sync_free (sync);
1725     return;
1726   }
1727
1728   sync->correction_field_delay = msg->correction_field;
1729
1730   sync->delay_req_recv_time_remote =
1731       PTP_TIMESTAMP_TO_GST_CLOCK_TIME (msg->message_specific.
1732       delay_resp.receive_timestamp);
1733   sync->delay_resp_recv_time_local = receive_time;
1734
1735   if (domain->mean_path_delay != 0
1736       && sync->sync_send_time_remote > sync->delay_req_recv_time_remote) {
1737     GST_WARNING ("Sync send time after delay req receive time for domain %u: %"
1738         GST_TIME_FORMAT " > %" GST_TIME_FORMAT, domain->domain,
1739         GST_TIME_ARGS (sync->sync_send_time_remote),
1740         GST_TIME_ARGS (sync->delay_req_recv_time_remote));
1741     g_queue_remove (&domain->pending_syncs, sync);
1742     ptp_pending_sync_free (sync);
1743     return;
1744   }
1745
1746   if (update_mean_path_delay (domain, sync))
1747     update_ptp_time (domain, sync);
1748   g_queue_remove (&domain->pending_syncs, sync);
1749   ptp_pending_sync_free (sync);
1750 }
1751
1752 static void
1753 handle_ptp_message (PtpMessage * msg, GstClockTime receive_time)
1754 {
1755   /* Ignore our own messages */
1756   if (msg->source_port_identity.clock_identity == ptp_clock_id.clock_identity &&
1757       msg->source_port_identity.port_number == ptp_clock_id.port_number) {
1758     GST_TRACE ("Ignoring our own message");
1759     return;
1760   }
1761
1762   GST_TRACE ("Message type %d receive_time %" GST_TIME_FORMAT,
1763       msg->message_type, GST_TIME_ARGS (receive_time));
1764   switch (msg->message_type) {
1765     case PTP_MESSAGE_TYPE_ANNOUNCE:
1766       handle_announce_message (msg, receive_time);
1767       break;
1768     case PTP_MESSAGE_TYPE_SYNC:
1769       handle_sync_message (msg, receive_time);
1770       break;
1771     case PTP_MESSAGE_TYPE_FOLLOW_UP:
1772       handle_follow_up_message (msg, receive_time);
1773       break;
1774     case PTP_MESSAGE_TYPE_DELAY_RESP:
1775       handle_delay_resp_message (msg, receive_time);
1776       break;
1777     default:
1778       break;
1779   }
1780 }
1781
1782 static gboolean
1783 have_stdin_data_cb (GIOChannel * channel, GIOCondition condition,
1784     gpointer user_data)
1785 {
1786   GIOStatus status;
1787   StdIOHeader header;
1788   gchar buffer[8192];
1789   GError *err = NULL;
1790   gsize read;
1791
1792   if ((condition & G_IO_STATUS_EOF)) {
1793     GST_ERROR ("Got EOF on stdin");
1794     g_main_loop_quit (main_loop);
1795     return G_SOURCE_REMOVE;
1796   }
1797
1798   status =
1799       g_io_channel_read_chars (channel, (gchar *) & header, sizeof (header),
1800       &read, &err);
1801   if (status == G_IO_STATUS_ERROR) {
1802     GST_ERROR ("Failed to read from stdin: %s", err->message);
1803     g_clear_error (&err);
1804     g_main_loop_quit (main_loop);
1805     return G_SOURCE_REMOVE;
1806   } else if (status == G_IO_STATUS_EOF) {
1807     GST_ERROR ("Got EOF on stdin");
1808     g_main_loop_quit (main_loop);
1809     return G_SOURCE_REMOVE;
1810   } else if (status != G_IO_STATUS_NORMAL) {
1811     GST_ERROR ("Unexpected stdin read status: %d", status);
1812     g_main_loop_quit (main_loop);
1813     return G_SOURCE_REMOVE;
1814   } else if (read != sizeof (header)) {
1815     GST_ERROR ("Unexpected read size: %" G_GSIZE_FORMAT, read);
1816     g_main_loop_quit (main_loop);
1817     return G_SOURCE_REMOVE;
1818   } else if (header.size > 8192) {
1819     GST_ERROR ("Unexpected size: %u", header.size);
1820     g_main_loop_quit (main_loop);
1821     return G_SOURCE_REMOVE;
1822   }
1823
1824   status = g_io_channel_read_chars (channel, buffer, header.size, &read, &err);
1825   if (status == G_IO_STATUS_ERROR) {
1826     GST_ERROR ("Failed to read from stdin: %s", err->message);
1827     g_clear_error (&err);
1828     g_main_loop_quit (main_loop);
1829     return G_SOURCE_REMOVE;
1830   } else if (status == G_IO_STATUS_EOF) {
1831     GST_ERROR ("EOF on stdin");
1832     g_main_loop_quit (main_loop);
1833     return G_SOURCE_REMOVE;
1834   } else if (status != G_IO_STATUS_NORMAL) {
1835     GST_ERROR ("Unexpected stdin read status: %d", status);
1836     g_main_loop_quit (main_loop);
1837     return G_SOURCE_REMOVE;
1838   } else if (read != header.size) {
1839     GST_ERROR ("Unexpected read size: %" G_GSIZE_FORMAT, read);
1840     g_main_loop_quit (main_loop);
1841     return G_SOURCE_REMOVE;
1842   }
1843
1844   switch (header.type) {
1845     case TYPE_EVENT:
1846     case TYPE_GENERAL:{
1847       GstClockTime receive_time = gst_clock_get_time (observation_system_clock);
1848       PtpMessage msg;
1849
1850       if (parse_ptp_message (&msg, (const guint8 *) buffer, header.size)) {
1851         dump_ptp_message (&msg);
1852         handle_ptp_message (&msg, receive_time);
1853       }
1854       break;
1855     }
1856     default:
1857     case TYPE_CLOCK_ID:{
1858       if (header.size != 8) {
1859         GST_ERROR ("Unexpected clock id size (%u != 8)", header.size);
1860         g_main_loop_quit (main_loop);
1861         return G_SOURCE_REMOVE;
1862       }
1863       g_mutex_lock (&ptp_lock);
1864       ptp_clock_id.clock_identity = GST_READ_UINT64_BE (buffer);
1865       ptp_clock_id.port_number = getpid ();
1866       GST_DEBUG ("Got clock id 0x%016" G_GINT64_MODIFIER "x %u",
1867           ptp_clock_id.clock_identity, ptp_clock_id.port_number);
1868       g_cond_signal (&ptp_cond);
1869       g_mutex_unlock (&ptp_lock);
1870       break;
1871     }
1872   }
1873
1874   return G_SOURCE_CONTINUE;
1875 }
1876
1877 /* Cleanup all announce messages and announce message senders
1878  * that are timed out by now, and clean up all pending syncs
1879  * that are missing their FOLLOW_UP or DELAY_RESP */
1880 static gboolean
1881 cleanup_cb (gpointer data)
1882 {
1883   GstClockTime now = gst_clock_get_time (observation_system_clock);
1884   GList *l, *m, *n;
1885
1886   for (l = domain_data; l; l = l->next) {
1887     PtpDomainData *domain = l->data;
1888
1889     for (n = domain->announce_senders; n;) {
1890       PtpAnnounceSender *sender = n->data;
1891       gboolean timed_out = TRUE;
1892
1893       /* Keep only 5 messages per sender around */
1894       while (g_queue_get_length (&sender->announce_messages) > 5) {
1895         PtpAnnounceMessage *msg = g_queue_pop_head (&sender->announce_messages);
1896         g_free (msg);
1897       }
1898
1899       for (m = sender->announce_messages.head; m; m = m->next) {
1900         PtpAnnounceMessage *msg = m->data;
1901
1902         if (msg->receive_time +
1903             sender->announce_interval * PTP_ANNOUNCE_RECEIPT_TIMEOUT > now) {
1904           timed_out = FALSE;
1905           break;
1906         }
1907       }
1908
1909       if (timed_out) {
1910         GST_DEBUG ("Announce sender 0x%016" G_GINT64_MODIFIER "x %u timed out",
1911             sender->master_clock_identity.clock_identity,
1912             sender->master_clock_identity.port_number);
1913         g_queue_foreach (&sender->announce_messages, (GFunc) g_free, NULL);
1914         g_queue_clear (&sender->announce_messages);
1915       }
1916
1917       if (g_queue_get_length (&sender->announce_messages) == 0) {
1918         GList *tmp = n->next;
1919
1920         if (compare_clock_identity (&sender->master_clock_identity,
1921                 &domain->master_clock_identity) == 0)
1922           GST_WARNING ("currently selected master clock timed out");
1923         g_free (sender);
1924         domain->announce_senders =
1925             g_list_delete_link (domain->announce_senders, n);
1926         n = tmp;
1927       } else {
1928         n = n->next;
1929       }
1930     }
1931     select_best_master_clock (domain, now);
1932
1933     /* Clean up any pending syncs */
1934     for (n = domain->pending_syncs.head; n;) {
1935       PtpPendingSync *sync = n->data;
1936       gboolean timed_out = FALSE;
1937
1938       /* Time out pending syncs after 4 sync intervals or 10 seconds,
1939        * and pending delay reqs after 4 delay req intervals or 10 seconds
1940        */
1941       if (sync->delay_req_send_time_local != GST_CLOCK_TIME_NONE &&
1942           ((domain->min_delay_req_interval != 0
1943                   && sync->delay_req_send_time_local +
1944                   4 * domain->min_delay_req_interval < now)
1945               || (sync->delay_req_send_time_local + 10 * GST_SECOND < now))) {
1946         timed_out = TRUE;
1947       } else if ((domain->sync_interval != 0
1948               && sync->sync_recv_time_local + 4 * domain->sync_interval < now)
1949           || (sync->sync_recv_time_local + 10 * GST_SECOND < now)) {
1950         timed_out = TRUE;
1951       }
1952
1953       if (timed_out) {
1954         GList *tmp = n->next;
1955         ptp_pending_sync_free (sync);
1956         g_queue_delete_link (&domain->pending_syncs, n);
1957         n = tmp;
1958       } else {
1959         n = n->next;
1960       }
1961     }
1962   }
1963
1964   return G_SOURCE_CONTINUE;
1965 }
1966
1967 static gpointer
1968 ptp_helper_main (gpointer data)
1969 {
1970   GSource *cleanup_source;
1971
1972   GST_DEBUG ("Starting PTP helper loop");
1973
1974   /* Check all 5 seconds, if we have to cleanup ANNOUNCE or pending syncs message */
1975   cleanup_source = g_timeout_source_new_seconds (5);
1976   g_source_set_priority (cleanup_source, G_PRIORITY_DEFAULT);
1977   g_source_set_callback (cleanup_source, (GSourceFunc) cleanup_cb, NULL, NULL);
1978   g_source_attach (cleanup_source, main_context);
1979   g_source_unref (cleanup_source);
1980
1981   g_main_loop_run (main_loop);
1982   GST_DEBUG ("Stopped PTP helper loop");
1983
1984   g_mutex_lock (&ptp_lock);
1985   ptp_clock_id.clock_identity = GST_PTP_CLOCK_ID_NONE;
1986   ptp_clock_id.port_number = 0;
1987   initted = FALSE;
1988   g_cond_signal (&ptp_cond);
1989   g_mutex_unlock (&ptp_lock);
1990
1991   return NULL;
1992 }
1993
1994 /**
1995  * gst_ptp_is_supported:
1996  *
1997  * Check if PTP clocks are generally supported on this system, and if previous
1998  * initializations did not fail.
1999  *
2000  * Returns: %TRUE if PTP clocks are generally supported on this system, and
2001  * previous initializations did not fail.
2002  *
2003  * Since: 1.6
2004  */
2005 gboolean
2006 gst_ptp_is_supported (void)
2007 {
2008   return supported;
2009 }
2010
2011 /**
2012  * gst_ptp_is_initialized:
2013  *
2014  * Check if the GStreamer PTP clock subsystem is initialized.
2015  *
2016  * Returns: %TRUE if the GStreamer PTP clock subsystem is initialized.
2017  *
2018  * Since: 1.6
2019  */
2020 gboolean
2021 gst_ptp_is_initialized (void)
2022 {
2023   return initted;
2024 }
2025
2026 /**
2027  * gst_ptp_init:
2028  * @clock_id: PTP clock id of this process' clock or %GST_PTP_CLOCK_ID_NONE
2029  * @interfaces: (transfer none) (array zero-terminated=1) (allow-none): network interfaces to run the clock on
2030  *
2031  * Initialize the GStreamer PTP subsystem and create a PTP ordinary clock in
2032  * slave-only mode for all domains on the given @interfaces with the
2033  * given @clock_id.
2034  *
2035  * If @clock_id is %GST_PTP_CLOCK_ID_NONE, a clock id is automatically
2036  * generated from the MAC address of the first network interface.
2037  *
2038  * This function is automatically called by gst_ptp_clock_new() with default
2039  * parameters if it wasn't called before.
2040  *
2041  * Returns: %TRUE if the GStreamer PTP clock subsystem could be initialized.
2042  *
2043  * Since: 1.6
2044  */
2045 gboolean
2046 gst_ptp_init (guint64 clock_id, gchar ** interfaces)
2047 {
2048   gboolean ret;
2049   const gchar *env;
2050   gchar **argv = NULL;
2051   gint argc, argc_c;
2052   gint fd_r, fd_w;
2053   GError *err = NULL;
2054   GSource *stdin_source;
2055
2056   GST_DEBUG_CATEGORY_INIT (ptp_debug, "ptp", 0, "PTP clock");
2057
2058   g_mutex_lock (&ptp_lock);
2059   if (!supported) {
2060     GST_ERROR ("PTP not supported");
2061     ret = FALSE;
2062     goto done;
2063   }
2064
2065   if (initted) {
2066     GST_DEBUG ("PTP already initialized");
2067     ret = TRUE;
2068     goto done;
2069   }
2070
2071   if (ptp_helper_pid) {
2072     GST_DEBUG ("PTP currently initializing");
2073     goto wait;
2074   }
2075
2076   if (!domain_stats_hooks_initted) {
2077     g_hook_list_init (&domain_stats_hooks, sizeof (GHook));
2078     domain_stats_hooks_initted = TRUE;
2079   }
2080
2081   argc = 1;
2082   if (clock_id != GST_PTP_CLOCK_ID_NONE)
2083     argc += 2;
2084   if (interfaces != NULL)
2085     argc += 2 * g_strv_length (interfaces);
2086
2087   argv = g_new0 (gchar *, argc + 2);
2088   argc_c = 0;
2089
2090   env = g_getenv ("GST_PTP_HELPER_1_0");
2091   if (env == NULL)
2092     env = g_getenv ("GST_PTP_HELPER");
2093   if (env != NULL && *env != '\0') {
2094     GST_LOG ("Trying GST_PTP_HELPER env var: %s", env);
2095     argv[argc_c++] = g_strdup (env);
2096   } else {
2097     argv[argc_c++] = g_strdup (GST_PTP_HELPER_INSTALLED);
2098   }
2099
2100   if (clock_id != GST_PTP_CLOCK_ID_NONE) {
2101     argv[argc_c++] = g_strdup ("-c");
2102     argv[argc_c++] = g_strdup_printf ("0x%016" G_GINT64_MODIFIER "x", clock_id);
2103   }
2104
2105   if (interfaces != NULL) {
2106     gchar **ptr = interfaces;
2107
2108     while (*ptr) {
2109       argv[argc_c++] = g_strdup ("-i");
2110       argv[argc_c++] = g_strdup (*ptr);
2111       ptr++;
2112     }
2113   }
2114
2115   main_context = g_main_context_new ();
2116   main_loop = g_main_loop_new (main_context, FALSE);
2117
2118   ptp_helper_thread =
2119       g_thread_try_new ("ptp-helper-thread", ptp_helper_main, NULL, &err);
2120   if (!ptp_helper_thread) {
2121     GST_ERROR ("Failed to start PTP helper thread: %s", err->message);
2122     g_clear_error (&err);
2123     ret = FALSE;
2124     goto done;
2125   }
2126
2127   if (!g_spawn_async_with_pipes (NULL, argv, NULL, 0, NULL, NULL,
2128           &ptp_helper_pid, &fd_w, &fd_r, NULL, &err)) {
2129     GST_ERROR ("Failed to start ptp helper process: %s", err->message);
2130     g_clear_error (&err);
2131     ret = FALSE;
2132     supported = FALSE;
2133     goto done;
2134   }
2135
2136   stdin_channel = g_io_channel_unix_new (fd_r);
2137   g_io_channel_set_encoding (stdin_channel, NULL, NULL);
2138   g_io_channel_set_buffered (stdin_channel, FALSE);
2139   g_io_channel_set_close_on_unref (stdin_channel, TRUE);
2140   stdin_source =
2141       g_io_create_watch (stdin_channel, G_IO_IN | G_IO_PRI | G_IO_HUP);
2142   g_source_set_priority (stdin_source, G_PRIORITY_DEFAULT);
2143   g_source_set_callback (stdin_source, (GSourceFunc) have_stdin_data_cb, NULL,
2144       NULL);
2145   g_source_attach (stdin_source, main_context);
2146   g_source_unref (stdin_source);
2147
2148   /* Create stdout channel */
2149   stdout_channel = g_io_channel_unix_new (fd_w);
2150   g_io_channel_set_encoding (stdout_channel, NULL, NULL);
2151   g_io_channel_set_close_on_unref (stdout_channel, TRUE);
2152   g_io_channel_set_buffered (stdout_channel, FALSE);
2153
2154   delay_req_rand = g_rand_new ();
2155   observation_system_clock =
2156       g_object_new (GST_TYPE_SYSTEM_CLOCK, "name", "ptp-observation-clock",
2157       NULL);
2158   gst_object_ref_sink (observation_system_clock);
2159
2160   initted = TRUE;
2161
2162 wait:
2163   GST_DEBUG ("Waiting for PTP to be initialized");
2164
2165   while (ptp_clock_id.clock_identity == GST_PTP_CLOCK_ID_NONE && initted)
2166     g_cond_wait (&ptp_cond, &ptp_lock);
2167
2168   ret = initted;
2169   if (ret) {
2170     GST_DEBUG ("Initialized and got clock id 0x%016" G_GINT64_MODIFIER "x %u",
2171         ptp_clock_id.clock_identity, ptp_clock_id.port_number);
2172   } else {
2173     GST_ERROR ("Failed to initialize");
2174     supported = FALSE;
2175   }
2176
2177 done:
2178   g_strfreev (argv);
2179
2180   if (!ret) {
2181     if (ptp_helper_pid) {
2182 #ifndef G_OS_WIN32
2183       kill (ptp_helper_pid, SIGKILL);
2184       waitpid (ptp_helper_pid, NULL, 0);
2185 #else
2186       TerminateProcess (ptp_helper_pid, 1);
2187       WaitForSingleObject (ptp_helper_pid, INFINITE);
2188 #endif
2189       g_spawn_close_pid (ptp_helper_pid);
2190     }
2191     ptp_helper_pid = 0;
2192
2193     if (stdin_channel)
2194       g_io_channel_unref (stdin_channel);
2195     stdin_channel = NULL;
2196     if (stdout_channel)
2197       g_io_channel_unref (stdout_channel);
2198     stdout_channel = NULL;
2199
2200     if (main_loop && ptp_helper_thread) {
2201       g_main_loop_quit (main_loop);
2202       g_thread_join (ptp_helper_thread);
2203     }
2204     ptp_helper_thread = NULL;
2205     if (main_loop)
2206       g_main_loop_unref (main_loop);
2207     main_loop = NULL;
2208     if (main_context)
2209       g_main_context_unref (main_context);
2210     main_context = NULL;
2211
2212     if (delay_req_rand)
2213       g_rand_free (delay_req_rand);
2214     delay_req_rand = NULL;
2215
2216     if (observation_system_clock)
2217       gst_object_unref (observation_system_clock);
2218     observation_system_clock = NULL;
2219   }
2220
2221   g_mutex_unlock (&ptp_lock);
2222
2223   return ret;
2224 }
2225
2226 /**
2227  * gst_ptp_deinit:
2228  *
2229  * Deinitialize the GStreamer PTP subsystem and stop the PTP clock. If there
2230  * are any remaining GstPtpClock instances, they won't be further synchronized
2231  * to the PTP network clock.
2232  *
2233  * Since: 1.6
2234  */
2235 void
2236 gst_ptp_deinit (void)
2237 {
2238   GList *l, *m;
2239
2240   g_mutex_lock (&ptp_lock);
2241
2242   if (ptp_helper_pid) {
2243 #ifndef G_OS_WIN32
2244     kill (ptp_helper_pid, SIGKILL);
2245     waitpid (ptp_helper_pid, NULL, 0);
2246 #else
2247     TerminateProcess (ptp_helper_pid, 1);
2248     WaitForSingleObject (ptp_helper_pid, INFINITE);
2249 #endif
2250     g_spawn_close_pid (ptp_helper_pid);
2251   }
2252   ptp_helper_pid = 0;
2253
2254   if (stdin_channel)
2255     g_io_channel_unref (stdin_channel);
2256   stdin_channel = NULL;
2257   if (stdout_channel)
2258     g_io_channel_unref (stdout_channel);
2259   stdout_channel = NULL;
2260
2261   if (main_loop && ptp_helper_thread) {
2262     GThread *tmp = ptp_helper_thread;
2263     ptp_helper_thread = NULL;
2264     g_mutex_unlock (&ptp_lock);
2265     g_main_loop_quit (main_loop);
2266     g_thread_join (tmp);
2267     g_mutex_lock (&ptp_lock);
2268   }
2269   if (main_loop)
2270     g_main_loop_unref (main_loop);
2271   main_loop = NULL;
2272   if (main_context)
2273     g_main_context_unref (main_context);
2274   main_context = NULL;
2275
2276   if (delay_req_rand)
2277     g_rand_free (delay_req_rand);
2278   delay_req_rand = NULL;
2279   if (observation_system_clock)
2280     gst_object_unref (observation_system_clock);
2281   observation_system_clock = NULL;
2282
2283   for (l = domain_data; l; l = l->next) {
2284     PtpDomainData *domain = l->data;
2285
2286     for (m = domain->announce_senders; m; m = m->next) {
2287       PtpAnnounceSender *sender = m->data;
2288
2289       g_queue_foreach (&sender->announce_messages, (GFunc) g_free, NULL);
2290       g_queue_clear (&sender->announce_messages);
2291       g_free (sender);
2292     }
2293     g_list_free (domain->announce_senders);
2294
2295     g_queue_foreach (&domain->pending_syncs, (GFunc) ptp_pending_sync_free,
2296         NULL);
2297     g_queue_clear (&domain->pending_syncs);
2298     gst_object_unref (domain->domain_clock);
2299     g_free (domain);
2300   }
2301   g_list_free (domain_data);
2302   domain_data = NULL;
2303   g_list_foreach (domain_clocks, (GFunc) g_free, NULL);
2304   g_list_free (domain_clocks);
2305   domain_clocks = NULL;
2306
2307   ptp_clock_id.clock_identity = GST_PTP_CLOCK_ID_NONE;
2308   ptp_clock_id.port_number = 0;
2309
2310   initted = FALSE;
2311
2312   g_mutex_unlock (&ptp_lock);
2313 }
2314
2315 #define DEFAULT_DOMAIN 0
2316
2317 enum
2318 {
2319   PROP_0,
2320   PROP_DOMAIN,
2321   PROP_INTERNAL_CLOCK,
2322   PROP_MASTER_CLOCK_ID,
2323   PROP_GRANDMASTER_CLOCK_ID
2324 };
2325
2326 struct _GstPtpClockPrivate
2327 {
2328   guint domain;
2329   GstClock *domain_clock;
2330   gulong domain_stats_id;
2331 };
2332
2333 #define gst_ptp_clock_parent_class parent_class
2334 G_DEFINE_TYPE_WITH_PRIVATE (GstPtpClock, gst_ptp_clock, GST_TYPE_SYSTEM_CLOCK);
2335
2336 static void gst_ptp_clock_set_property (GObject * object, guint prop_id,
2337     const GValue * value, GParamSpec * pspec);
2338 static void gst_ptp_clock_get_property (GObject * object, guint prop_id,
2339     GValue * value, GParamSpec * pspec);
2340 static void gst_ptp_clock_finalize (GObject * object);
2341
2342 static GstClockTime gst_ptp_clock_get_internal_time (GstClock * clock);
2343
2344 static void
2345 gst_ptp_clock_class_init (GstPtpClockClass * klass)
2346 {
2347   GObjectClass *gobject_class;
2348   GstClockClass *clock_class;
2349
2350   gobject_class = G_OBJECT_CLASS (klass);
2351   clock_class = GST_CLOCK_CLASS (klass);
2352
2353   gobject_class->finalize = gst_ptp_clock_finalize;
2354   gobject_class->get_property = gst_ptp_clock_get_property;
2355   gobject_class->set_property = gst_ptp_clock_set_property;
2356
2357   g_object_class_install_property (gobject_class, PROP_DOMAIN,
2358       g_param_spec_uint ("domain", "Domain",
2359           "The PTP domain", 0, G_MAXUINT8,
2360           DEFAULT_DOMAIN,
2361           G_PARAM_READWRITE | G_PARAM_CONSTRUCT_ONLY | G_PARAM_STATIC_STRINGS));
2362
2363   g_object_class_install_property (gobject_class, PROP_INTERNAL_CLOCK,
2364       g_param_spec_object ("internal-clock", "Internal Clock",
2365           "Internal clock", GST_TYPE_CLOCK,
2366           G_PARAM_READABLE | G_PARAM_STATIC_STRINGS));
2367
2368   g_object_class_install_property (gobject_class, PROP_MASTER_CLOCK_ID,
2369       g_param_spec_uint64 ("master-clock-id", "Master Clock ID",
2370           "Master Clock ID", 0, G_MAXUINT64, 0,
2371           G_PARAM_READABLE | G_PARAM_STATIC_STRINGS));
2372
2373   g_object_class_install_property (gobject_class, PROP_GRANDMASTER_CLOCK_ID,
2374       g_param_spec_uint64 ("grandmaster-clock-id", "Grand Master Clock ID",
2375           "Grand Master Clock ID", 0, G_MAXUINT64, 0,
2376           G_PARAM_READABLE | G_PARAM_STATIC_STRINGS));
2377
2378   clock_class->get_internal_time = gst_ptp_clock_get_internal_time;
2379 }
2380
2381 static void
2382 gst_ptp_clock_init (GstPtpClock * self)
2383 {
2384   GstPtpClockPrivate *priv;
2385
2386   self->priv = priv = gst_ptp_clock_get_instance_private (self);
2387
2388   GST_OBJECT_FLAG_SET (self, GST_CLOCK_FLAG_CAN_SET_MASTER);
2389   GST_OBJECT_FLAG_SET (self, GST_CLOCK_FLAG_NEEDS_STARTUP_SYNC);
2390
2391   priv->domain = DEFAULT_DOMAIN;
2392 }
2393
2394 static gboolean
2395 gst_ptp_clock_ensure_domain_clock (GstPtpClock * self)
2396 {
2397   gboolean got_clock = TRUE;
2398
2399   if (G_UNLIKELY (!self->priv->domain_clock)) {
2400     g_mutex_lock (&domain_clocks_lock);
2401     if (!self->priv->domain_clock) {
2402       GList *l;
2403
2404       got_clock = FALSE;
2405
2406       for (l = domain_clocks; l; l = l->next) {
2407         PtpDomainData *clock_data = l->data;
2408
2409         if (clock_data->domain == self->priv->domain &&
2410             clock_data->have_master_clock && clock_data->last_ptp_time != 0) {
2411           GST_DEBUG ("Switching domain clock on domain %d", clock_data->domain);
2412           self->priv->domain_clock = clock_data->domain_clock;
2413           got_clock = TRUE;
2414           break;
2415         }
2416       }
2417     }
2418     g_mutex_unlock (&domain_clocks_lock);
2419     if (got_clock) {
2420       g_object_notify (G_OBJECT (self), "internal-clock");
2421       gst_clock_set_synced (GST_CLOCK (self), TRUE);
2422     }
2423   }
2424
2425   return got_clock;
2426 }
2427
2428 static gboolean
2429 gst_ptp_clock_stats_callback (guint8 domain, const GstStructure * stats,
2430     gpointer user_data)
2431 {
2432   GstPtpClock *self = user_data;
2433
2434   if (domain != self->priv->domain
2435       || !gst_structure_has_name (stats, GST_PTP_STATISTICS_TIME_UPDATED))
2436     return TRUE;
2437
2438   /* Let's set our internal clock */
2439   if (!gst_ptp_clock_ensure_domain_clock (self))
2440     return TRUE;
2441
2442   self->priv->domain_stats_id = 0;
2443
2444   return FALSE;
2445 }
2446
2447 static void
2448 gst_ptp_clock_set_property (GObject * object, guint prop_id,
2449     const GValue * value, GParamSpec * pspec)
2450 {
2451   GstPtpClock *self = GST_PTP_CLOCK (object);
2452
2453   switch (prop_id) {
2454     case PROP_DOMAIN:
2455       self->priv->domain = g_value_get_uint (value);
2456       gst_ptp_clock_ensure_domain_clock (self);
2457       if (!self->priv->domain_clock)
2458         self->priv->domain_stats_id =
2459             gst_ptp_statistics_callback_add (gst_ptp_clock_stats_callback, self,
2460             NULL);
2461       break;
2462     default:
2463       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
2464       break;
2465   }
2466 }
2467
2468 static void
2469 gst_ptp_clock_get_property (GObject * object, guint prop_id,
2470     GValue * value, GParamSpec * pspec)
2471 {
2472   GstPtpClock *self = GST_PTP_CLOCK (object);
2473
2474   switch (prop_id) {
2475     case PROP_DOMAIN:
2476       g_value_set_uint (value, self->priv->domain);
2477       break;
2478     case PROP_INTERNAL_CLOCK:
2479       gst_ptp_clock_ensure_domain_clock (self);
2480       g_value_set_object (value, self->priv->domain_clock);
2481       break;
2482     case PROP_MASTER_CLOCK_ID:
2483     case PROP_GRANDMASTER_CLOCK_ID:{
2484       GList *l;
2485
2486       g_mutex_lock (&domain_clocks_lock);
2487       g_value_set_uint64 (value, 0);
2488
2489       for (l = domain_clocks; l; l = l->next) {
2490         PtpDomainData *clock_data = l->data;
2491
2492         if (clock_data->domain == self->priv->domain) {
2493           if (prop_id == PROP_MASTER_CLOCK_ID)
2494             g_value_set_uint64 (value,
2495                 clock_data->master_clock_identity.clock_identity);
2496           else
2497             g_value_set_uint64 (value, clock_data->grandmaster_identity);
2498           break;
2499         }
2500       }
2501       g_mutex_unlock (&domain_clocks_lock);
2502       break;
2503     }
2504     default:
2505       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
2506       break;
2507   }
2508 }
2509
2510 static void
2511 gst_ptp_clock_finalize (GObject * object)
2512 {
2513   GstPtpClock *self = GST_PTP_CLOCK (object);
2514
2515   if (self->priv->domain_stats_id)
2516     gst_ptp_statistics_callback_remove (self->priv->domain_stats_id);
2517
2518   G_OBJECT_CLASS (gst_ptp_clock_parent_class)->finalize (object);
2519 }
2520
2521 static GstClockTime
2522 gst_ptp_clock_get_internal_time (GstClock * clock)
2523 {
2524   GstPtpClock *self = GST_PTP_CLOCK (clock);
2525
2526   gst_ptp_clock_ensure_domain_clock (self);
2527
2528   if (!self->priv->domain_clock) {
2529     GST_ERROR_OBJECT (self, "Domain %u has no clock yet and is not synced",
2530         self->priv->domain);
2531     return GST_CLOCK_TIME_NONE;
2532   }
2533
2534   return gst_clock_get_time (self->priv->domain_clock);
2535 }
2536
2537 /**
2538  * gst_ptp_clock_new:
2539  * @name: Name of the clock
2540  * @domain: PTP domain
2541  *
2542  * Creates a new PTP clock instance that exports the PTP time of the master
2543  * clock in @domain. This clock can be slaved to other clocks as needed.
2544  *
2545  * If gst_ptp_init() was not called before, this will call gst_ptp_init() with
2546  * default parameters.
2547  *
2548  * This clock only returns valid timestamps after it received the first
2549  * times from the PTP master clock on the network. Once this happens the
2550  * GstPtpClock::internal-clock property will become non-NULL. You can
2551  * check this with gst_clock_wait_for_sync(), the GstClock::synced signal and
2552  * gst_clock_is_synced().
2553  *
2554  * Returns: (transfer full): A new #GstClock
2555  *
2556  * Since: 1.6
2557  */
2558 GstClock *
2559 gst_ptp_clock_new (const gchar * name, guint domain)
2560 {
2561   GstClock *clock;
2562
2563   g_return_val_if_fail (domain <= G_MAXUINT8, NULL);
2564
2565   if (!initted && !gst_ptp_init (GST_PTP_CLOCK_ID_NONE, NULL)) {
2566     GST_ERROR ("Failed to initialize PTP");
2567     return NULL;
2568   }
2569
2570   clock = g_object_new (GST_TYPE_PTP_CLOCK, "name", name, "domain", domain,
2571       NULL);
2572
2573   /* Clear floating flag */
2574   gst_object_ref_sink (clock);
2575
2576   return clock;
2577 }
2578
2579 typedef struct
2580 {
2581   guint8 domain;
2582   const GstStructure *stats;
2583 } DomainStatsMarshalData;
2584
2585 static void
2586 domain_stats_marshaller (GHook * hook, DomainStatsMarshalData * data)
2587 {
2588   GstPtpStatisticsCallback callback = (GstPtpStatisticsCallback) hook->func;
2589
2590   if (!callback (data->domain, data->stats, hook->data))
2591     g_hook_destroy (&domain_stats_hooks, hook->hook_id);
2592 }
2593
2594 static void
2595 emit_ptp_statistics (guint8 domain, const GstStructure * stats)
2596 {
2597   DomainStatsMarshalData data = { domain, stats };
2598
2599   g_mutex_lock (&ptp_lock);
2600   g_hook_list_marshal (&domain_stats_hooks, TRUE,
2601       (GHookMarshaller) domain_stats_marshaller, &data);
2602   g_mutex_unlock (&ptp_lock);
2603 }
2604
2605 /**
2606  * gst_ptp_statistics_callback_add:
2607  * @callback: GstPtpStatisticsCallback to call
2608  * @user_data: Data to pass to the callback
2609  * @destroy_data: GDestroyNotify to destroy the data
2610  *
2611  * Installs a new statistics callback for gathering PTP statistics. See
2612  * GstPtpStatisticsCallback for a list of statistics that are provided.
2613  *
2614  * Returns: Id for the callback that can be passed to
2615  * gst_ptp_statistics_callback_remove()
2616  *
2617  * Since: 1.6
2618  */
2619 gulong
2620 gst_ptp_statistics_callback_add (GstPtpStatisticsCallback callback,
2621     gpointer user_data, GDestroyNotify destroy_data)
2622 {
2623   GHook *hook;
2624
2625   g_mutex_lock (&ptp_lock);
2626
2627   if (!domain_stats_hooks_initted) {
2628     g_hook_list_init (&domain_stats_hooks, sizeof (GHook));
2629     domain_stats_hooks_initted = TRUE;
2630   }
2631
2632   hook = g_hook_alloc (&domain_stats_hooks);
2633   hook->func = callback;
2634   hook->data = user_data;
2635   hook->destroy = destroy_data;
2636   g_hook_prepend (&domain_stats_hooks, hook);
2637   g_atomic_int_add (&domain_stats_n_hooks, 1);
2638
2639   g_mutex_unlock (&ptp_lock);
2640
2641   return hook->hook_id;
2642 }
2643
2644 /**
2645  * gst_ptp_statistics_callback_remove:
2646  * @id: Callback id to remove
2647  *
2648  * Removes a PTP statistics callback that was previously added with
2649  * gst_ptp_statistics_callback_add().
2650  *
2651  * Since: 1.6
2652  */
2653 void
2654 gst_ptp_statistics_callback_remove (gulong id)
2655 {
2656   g_mutex_lock (&ptp_lock);
2657   if (g_hook_destroy (&domain_stats_hooks, id))
2658     g_atomic_int_add (&domain_stats_n_hooks, -1);
2659   g_mutex_unlock (&ptp_lock);
2660 }