nqptp-clock-sources.c

   1 /*
   2  * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
   3  * Copyright (c) 2021 Mike Brady.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, version 2.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16  *
  17  * Commercial licensing is also available.
  18  */
  19
  20 #include "nqptp-clock-sources.h"
  21 #include "debug.h"
  22 #include "nqptp-ptp-definitions.h"
  23 #include <arpa/inet.h>
  24 #include <ifaddrs.h>
  25 #include <string.h>
  26 #include <sys/types.h>
  27
  28 #ifndef FIELD_SIZEOF
  29 #define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))
  30 #endif
  31
  32 clock_source_private_data clocks_private[MAX_CLOCKS];
  33
  34 int find_clock_source_record(char *sender_string, clock_source_private_data *clocks_private_info) {
  35   // return the index of the clock in the clock information arrays or -1
  36   int response = -1;
  37   int i = 0;
  38   int found = 0;
  39   while ((found == 0) && (i < MAX_CLOCKS)) {
  40     if ((clocks_private_info[i].in_use != 0) &&
  41         (strcasecmp(sender_string, (const char *)&clocks_private_info[i].ip) == 0))
  42       found = 1;
  43     else
  44       i++;
  45   }
  46   if (found == 1)
  47     response = i;
  48   return response;
  49 }
  50
  51 int create_clock_source_record(char *sender_string,
  52                                clock_source_private_data *clocks_private_info) {
  53   // sometimes, the mutex will already be locked
  54   // return the index of a clock entry in the clock information arrays or -1 if full
  55   // initialise the entries in the shared and private arrays
  56   int response = -1;
  57   int i = 0;
  58   int found = 0;
  59   while ((found == 0) && (i < MAX_CLOCKS)) {
  60     if (clocks_private_info[i].in_use == 0)
  61       found = 1;
  62     else
  63       i++;
  64   }
  65
  66   if (found == 1) {
  67     response = i;
  68     memset(&clocks_private_info[i], 0, sizeof(clock_source_private_data));
  69     strncpy((char *)&clocks_private_info[i].ip, sender_string,
  70             FIELD_SIZEOF(clock_source_private_data, ip) - 1);
  71     clocks_private_info[i].in_use = 1;
  72     clocks_private_info[i].t2 = 0;
  73     clocks_private_info[i].current_stage = waiting_for_sync;
  74     clocks_private_info[i].vacant_samples = MAX_TIMING_SAMPLES;
  75     debug(2, "create record for ip: %s.", &clocks_private_info[i].ip);
  76   } else {
  77     die("Clock tables full!");
  78   }
  79   return response;
  80 }
  81
  82 void manage_clock_sources(uint64_t reception_time, clock_source_private_data *clocks_private_info) {
  83   debug(3, "manage_clock_sources");
  84   int i;
  85
  86   // do a garbage collect for clock records no longer in use
  87   for (i = 0; i < MAX_CLOCKS; i++) {
  88     // only if its in use and not a timing peer... don't need a mutex to check
  89     if ((clocks_private_info[i].in_use != 0) &&
  90         ((clocks_private_info[i].flags & (1 << clock_is_a_timing_peer)) == 0)) {
  91       int64_t time_since_last_use = reception_time - clocks_private_info[i].time_of_last_use;
  92       // using a sync timeout to determine when to drop the record...
  93       // the following give the sync receipt time in whole seconds
  94       // depending on the aPTPinitialLogSyncInterval and the aPTPsyncReceiptTimeout
  95       int64_t syncTimeout = (1 << (32 + aPTPinitialLogSyncInterval));
  96       syncTimeout = syncTimeout * aPTPsyncReceiptTimeout;
  97       syncTimeout = syncTimeout >> 32;
  98       // seconds to nanoseconds
  99       syncTimeout = syncTimeout * 1000000000;
 100       if (time_since_last_use > syncTimeout) {
 101         uint32_t old_flags = clocks_private_info[i].flags;
 102         debug(2, "delete record for: %s.", &clocks_private_info[i].ip);
 103         memset(&clocks_private_info[i], 0, sizeof(clock_source_private_data));
 104         if (old_flags != 0)
 105           update_master();
 106       }
 107     }
 108   }
 109 }
 110
 111 // check all the entries in the clock array and mark all those that
 112 // belong to ourselves
 113
 114 void update_clock_self_identifications(clock_source_private_data *clocks_private_info) {
 115   // first, turn off all the self-id flags
 116   int i;
 117   for (i = 0; i < MAX_CLOCKS; i++) {
 118     clocks_private_info[i].is_one_of_ours = 0;
 119   }
 120
 121   struct ifaddrs *ifap, *ifa;
 122   void *addr = NULL;
 123   short family;
 124   getifaddrs(&ifap);
 125   for (ifa = ifap; ifa; ifa = ifa->ifa_next) {
 126     family = ifa->ifa_addr->sa_family;
 127 #ifdef AF_INET6
 128     if (ifa->ifa_addr && family == AF_INET6) {
 129       struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)ifa->ifa_addr;
 130       addr = &(sa6->sin6_addr);
 131     }
 132 #endif
 133     if (ifa->ifa_addr && family == AF_INET) {
 134       struct sockaddr_in *sa4 = (struct sockaddr_in *)ifa->ifa_addr;
 135       addr = &(sa4->sin_addr);
 136     }
 137     char ip_string[64];
 138     memset(ip_string, 0, sizeof(ip_string));
 139     if (addr != NULL)
 140       inet_ntop(family, addr, ip_string, sizeof(ip_string));
 141     if (strlen(ip_string) != 0) {
 142       // now set the is_one_of_ours flag of any clock with this ip
 143       for (i = 0; i < MAX_CLOCKS; i++) {
 144         if (strcasecmp(ip_string, clocks_private_info[i].ip) == 0) {
 145           debug(2, "found an entry for one of our clocks");
 146           clocks_private_info[i].is_one_of_ours = 1;
 147         }
 148       }
 149     }
 150   }
 151   freeifaddrs(ifap);
 152 }
 153
 154 void update_master() {
 155   int old_master = -1;
 156   // find the current master clock if there is one and turn off all mastership
 157   int i;
 158   for (i = 0; i < MAX_CLOCKS; i++) {
 159     if ((clocks_private[i].flags & (1 << clock_is_master)) != 0)
 160       if (old_master == -1)
 161         old_master = i;                                 // find old master
 162     clocks_private[i].flags &= ~(1 << clock_is_master); // turn them all off
 163   }
 164
 165   int best_so_far = -1;
 166   int timing_peer_count = 0;
 167   uint32_t acceptance_mask =
 168       (1 << clock_is_valid) | (1 << clock_is_qualified) | (1 << clock_is_a_timing_peer);
 169   for (i = 0; i < MAX_CLOCKS; i++) {
 170     if ((clocks_private[i].flags & acceptance_mask) == acceptance_mask) {
 171       // found a possible clock candidate
 172       timing_peer_count++;
 173       if (best_so_far == -1) {
 174         best_so_far = i;
 175       } else {
 176         // do the data set comparison detailed in Figure 27 and Figure 28 on pp89-90
 177         if (clocks_private[i].grandmasterIdentity ==
 178             clocks_private[best_so_far].grandmasterIdentity) {
 179           // should implement Figure 28 here
 180         } else if (clocks_private[i].grandmasterPriority1 <
 181                    clocks_private[best_so_far].grandmasterPriority1) {
 182           best_so_far = i;
 183         } else if (clocks_private[i].grandmasterClass <
 184                    clocks_private[best_so_far].grandmasterClass) {
 185           best_so_far = i;
 186         } else if (clocks_private[i].grandmasterAccuracy <
 187                    clocks_private[best_so_far].grandmasterAccuracy) {
 188           best_so_far = i;
 189         } else if (clocks_private[i].grandmasterVariance <
 190                    clocks_private[best_so_far].grandmasterVariance) {
 191           best_so_far = i;
 192         } else if (clocks_private[i].grandmasterPriority2 <
 193                    clocks_private[best_so_far].grandmasterPriority2) {
 194           best_so_far = i;
 195         } else if (clocks_private[i].grandmasterIdentity <
 196                    clocks_private[best_so_far].grandmasterIdentity) {
 197           best_so_far = i;
 198         }
 199       }
 200     }
 201   }
 202   if (best_so_far == -1) {
 203     // no master clock
 204     if (old_master != -1) {
 205       // but there was a master clock, so remove it
 206       debug(1, "shm interface -- remove master clock designation");
 207       update_master_clock_info(0, NULL, 0, 0);
 208     }
 209     if (timing_peer_count == 0)
 210       debug(2, "No timing peer list found");
 211     else
 212       debug(1, "No master clock not found!");
 213   } else {
 214     // we found a master clock
 215     clocks_private[best_so_far].flags |= (1 << clock_is_master);
 216     // master_clock_index = best_so_far;
 217     if (old_master != best_so_far) {
 218       update_master_clock_info(clocks_private[best_so_far].clock_id,
 219                                (const char *)&clocks_private[best_so_far].ip,
 220                                clocks_private[best_so_far].local_time,
 221                                clocks_private[best_so_far].local_to_source_time_offset);
 222     }
 223   }
 224
 225   int records_in_use = 0;
 226   for (i = 0; i < MAX_CLOCKS; i++)
 227     if (clocks_private[i].in_use != 0)
 228       records_in_use++;
 229   if (records_in_use > 0) {
 230     debug(1, "");
 231     debug(1, "Current NQPTP Status:");
 232     uint32_t peer_mask = (1 << clock_is_a_timing_peer);
 233     uint32_t peer_clock_mask = peer_mask | (1 << clock_is_valid);
 234     uint32_t peer_master_mask = peer_clock_mask | (1 << clock_is_master);
 235     uint32_t non_peer_clock_mask = (1 << clock_is_valid);
 236     uint32_t non_peer_master_mask = non_peer_clock_mask | (1 << clock_is_master);
 237     for (i = 0; i < MAX_CLOCKS; i++) {
 238       if (clocks_private[i].in_use != 0) {
 239         if ((clocks_private[i].flags & peer_master_mask) == peer_master_mask) {
 240           debug(1, "  Peer Master:     %" PRIx64 "  %s.", clocks_private[i].clock_id,
 241                 clocks_private[i].ip);
 242         } else if ((clocks_private[i].flags & peer_clock_mask) == peer_clock_mask) {
 243           debug(1, "  Peer Clock:      %" PRIx64 "  %s.", clocks_private[i].clock_id,
 244                 clocks_private[i].ip);
 245         } else if ((clocks_private[i].flags & peer_mask) == peer_mask) {
 246           debug(1, "  Peer:                              %s.", clocks_private[i].ip);
 247         } else if ((clocks_private[i].flags & non_peer_master_mask) == non_peer_master_mask) {
 248           debug(1, "  Non Peer Master: %" PRIx64 "  %s.", clocks_private[i].clock_id,
 249                 clocks_private[i].ip);
 250         } else if ((clocks_private[i].flags & non_peer_clock_mask) == non_peer_clock_mask) {
 251           debug(1, "  Non Peer Clock:  %" PRIx64 "  %s.", clocks_private[i].clock_id,
 252                 clocks_private[i].ip);
 253         } else {
 254           debug(1, "  Non Peer Record:                   %s.", clocks_private[i].ip);
 255         }
 256       }
 257     }
 258   }
 259 }