[thirdparty/nqptp.git] / nqptp-clock-sources.c

/*
 * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
 * Copyright (c) 2021-2022 Mike Brady.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 2.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * Commercial licensing is also available.
 */

#include "nqptp-clock-sources.h"
#include "debug.h"
#include "general-utilities.h"
#include "nqptp-ptp-definitions.h"
#include <arpa/inet.h>
#include <errno.h>
#include <ifaddrs.h>
#include <netdb.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h> // for ftruncate and others
#include <unistd.h>    // for ftruncate and others

#include <fcntl.h>      /* For O_* constants */
#include <sys/mman.h>   // for shared memory stuff
#include <sys/select.h> // for fd_set
#include <sys/stat.h>   // umask

#ifdef CONFIG_FOR_FREEBSD
#include <netinet/in.h>
#endif

#ifndef FIELD_SIZEOF
#define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))
#endif

int shm_fd;
struct shm_structure *shared_memory;

clock_source_private_data clocks_private[MAX_CLOCKS];
client_record clients[MAX_CLIENTS];

/*
const char *get_client_name(int client_id) {
  if ((client_id >= 0) && (client_id < MAX_CLIENTS)) {
    return clients[client_id].shm_interface_name;
  } else {
    return "";
  }
}
*/

int get_client_id(char *client_shared_memory_interface_name) {
  int response = -1; // signify not found
  if (client_shared_memory_interface_name != NULL) {
    int i = 0;
    // first, see if yu can find it anywhere
    while ((response == -1) && (i < MAX_CLIENTS)) {
      if (strcmp(clients[i].shm_interface_name, client_shared_memory_interface_name) == 0)
        response = i;
      else
        i++;
    }

    if (response == -1) { // no match, so create one
      i = 0;
      while ((response == -1) && (i < MAX_CLIENTS)) {
        if (clients[i].shm_interface_name[0] == '\0')
          response = i;
        else
          i++;
      }
      if (response != -1) {
        strncpy(clients[i].shm_interface_name, client_shared_memory_interface_name,
                sizeof(clients[i].shm_interface_name));
        // create the named smi interface

        // open a shared memory interface.
        debug(2, "Create a shm interface named \"%s\"", clients[i].shm_interface_name);
        clients[i].shm_fd = -1;

        mode_t oldumask = umask(0);
        clients[i].shm_fd = shm_open(client_shared_memory_interface_name, O_RDWR | O_CREAT, 0666);
        if (clients[i].shm_fd == -1) {
          die("cannot open shared memory \"%s\".", client_shared_memory_interface_name);
        }
        (void)umask(oldumask);

        if (ftruncate(clients[i].shm_fd, sizeof(struct shm_structure)) == -1) {
          die("failed to set size of shared memory \"%s\".", client_shared_memory_interface_name);
        }

#ifdef CONFIG_FOR_FREEBSD
        clients[i].shared_memory =
            (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE,
                                         MAP_SHARED, clients[i].shm_fd, 0);
#endif

#ifdef CONFIG_FOR_LINUX
        clients[i].shared_memory =
            (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE,
                                         MAP_LOCKED | MAP_SHARED, clients[i].shm_fd, 0);
#endif

        if (clients[i].shared_memory == (struct shm_structure *)-1) {
          die("failed to mmap shared memory \"%s\".", client_shared_memory_interface_name);
        }

        if ((close(clients[i].shm_fd) == -1)) {
          warn("error closing \"%s\" after mapping.", client_shared_memory_interface_name);
        }

        // zero it
        memset(clients[i].shared_memory, 0, sizeof(struct shm_structure));
        clients[i].shared_memory->version = NQPTP_SHM_STRUCTURES_VERSION;

        // for (i = 0; i < MAX_CLOCKS; i++) {
        //  clocks_private[i].client_flags[response] =
        //      0; // turn off all client flags in every clock for this client
        // }
      } else {
        debug(1, "could not create a client record for client \"%s\".",
              client_shared_memory_interface_name);
      }
    }
  } else {
    debug(1, "no client_shared_memory_interface_name");
  }
  debug(2, "get_client_id \"%s\" response %d", client_shared_memory_interface_name, response);
  return response;
}

int delete_client(int client_id) {
  int response = 0; // okay unless something happens
  if (clients[client_id].shm_interface_name[0] != '\0') {
    if (clients[client_id].shared_memory != NULL) {
      // mmap cleanup
      if (munmap(clients[client_id].shared_memory, sizeof(struct shm_structure)) != 0) {
        debug(1, "error unmapping shared memory");
        response = -1;
      }
      // shm_open cleanup
      if (shm_unlink(clients[client_id].shm_interface_name) == -1) {
        debug(1, "error unlinking shared memory \"%s\"", clients[client_id].shm_interface_name);
        response = -1;
      }
    }
    clients[client_id].shm_interface_name[0] = '\0'; // remove the name to signify it's vacant
  }
  return response;
}

int delete_clients() {
  int response = 0; // okay unless something happens
  int i;
  for (i = 0; i < MAX_CLIENTS; i++)
    if (delete_client(i) != 0)
      response = -1;
  return response;
}

int find_clock_source_record(char *sender_string, clock_source_private_data *clocks_private_info) {
  // return the index of the clock in the clock information arrays or -1
  int response = -1;
  int i = 0;
  int found = 0;
  while ((found == 0) && (i < MAX_CLOCKS)) {
    if (((clocks_private_info[i].flags & (1 << clock_is_in_use)) != 0) &&
        (strcasecmp(sender_string, (const char *)&clocks_private_info[i].ip) == 0))
      found = 1;
    else
      i++;
  }
  if (found == 1)
    response = i;
  return response;
}

int create_clock_source_record(char *sender_string,
                               clock_source_private_data *clocks_private_info) {
  // return the index of a clock entry in the clock information arrays or -1 if full
  // initialise the entries in the shared and private arrays
  int response = -1;
  int i = 0;
  int found = 0; // trying to find an unused entry
  while ((found == 0) && (i < MAX_CLOCKS)) {
    if ((clocks_private_info[i].flags & (1 << clock_is_in_use)) == 0)
      found = 1;
    else
      i++;
  }

  if (found == 1) {
    int family = 0;

    // check its ipv4/6 family -- derived from https://stackoverflow.com/a/3736377, with thanks.
    struct addrinfo hint, *res = NULL;
    memset(&hint, '\0', sizeof hint);
    hint.ai_family = PF_UNSPEC;
    hint.ai_flags = AI_NUMERICHOST;
    if (getaddrinfo(sender_string, NULL, &hint, &res) == 0) {
      family = res->ai_family;
      freeaddrinfo(res);
      response = i;
      memset(&clocks_private_info[i], 0, sizeof(clock_source_private_data));
      strncpy((char *)&clocks_private_info[i].ip, sender_string,
              FIELD_SIZEOF(clock_source_private_data, ip) - 1);
      clocks_private_info[i].family = family;
      clocks_private_info[i].flags |= (1 << clock_is_in_use);
      debug(2, "create record for ip: %s, family: %s.", &clocks_private_info[i].ip,
            clocks_private_info[i].family == AF_INET6 ? "IPv6" : "IPv4");
    } else {
      debug(1, "cannot getaddrinfo for ip: %s.", &clocks_private_info[i].ip);
    }
  } else {
    debug(1, "Clock tables full!");
  }
  return response;
}

void update_master_clock_info(uint64_t master_clock_id, const char *ip, uint64_t local_time,
                              uint64_t local_to_master_offset, uint64_t mastership_start_time) {
  // to ensure that a full update has taken place, the
  // reader must ensure that the main and secondary
  // structures are identical

  shared_memory->main.master_clock_id = master_clock_id;
  if (ip != NULL) {
    shared_memory->main.master_clock_start_time = mastership_start_time;
    shared_memory->main.local_time = local_time;
    shared_memory->main.local_to_master_time_offset = local_to_master_offset;
  } else {
    shared_memory->main.master_clock_start_time = 0;
    shared_memory->main.local_time = 0;
    shared_memory->main.local_to_master_time_offset = 0;
  }
  __sync_synchronize();
  shared_memory->secondary = shared_memory->main;
  __sync_synchronize();
}
Commit	Line	Data
68cae740 MB	1	/*
68cae740 MB	2	* This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
1c610279	3	* Copyright (c) 2021-2022 Mike Brady.
68cae740 MB	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
68cae740 MB	20	#include "nqptp-clock-sources.h"
68cae740 MB	21	#include "debug.h"
83ff626d	22	#include "general-utilities.h"
01fa06a8	23	#include "nqptp-ptp-definitions.h"
82e5414e	24	#include <arpa/inet.h>
77116ab0	25	#include <errno.h>
82e5414e	26	#include <ifaddrs.h>
73164f4f	27	#include <netdb.h>
82e5414e	28	#include <string.h>
5e02deec	29	#include <sys/socket.h>
96dd6c2b MB	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
68cae740	37
c02098d4	38	#ifdef CONFIG_FOR_FREEBSD
c02098d4 MB	39	#include <netinet/in.h>
	40	#endif
	41
68cae740	42	#ifndef FIELD_SIZEOF
82e5414e	43	#define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f))
68cae740 MB	44	#endif
68cae740 MB	45
1c610279 MB	46	int shm_fd;
	47	struct shm_structure *shared_memory;
	48
339e00f6	49	clock_source_private_data clocks_private[MAX_CLOCKS];
96dd6c2b MB	50	client_record clients[MAX_CLIENTS];
96dd6c2b MB	51
fb9928f7	52	/*
3e1c4ac1 MB	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	}
fb9928f7	60	*/
3e1c4ac1	61
96dd6c2b MB	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	}
f7f6ad2a	73
96dd6c2b MB	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) {
96dd6c2b MB	83	strncpy(clients[i].shm_interface_name, client_shared_memory_interface_name,
96dd6c2b MB	84	sizeof(clients[i].shm_interface_name));
5d111219	85	// create the named smi interface
96dd6c2b MB	86
96dd6c2b MB	87	// open a shared memory interface.
5d111219	88	debug(2, "Create a shm interface named \"%s\"", clients[i].shm_interface_name);
96dd6c2b MB	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
fb9928f7 MB	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	// }
96dd6c2b MB	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	}
5d111219	138	debug(2, "get_client_id \"%s\" response %d", client_shared_memory_interface_name, response);
96dd6c2b MB	139	return response;
	140	}
	141
9c484afc	142	int delete_client(int client_id) {
96dd6c2b	143	int response = 0; // okay unless something happens
9c484afc MB	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;
96dd6c2b	155	}
96dd6c2b	156	}
9c484afc	157	clients[client_id].shm_interface_name[0] = '\0'; // remove the name to signify it's vacant
96dd6c2b MB	158	}
	159	return response;
	160	}
339e00f6	161
9c484afc MB	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
f7a4f63d	171	int find_clock_source_record(char sender_string, clock_source_private_data clocks_private_info) {
68cae740 MB	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)) {
96dd6c2b	177	if (((clocks_private_info[i].flags & (1 << clock_is_in_use)) != 0) &&
a5fbe4a3	178	(strcasecmp(sender_string, (const char *)&clocks_private_info[i].ip) == 0))
68cae740 MB	179	found = 1;
	180	else
	181	i++;
	182	}
	183	if (found == 1)
	184	response = i;
	185	return response;
	186	}
	187
a5fbe4a3 MB	188	int create_clock_source_record(char *sender_string,
a5fbe4a3 MB	189	clock_source_private_data *clocks_private_info) {
96dd6c2b	190	// return the index of a clock entry in the clock information arrays or -1 if full
68cae740 MB	191	// initialise the entries in the shared and private arrays
	192	int response = -1;
	193	int i = 0;
d99f2ba3	194	int found = 0; // trying to find an unused entry
68cae740	195	while ((found == 0) && (i < MAX_CLOCKS)) {
96dd6c2b	196	if ((clocks_private_info[i].flags & (1 << clock_is_in_use)) == 0)
68cae740 MB	197	found = 1;
	198	else
	199	i++;
	200	}
	201
	202	if (found == 1) {
65fea24b	203	int family = 0;
5e02deec	204
73164f4f	205	// check its ipv4/6 family -- derived from https://stackoverflow.com/a/3736377, with thanks.
65fea24b MB	206	struct addrinfo hint, *res = NULL;
	207	memset(&hint, '\0', sizeof hint);
	208	hint.ai_family = PF_UNSPEC;
	209	hint.ai_flags = AI_NUMERICHOST;
65fea24b MB	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;
96dd6c2b	218	clocks_private_info[i].flags \|= (1 << clock_is_in_use);
73164f4f MB	219	debug(2, "create record for ip: %s, family: %s.", &clocks_private_info[i].ip,
73164f4f MB	220	clocks_private_info[i].family == AF_INET6 ? "IPv6" : "IPv4");
65fea24b	221	} else {
d99f2ba3	222	debug(1, "cannot getaddrinfo for ip: %s.", &clocks_private_info[i].ip);
65fea24b	223	}
68cae740	224	} else {
d99f2ba3	225	debug(1, "Clock tables full!");
68cae740 MB	226	}
	227	return response;
	228	}
	229
fb9928f7 MB	230	void update_master_clock_info(uint64_t master_clock_id, const char *ip, uint64_t local_time,
fb9928f7 MB	231	uint64_t local_to_master_offset, uint64_t mastership_start_time) {
10969eaa MB	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;
1c610279	237	if (ip != NULL) {
10969eaa MB	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;
1c610279	241	} else {
10969eaa MB	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;
1c610279	245	}
10969eaa MB	246	__sync_synchronize();
	247	shared_memory->secondary = shared_memory->main;
	248	__sync_synchronize();
1c610279	249	}