nqptp.c

   1 /*
   2  * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
   3  * Copyright (c) 2021-2022 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 #include "nqptp.h"
  20 #include "config.h"
  21 #include "debug.h"
  22 #include "general-utilities.h"
  23 #include "nqptp-clock-sources.h"
  24 #include "nqptp-message-handlers.h"
  25 #include "nqptp-ptp-definitions.h"
  26 #include "nqptp-utilities.h"
  27
  28 #ifdef CONFIG_USE_GIT_VERSION_STRING
  29 #include "gitversion.h"
  30 #endif
  31
  32 #include <arpa/inet.h> // inet_ntop
  33 #include <stdio.h>     // fprint
  34 #include <stdlib.h>    // malloc;
  35 #include <string.h>    // memset
  36
  37 #include <errno.h>
  38 #include <unistd.h> // close
  39
  40 #include <fcntl.h>      /* For O_* constants */
  41 #include <sys/mman.h>   // for shared memory stuff
  42 #include <sys/select.h> // for fd_set
  43 #include <sys/stat.h>   // umask
  44 #include <time.h>       // for timeval
  45
  46 #include <signal.h> // SIGTERM and stuff like that
  47
  48 #include <netdb.h>
  49 #include <sys/socket.h>
  50
  51 #ifdef CONFIG_FOR_FREEBSD
  52 #include <netinet/in.h>
  53 #include <sys/socket.h>
  54 #endif
  55
  56 #ifndef FIELD_SIZEOF
  57 #define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))
  58 #endif
  59
  60 // 8 samples per second
  61
  62 #define BUFLEN 4096 // Max length of buffer
  63
  64 sockets_open_bundle sockets_open_stuff;
  65
  66 typedef struct {
  67   uint64_t trigger_time;
  68   uint64_t (*task)(uint64_t nominal_call_time, void *private_data);
  69   void *private_data;
  70 } timed_task_t;
  71
  72 #define TIMED_TASKS 1
  73
  74 timed_task_t timed_tasks[TIMED_TASKS];
  75
  76 /*
  77 uint64_t sample_task(uint64_t call_time, __attribute__((unused)) void *private_data) {
  78   debug(1,"sample_task called.");
  79   uint64_t next_time = call_time;
  80   next_time = next_time + 1000000000;
  81   return next_time;
  82 }
  83 */
  84
  85 int epoll_fd;
  86
  87 void goodbye(void) {
  88   // close any open sockets
  89   unsigned int i;
  90   for (i = 0; i < sockets_open_stuff.sockets_open; i++)
  91     close(sockets_open_stuff.sockets[i].number);
  92
  93   // close off shared memory interface
  94   delete_clients();
  95
  96   // close off new smi
  97   // mmap cleanup
  98   if (munmap(shared_memory, sizeof(struct shm_structure)) != 0) {
  99     debug(1, "error unmapping shared memory \"%s\": \"%s\".", NQPTP_INTERFACE_NAME, strerror(errno));
 100   }
 101   // shm_open cleanup
 102   if (shm_unlink(NQPTP_INTERFACE_NAME) == -1) {
 103     debug(1, "error unlinking shared memory \"%s\": \"%s\".", NQPTP_INTERFACE_NAME, strerror(errno));
 104   }
 105
 106   if (shm_fd != -1)
 107     close(shm_fd);
 108
 109   if (epoll_fd != -1)
 110     close(epoll_fd);
 111
 112   debug(1, "goodbye");
 113 }
 114
 115 void intHandler(__attribute__((unused)) int k) {
 116   debug(1, "exit on SIGINT");
 117   exit(EXIT_SUCCESS);
 118 }
 119
 120 void termHandler(__attribute__((unused)) int k) {
 121   debug(1, "exit on SIGTERM");
 122   exit(EXIT_SUCCESS);
 123 }
 124
 125 int main(int argc, char **argv) {
 126
 127   int debug_level = 0;
 128   int i;
 129   for (i = 1; i < argc; ++i) {
 130     if (argv[i][0] == '-') {
 131       if (strcmp(argv[i] + 1, "V") == 0) {
 132 #ifdef CONFIG_USE_GIT_VERSION_STRING
 133         if (git_version_string[0] != '\0')
 134           fprintf(stdout, "Version: %s. Shared Memory Interface Version: %u.\n", git_version_string,
 135                   NQPTP_SHM_STRUCTURES_VERSION);
 136         else
 137 #endif
 138
 139           fprintf(stdout, "Version: %s. Shared Memory Interface Version: %u.\n", VERSION,
 140                   NQPTP_SHM_STRUCTURES_VERSION);
 141         exit(EXIT_SUCCESS);
 142       } else if (strcmp(argv[i] + 1, "vvv") == 0) {
 143         debug_level = 3;
 144       } else if (strcmp(argv[i] + 1, "vv") == 0) {
 145         debug_level = 2;
 146       } else if (strcmp(argv[i] + 1, "v") == 0) {
 147         debug_level = 1;
 148       } else if (strcmp(argv[i] + 1, "h") == 0) {
 149         fprintf(stdout, "    -V     print version,\n"
 150                         "    -v     verbose log,\n"
 151                         "    -vv    more verbose log,\n"
 152                         "    -vvv   very verbose log,\n"
 153                         "    -h     this help text.\n");
 154         exit(EXIT_SUCCESS);
 155       } else {
 156         fprintf(stdout, "%s -- unknown option. Program terminated.\n", argv[0]);
 157         exit(EXIT_FAILURE);
 158       }
 159     }
 160   }
 161
 162   debug_init(debug_level, 0, 1, 1);
 163
 164 #ifdef CONFIG_USE_GIT_VERSION_STRING
 165   if (git_version_string[0] != '\0')
 166     debug(1, "Version: %s, SMI: %u. Clock ID: \"%" PRIx64 "\".", git_version_string,
 167           NQPTP_SHM_STRUCTURES_VERSION, get_self_clock_id());
 168   else
 169 #endif
 170     debug(1, "Version: %s, SMI: %u. Clock ID: \"%" PRIx64 "\".", VERSION,
 171           NQPTP_SHM_STRUCTURES_VERSION, get_self_clock_id());
 172
 173   // debug(1, "size of a clock entry is %u bytes.", sizeof(clock_source_private_data));
 174   atexit(goodbye);
 175
 176   sockets_open_stuff.sockets_open = 0;
 177
 178   epoll_fd = -1;
 179
 180   // control-c (SIGINT) cleanly
 181   struct sigaction act;
 182   memset(&act, 0, sizeof(struct sigaction));
 183   act.sa_handler = intHandler;
 184   sigaction(SIGINT, &act, NULL);
 185
 186   // terminate (SIGTERM)
 187   struct sigaction act2;
 188   memset(&act2, 0, sizeof(struct sigaction));
 189   act2.sa_handler = termHandler;
 190   sigaction(SIGTERM, &act2, NULL);
 191
 192   // open the SMI
 193
 194   pthread_mutexattr_t shared;
 195   int err;
 196
 197   shm_fd = -1;
 198
 199   mode_t oldumask = umask(0);
 200   shm_fd = shm_open(NQPTP_INTERFACE_NAME, O_RDWR | O_CREAT, 0666);
 201   if (shm_fd == -1) {
 202     die("cannot open shared memory \"%s\".", NQPTP_INTERFACE_NAME);
 203   }
 204   (void)umask(oldumask);
 205
 206   if (ftruncate(shm_fd, sizeof(struct shm_structure)) == -1) {
 207     die("failed to set size of shared memory \"%s\".", NQPTP_INTERFACE_NAME);
 208   }
 209
 210 #ifdef CONFIG_FOR_FREEBSD
 211   shared_memory = (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure),
 212                                                PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0);
 213 #endif
 214
 215 #ifdef CONFIG_FOR_LINUX
 216   shared_memory =
 217       (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE,
 218                                    MAP_LOCKED | MAP_SHARED, shm_fd, 0);
 219 #endif
 220
 221   if (shared_memory == (struct shm_structure *)-1) {
 222     die("failed to mmap shared memory \"%s\".", NQPTP_INTERFACE_NAME);
 223   }
 224
 225   if (shm_fd == -1) {
 226     warn("error closing \"%s\" after mapping.", shm_fd);
 227   }
 228
 229   // zero it
 230   memset(shared_memory, 0, sizeof(struct shm_structure));
 231   shared_memory->version = NQPTP_SHM_STRUCTURES_VERSION;
 232
 233   /*create mutex attr */
 234   err = pthread_mutexattr_init(&shared);
 235   if (err != 0) {
 236     die("mutex attribute initialization failed - %s.", strerror(errno));
 237   }
 238   pthread_mutexattr_setpshared(&shared, 1);
 239   /*create a mutex */
 240   err = pthread_mutex_init((pthread_mutex_t *)&shared_memory->shm_mutex, &shared);
 241   if (err != 0) {
 242     die("mutex initialization failed - %s.", strerror(errno));
 243   }
 244
 245   err = pthread_mutexattr_destroy(&shared);
 246   if (err != 0) {
 247     die("mutex attribute destruction failed - %s.", strerror(errno));
 248   }
 249
 250   ssize_t recv_len;
 251
 252   char buf[BUFLEN];
 253
 254   // open sockets 319 and 320
 255
 256   open_sockets_at_port(NULL, 319, &sockets_open_stuff);
 257   open_sockets_at_port(NULL, 320, &sockets_open_stuff);
 258   open_sockets_at_port("localhost", NQPTP_CONTROL_PORT,
 259                        &sockets_open_stuff); // this for messages from the client
 260
 261   // start the timed tasks
 262   uint64_t broadcasting_task(uint64_t call_time, void *private_data);
 263
 264   timed_tasks[0].trigger_time = get_time_now() + 100000000; // start after 100 ms
 265   timed_tasks[0].private_data = (void *)&clocks_private;
 266   timed_tasks[0].task = broadcasting_task;
 267
 268   // now, get down to business
 269   if (sockets_open_stuff.sockets_open > 0) {
 270
 271     while (1) {
 272       fd_set readSockSet;
 273       struct timeval timeout;
 274       FD_ZERO(&readSockSet);
 275       int smax = -1;
 276       unsigned int s;
 277       for (s = 0; s < sockets_open_stuff.sockets_open; s++) {
 278         if (sockets_open_stuff.sockets[s].number > smax)
 279           smax = sockets_open_stuff.sockets[s].number;
 280         FD_SET(sockets_open_stuff.sockets[s].number, &readSockSet);
 281       }
 282
 283       timeout.tv_sec = 0;
 284       timeout.tv_usec = 10000; // timeout after ten milliseconds
 285       int retval = select(smax + 1, &readSockSet, NULL, NULL, &timeout);
 286       uint64_t reception_time = get_time_now(); // use this if other methods fail
 287       if (retval > 0) {
 288         unsigned t;
 289         for (t = 0; t < sockets_open_stuff.sockets_open; t++) {
 290           int socket_number = sockets_open_stuff.sockets[t].number;
 291           if (FD_ISSET(socket_number, &readSockSet)) {
 292
 293             SOCKADDR from_sock_addr;
 294             memset(&from_sock_addr, 0, sizeof(SOCKADDR));
 295
 296             struct {
 297               struct cmsghdr cm;
 298               char control[512];
 299             } control;
 300
 301             struct msghdr msg;
 302             struct iovec iov[1];
 303             memset(iov, 0, sizeof(iov));
 304             memset(&msg, 0, sizeof(msg));
 305             memset(&control, 0, sizeof(control));
 306
 307             iov[0].iov_base = buf;
 308             iov[0].iov_len = BUFLEN;
 309
 310             msg.msg_iov = iov;
 311             msg.msg_iovlen = 1;
 312
 313             msg.msg_name = &from_sock_addr;
 314             msg.msg_namelen = sizeof(from_sock_addr);
 315             msg.msg_control = &control;
 316             msg.msg_controllen = sizeof(control);
 317
 318             uint16_t receiver_port = 0;
 319             // int msgsize = recv(udpsocket_fd, &msg_buffer, 4, 0);
 320             recv_len = recvmsg(socket_number, &msg, MSG_DONTWAIT);
 321
 322             if (recv_len != -1) {
 323               // get the receiver port
 324               unsigned int jp;
 325               for (jp = 0; jp < sockets_open_stuff.sockets_open; jp++) {
 326                 if (socket_number == sockets_open_stuff.sockets[jp].number)
 327                   receiver_port = sockets_open_stuff.sockets[jp].port;
 328               }
 329             }
 330             if (recv_len == -1) {
 331               if (errno == EAGAIN) {
 332                 usleep(1000); // this can happen, it seems...
 333               } else {
 334                 debug(1, "recvmsg() error %d", errno);
 335               }
 336               // check if it's a control port message before checking for the length of the
 337               // message.
 338             } else if (receiver_port == NQPTP_CONTROL_PORT) {
 339               handle_control_port_messages(
 340                   buf, recv_len, (clock_source_private_data *)&clocks_private, reception_time);
 341             } else if (recv_len >= (ssize_t)sizeof(struct ptp_common_message_header)) {
 342               debug_print_buffer(2, buf, recv_len);
 343
 344               // check its credentials
 345               // the sending and receiving ports must be the same (i.e. 319 -> 319 or 320 -> 320)
 346
 347               // initialise the connection info
 348               void *sender_addr = NULL;
 349               uint16_t sender_port = 0;
 350
 351               sa_family_t connection_ip_family = from_sock_addr.SAFAMILY;
 352
 353 #ifdef AF_INET6
 354               if (connection_ip_family == AF_INET6) {
 355                 struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)&from_sock_addr;
 356                 sender_addr = &(sa6->sin6_addr);
 357                 sender_port = ntohs(sa6->sin6_port);
 358               }
 359 #endif
 360               if (connection_ip_family == AF_INET) {
 361                 struct sockaddr_in *sa4 = (struct sockaddr_in *)&from_sock_addr;
 362                 sender_addr = &(sa4->sin_addr);
 363                 sender_port = ntohs(sa4->sin_port);
 364               }
 365
 366               if (sender_port == receiver_port) {
 367
 368                 char sender_string[256];
 369                 memset(sender_string, 0, sizeof(sender_string));
 370                 inet_ntop(connection_ip_family, sender_addr, sender_string, sizeof(sender_string));
 371                 // now, find the record for this ip
 372                 int the_clock = find_clock_source_record(
 373                     sender_string, (clock_source_private_data *)&clocks_private);
 374                 // not sure about requiring a Sync before creating it...
 375                 // if ((the_clock == -1) && ((buf[0] & 0xF) == Sync)) {
 376                 /*
 377                 if (the_clock == -1) {
 378                   the_clock = create_clock_source_record(
 379                       sender_string, (clock_source_private_data *)&clocks_private);
 380                 }
 381                 */
 382                 if (the_clock != -1) {
 383                   clocks_private[the_clock].time_of_last_use =
 384                       reception_time; // for garbage collection
 385                   switch (buf[0] & 0xF) {
 386                   case Announce:
 387                     handle_announce(buf, recv_len, &clocks_private[the_clock], reception_time);
 388                     break;
 389                   case Follow_Up:
 390                     handle_follow_up(buf, recv_len, &clocks_private[the_clock], reception_time);
 391                     break;
 392                   case Sync:
 393                     handle_sync(buf, recv_len, &clocks_private[the_clock], reception_time);
 394                     break;
 395                   default:
 396                     debug_print_buffer(2, buf,
 397                                        recv_len); // unusual messages will have debug level 1.
 398                     break;
 399                   }
 400                 } // otherwise, just forget it
 401               }
 402             }
 403           }
 404         }
 405       }
 406       // if (retval >= 0)
 407       //  manage_clock_sources(reception_time, (clock_source_private_data *)&clocks_private);
 408       int i;
 409       for (i = 0; i < TIMED_TASKS; i++) {
 410         if (timed_tasks[i].trigger_time != 0) {
 411           int64_t time_to_wait = timed_tasks[i].trigger_time - reception_time;
 412           if (time_to_wait <= 0) {
 413             timed_tasks[i].trigger_time =
 414                 timed_tasks[i].task(reception_time, timed_tasks[i].private_data);
 415           }
 416         }
 417       }
 418     }
 419   }
 420   // should never get to here, unless no sockets were ever opened.
 421   return 0;
 422 }
 423
 424 void send_awakening_announcement_sequence(const uint64_t clock_id, const char *clock_ip,
 425                                           const int ip_family, const uint8_t priority1,
 426                                           const uint8_t priority2) {
 427   struct ptp_announce_message *msg;
 428   size_t msg_length = sizeof(struct ptp_announce_message);
 429   msg = malloc(msg_length);
 430   memset((void *)msg, 0, msg_length);
 431
 432   uint64_t my_clock_id = get_self_clock_id();
 433   msg->header.transportSpecificAndMessageID = 0x10 + Announce;
 434   msg->header.reservedAndVersionPTP = 0x02;
 435   msg->header.messageLength = htons(sizeof(struct ptp_announce_message));
 436   msg->header.flags = htons(0x0408);
 437   hcton64(my_clock_id, &msg->header.clockIdentity[0]);
 438   msg->header.sourcePortID = htons(32776);
 439   msg->header.controlField = 0x05;
 440   msg->header.logMessagePeriod = 0xFE;
 441   msg->announce.currentUtcOffset = htons(37);
 442   hcton64(my_clock_id, &msg->announce.grandmasterIdentity[0]);
 443   uint32_t my_clock_quality = 0xf8fe436a;
 444   msg->announce.grandmasterClockQuality = htonl(my_clock_quality);
 445   if (priority1 > 2) {
 446     msg->announce.grandmasterPriority1 =
 447         priority1 - 1; // make this announcement seem better than the clock we are about to ping
 448     msg->announce.grandmasterPriority2 = priority2;
 449   } else {
 450     warn("Cannot select a suitable priority for pinging clock %" PRIx64 " at %s.", clock_id,
 451          clock_ip);
 452     msg->announce.grandmasterPriority1 = 248;
 453     msg->announce.grandmasterPriority2 = 248;
 454   }
 455   msg->announce.timeSource = 160; // Internal Oscillator
 456
 457   // get the socket for the correct port -- 320 -- and family -- IPv4 or IPv6 -- to send it
 458   // from.
 459
 460   int s = 0;
 461   unsigned t;
 462   for (t = 0; t < sockets_open_stuff.sockets_open; t++) {
 463     if ((sockets_open_stuff.sockets[t].port == 320) &&
 464         (sockets_open_stuff.sockets[t].family == ip_family))
 465       s = sockets_open_stuff.sockets[t].number;
 466   }
 467   if (s == 0) {
 468     debug(1, "sending socket not found for clock %" PRIx64 " at %s, family %s.", clock_id, clock_ip,
 469           ip_family == AF_INET    ? "IPv4"
 470           : ip_family == AF_INET6 ? "IPv6"
 471                                   : "Unknown");
 472   } else {
 473     // debug(1, "Send message from socket %d.", s);
 474
 475     const char *portname = "320";
 476     struct addrinfo hints;
 477     memset(&hints, 0, sizeof(hints));
 478     hints.ai_family = AF_UNSPEC;
 479     hints.ai_socktype = SOCK_DGRAM;
 480     hints.ai_protocol = 0;
 481     hints.ai_flags = AI_ADDRCONFIG;
 482     struct addrinfo *res = NULL;
 483     int err = getaddrinfo(clock_ip, portname, &hints, &res);
 484     if (err != 0) {
 485       debug(1, "failed to resolve remote socket address (err=%d)", err);
 486     } else {
 487       // here, we have the destination, so send it
 488
 489       // debug_print_buffer(1, (char *)msg, msg_length);
 490       int ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
 491       if (ret == -1)
 492         debug(1, "result of sendto is %d.", ret);
 493       debug(2, "Send awaken Announce message to clock \"%" PRIx64 "\" at %s on %s.", clock_id,
 494             clock_ip, ip_family == AF_INET6 ? "IPv6" : "IPv4");
 495
 496       if (priority1 < 254) {
 497         msg->announce.grandmasterPriority1 =
 498             priority1 + 1; // make this announcement seem worse than the clock we about to ping
 499       } else {
 500         warn("Cannot select a suitable priority for second ping of clock %" PRIx64 " at %s.",
 501              clock_id, clock_ip);
 502         msg->announce.grandmasterPriority1 = 250;
 503       }
 504
 505       msg->announce.grandmasterPriority2 = priority2;
 506       usleep(150000);
 507       ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
 508       if (ret == -1)
 509         debug(1, "result of second sendto is %d.", ret);
 510       freeaddrinfo(res);
 511     }
 512   }
 513   free(msg);
 514 }
 515
 516 uint64_t broadcasting_task(uint64_t call_time, __attribute__((unused)) void *private_data) {
 517   clock_source_private_data *clocks_private = (clock_source_private_data *)private_data;
 518   int i;
 519   for (i = 0; i < MAX_CLOCKS; i++) {
 520
 521     /*
 522         int is_a_master = 0;
 523         int temp_client_id;
 524
 525         for (temp_client_id = 0; temp_client_id < MAX_CLIENTS; temp_client_id++)
 526           if ((clocks_private->client_flags[temp_client_id] & (1 << clock_is_master)) != 0)
 527             is_a_master = 1;
 528         // only process it if it's a master somewhere...
 529         if ((is_a_master != 0) && (clocks_private[i].announcements_without_followups == 3)) {
 530     */
 531     if (clocks_private[i].announcements_without_followups == 3) {
 532       if (clocks_private[i].follow_up_number == 0) {
 533         debug(1,
 534               "Attempt to awaken a silent clock %" PRIx64
 535               ", index %u, at follow_up_number %u at IP %s.",
 536               clocks_private[i].clock_id, i, clocks_private[i].follow_up_number,
 537               clocks_private[i].ip);
 538
 539         // send an Announce message to attempt to waken this silent PTP clock by
 540         // getting it to negotiate with an apparently better clock
 541         // that then immediately sends another Announce message indicating that it's inferior
 542
 543         clocks_private[i].announcements_without_followups++; // set to 4 to indicate done/parked
 544         send_awakening_announcement_sequence(
 545             clocks_private[i].clock_id, clocks_private[i].ip, clocks_private[i].family,
 546             clocks_private[i].grandmasterPriority1, clocks_private[i].grandmasterPriority2);
 547       } else {
 548         debug(1,
 549               "Silent clock %" PRIx64
 550               " detected, index %u, at follow_up_number %u at IP %s. No attempt to awaken it.",
 551               clocks_private[i].clock_id, i, clocks_private[i].follow_up_number,
 552               clocks_private[i].ip);
 553       }
 554     }
 555   }
 556
 557   /*
 558     uint64_t announce_interval = 1;
 559     announce_interval = announce_interval << (8 + aPTPinitialLogAnnounceInterval);
 560     announce_interval = announce_interval * 1000000000;
 561     announce_interval = announce_interval >> 8; // nanoseconds
 562     return call_time + announce_interval;
 563   */
 564   return call_time + 50000000;
 565 }