nqptp-clock-sources.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
  20 #include "nqptp-clock-sources.h"
  21 #include "debug.h"
  22 #include "general-utilities.h"
  23 #include "nqptp-ptp-definitions.h"
  24 #include <arpa/inet.h>
  25 #include <errno.h>
  26 #include <ifaddrs.h>
  27 #include <netdb.h>
  28 #include <string.h>
  29 #include <sys/socket.h>
  30 #include <sys/types.h> // for ftruncate and others
  31 #include <unistd.h>    // for ftruncate and others
  32
  33 #include <fcntl.h>      /* For O_* constants */
  34 #include <sys/mman.h>   // for shared memory stuff
  35 #include <sys/select.h> // for fd_set
  36 #include <sys/stat.h>   // umask
  37
  38 #ifdef CONFIG_FOR_FREEBSD
  39 #include <netinet/in.h>
  40 #endif
  41
  42 #ifndef FIELD_SIZEOF
  43 #define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))
  44 #endif
  45
  46 int shm_fd;
  47 struct shm_structure *shared_memory;
  48
  49 clock_source_private_data clocks_private[MAX_CLOCKS];
  50 client_record clients[MAX_CLIENTS];
  51
  52 /*
  53 const char *get_client_name(int client_id) {
  54   if ((client_id >= 0) && (client_id < MAX_CLIENTS)) {
  55     return clients[client_id].shm_interface_name;
  56   } else {
  57     return "";
  58   }
  59 }
  60 */
  61
  62 int get_client_id(char *client_shared_memory_interface_name) {
  63   int response = -1; // signify not found
  64   if (client_shared_memory_interface_name != NULL) {
  65     int i = 0;
  66     // first, see if yu can find it anywhere
  67     while ((response == -1) && (i < MAX_CLIENTS)) {
  68       if (strcmp(clients[i].shm_interface_name, client_shared_memory_interface_name) == 0)
  69         response = i;
  70       else
  71         i++;
  72     }
  73
  74     if (response == -1) { // no match, so create one
  75       i = 0;
  76       while ((response == -1) && (i < MAX_CLIENTS)) {
  77         if (clients[i].shm_interface_name[0] == '\0')
  78           response = i;
  79         else
  80           i++;
  81       }
  82       if (response != -1) {
  83         strncpy(clients[i].shm_interface_name, client_shared_memory_interface_name,
  84                 sizeof(clients[i].shm_interface_name));
  85         // create the named smi interface
  86
  87         // open a shared memory interface.
  88         debug(2, "Create a shm interface named \"%s\"", clients[i].shm_interface_name);
  89         clients[i].shm_fd = -1;
  90
  91         mode_t oldumask = umask(0);
  92         clients[i].shm_fd = shm_open(client_shared_memory_interface_name, O_RDWR | O_CREAT, 0666);
  93         if (clients[i].shm_fd == -1) {
  94           die("cannot open shared memory \"%s\".", client_shared_memory_interface_name);
  95         }
  96         (void)umask(oldumask);
  97
  98         if (ftruncate(clients[i].shm_fd, sizeof(struct shm_structure)) == -1) {
  99           die("failed to set size of shared memory \"%s\".", client_shared_memory_interface_name);
 100         }
 101
 102 #ifdef CONFIG_FOR_FREEBSD
 103         clients[i].shared_memory =
 104             (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE,
 105                                          MAP_SHARED, clients[i].shm_fd, 0);
 106 #endif
 107
 108 #ifdef CONFIG_FOR_LINUX
 109         clients[i].shared_memory =
 110             (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE,
 111                                          MAP_LOCKED | MAP_SHARED, clients[i].shm_fd, 0);
 112 #endif
 113
 114         if (clients[i].shared_memory == (struct shm_structure *)-1) {
 115           die("failed to mmap shared memory \"%s\".", client_shared_memory_interface_name);
 116         }
 117
 118         if ((close(clients[i].shm_fd) == -1)) {
 119           warn("error closing \"%s\" after mapping.", client_shared_memory_interface_name);
 120         }
 121
 122         // zero it
 123         memset(clients[i].shared_memory, 0, sizeof(struct shm_structure));
 124         clients[i].shared_memory->version = NQPTP_SHM_STRUCTURES_VERSION;
 125
 126         // for (i = 0; i < MAX_CLOCKS; i++) {
 127         //  clocks_private[i].client_flags[response] =
 128         //      0; // turn off all client flags in every clock for this client
 129         // }
 130       } else {
 131         debug(1, "could not create a client record for client \"%s\".",
 132               client_shared_memory_interface_name);
 133       }
 134     }
 135   } else {
 136     debug(1, "no client_shared_memory_interface_name");
 137   }
 138   debug(2, "get_client_id \"%s\" response %d", client_shared_memory_interface_name, response);
 139   return response;
 140 }
 141
 142 int delete_client(int client_id) {
 143   int response = 0; // okay unless something happens
 144   if (clients[client_id].shm_interface_name[0] != '\0') {
 145     if (clients[client_id].shared_memory != NULL) {
 146       // mmap cleanup
 147       if (munmap(clients[client_id].shared_memory, sizeof(struct shm_structure)) != 0) {
 148         debug(1, "error unmapping shared memory");
 149         response = -1;
 150       }
 151       // shm_open cleanup
 152       if (shm_unlink(clients[client_id].shm_interface_name) == -1) {
 153         debug(1, "error unlinking shared memory \"%s\"", clients[client_id].shm_interface_name);
 154         response = -1;
 155       }
 156     }
 157     clients[client_id].shm_interface_name[0] = '\0'; // remove the name to signify it's vacant
 158   }
 159   return response;
 160 }
 161
 162 int delete_clients() {
 163   int response = 0; // okay unless something happens
 164   int i;
 165   for (i = 0; i < MAX_CLIENTS; i++)
 166     if (delete_client(i) != 0)
 167       response = -1;
 168   return response;
 169 }
 170
 171 int find_clock_source_record(char *sender_string, clock_source_private_data *clocks_private_info) {
 172   // return the index of the clock in the clock information arrays or -1
 173   int response = -1;
 174   int i = 0;
 175   int found = 0;
 176   while ((found == 0) && (i < MAX_CLOCKS)) {
 177     if (((clocks_private_info[i].flags & (1 << clock_is_in_use)) != 0) &&
 178         (strcasecmp(sender_string, (const char *)&clocks_private_info[i].ip) == 0))
 179       found = 1;
 180     else
 181       i++;
 182   }
 183   if (found == 1)
 184     response = i;
 185   return response;
 186 }
 187
 188 int create_clock_source_record(char *sender_string,
 189                                clock_source_private_data *clocks_private_info) {
 190   // return the index of a clock entry in the clock information arrays or -1 if full
 191   // initialise the entries in the shared and private arrays
 192   int response = -1;
 193   int i = 0;
 194   int found = 0; // trying to find an unused entry
 195   while ((found == 0) && (i < MAX_CLOCKS)) {
 196     if ((clocks_private_info[i].flags & (1 << clock_is_in_use)) == 0)
 197       found = 1;
 198     else
 199       i++;
 200   }
 201
 202   if (found == 1) {
 203     int family = 0;
 204
 205     // check its ipv4/6 family -- derived from https://stackoverflow.com/a/3736377, with thanks.
 206     struct addrinfo hint, *res = NULL;
 207     memset(&hint, '\0', sizeof hint);
 208     hint.ai_family = PF_UNSPEC;
 209     hint.ai_flags = AI_NUMERICHOST;
 210     if (getaddrinfo(sender_string, NULL, &hint, &res) == 0) {
 211       family = res->ai_family;
 212       freeaddrinfo(res);
 213       response = i;
 214       memset(&clocks_private_info[i], 0, sizeof(clock_source_private_data));
 215       strncpy((char *)&clocks_private_info[i].ip, sender_string,
 216               FIELD_SIZEOF(clock_source_private_data, ip) - 1);
 217       clocks_private_info[i].family = family;
 218       clocks_private_info[i].flags |= (1 << clock_is_in_use);
 219       debug(2, "create record for ip: %s, family: %s.", &clocks_private_info[i].ip,
 220             clocks_private_info[i].family == AF_INET6 ? "IPv6" : "IPv4");
 221     } else {
 222       debug(1, "cannot getaddrinfo for ip: %s.", &clocks_private_info[i].ip);
 223     }
 224   } else {
 225     debug(1, "Clock tables full!");
 226   }
 227   return response;
 228 }
 229
 230 void update_master_clock_info(uint64_t master_clock_id, const char *ip, uint64_t local_time,
 231                               uint64_t local_to_master_offset, uint64_t mastership_start_time) {
 232   // to ensure that a full update has taken place, the
 233   // reader must ensure that the main and secondary
 234   // structures are identical
 235
 236   shared_memory->main.master_clock_id = master_clock_id;
 237   if (ip != NULL) {
 238     shared_memory->main.master_clock_start_time = mastership_start_time;
 239     shared_memory->main.local_time = local_time;
 240     shared_memory->main.local_to_master_time_offset = local_to_master_offset;
 241   } else {
 242     shared_memory->main.master_clock_start_time = 0;
 243     shared_memory->main.local_time = 0;
 244     shared_memory->main.local_to_master_time_offset = 0;
 245   }
 246   __sync_synchronize();
 247   shared_memory->secondary = shared_memory->main;
 248   __sync_synchronize();
 249 }