integ/base/linuxptp/debian/patches/0041-HA-phc2sys-com-socket.patch

From: Andre Mauricio Zelak <andre.zelak@windriver.com>
Date: Thu, 27 Jul 2023 14:22:47 -0300
Subject: [PATCH 41/61] HA phc2sys com socket

A new communication path was created to retrieve status and to control
the high availability algorithm.

The ha_phc2sys_com_socket option is a global setting to configure
the socket path. Its default value is /var/run/phc2sys.

The command 'status' was created to retrieve the current HA clock status.
The answer is a table of configured clocks and its status.

act   interface   priority   clockClass   clockAcc   offset   time   freq   gm.

 *       ens2f1        200          248       0xfe   0xffff     no     no   6
         ens1f2        100          248       0xfe   0xffff     no     no   6

Source forced? no

The * sign marks the active source clock.
The - sign marks the active candidate source clock, which will be set active
after the stability timer expiration.
The x sign marks the disabled interfaces (see 'disable source' command).

The 'Source forced?' field shows if the active source is forced lock or not.

The 'clock source' command can be used to retrive the active
clock source. It returns the interface name of the active
clock source or "None" when there is no one select.

The 'forced lock' command can be used to retrieve if the active
clock source is forced lock, and the clock source selection
algorithm is disabled. It returns "True" when is forced lock
and "False" otherwise.

Test plan: socket path configuration
PASS Verify the socket using the default path.
PASS Verify the socket using a given socket path.

Test plan: status command
PASS: Verify the 'status' command after start up.
PASS: Verify the 'status' command while stability timer is running.
PASS: Verify the 'status' command with a forced lock interface by
configuring ha_priority 254.

Test plan: clock source command
PASS: Verify the 'clock source' command response is the highest priority
interface after start up.
PASS: Verify the 'clock source' command response is the active interface
after the primary has degraded.
PASS: Verify the 'clock source' command response is the forced lock
interface, when ha_priority 254 is configured in one of them.

Test plan: forced lock command
PASS: Verify the 'forced lock' command response is 'False' when no
interface is configured with ha_priority 254.
PASS: Verify the 'forced lock' command response is 'True' when one
interface is configured with ha_priority 254.

Reviewed-by: Cole Walker <cole.walker@windriver.com>
Reviewed-by: Andre Fernando Zanella Kantek <andrefernandozanella.kantek@windriver.com>

[commit 0cfcbb78485a83d324963130f9558fd0a1962a79 upstream]
[commit 73b9afa33a0d8dcfd9c4ebb7bceacee40af8eb2b upstream]
[commit 6e93059d34639a3c2aac6b56dcf94ddf1e48e9b4 upstream]
[commit 4f118cf954bc3543582765bc039c42aeac05caf5 upstream]
[commit 6387ddf644afcb880b67368be8416b8ce906e029 upstream]

Signed-off-by: Andre Mauricio Zelak <andre.zelak@windriver.com>
---
 config.c  |   1 +
 phc2sys.c | 231 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 216 insertions(+), 16 deletions(-)

diff --git a/config.c b/config.c
index dba1eef..6a1bfb4 100644
--- a/config.c
+++ b/config.c
@@ -256,6 +256,7 @@ struct config_item config_tab[] = {
 	GLOB_ITEM_INT("ha_min_gm_ClockClass", 135, 6, 255),
 	GLOB_ITEM_INT("ha_min_local_clockClass", 135, 6, 255),
 	GLOB_ITEM_INT("ha_min_offsetScaledLogVariance", 65535, 0, 65535),
+	GLOB_ITEM_STR("ha_phc2sys_com_socket", "/var/run/phc2sys-phc-inst1"),
 	PORT_ITEM_INT("ha_priority", 0, 0, 254),
 	PORT_ITEM_INT("ha_stability_timer", 0, 0, INT_MAX),
 	GLOB_ITEM_INT("ha_timeTraceable", 0, 0, 1),
diff --git a/phc2sys.c b/phc2sys.c
index 0b3f724..0bc3709 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -66,6 +66,9 @@
 
 #define FORCED_SOURCE_CLOCK_PRIORITY 254
 #define MAX_SRC_CLOCKS 128
+#define HA_SCK_N_FD 1
+#define HA_SCK_BUFFER_SIZE 1024
+#define HA_SCK_FILEMODE (S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP) /*0660*/
 
 #define PORT_INDEX_TO_PORT_ID(port, index) (((((unsigned int) port) & 0xFF) << 8) | (((unsigned int) index) & 0xFF))
 #define PORT_ID_TO_PORT(id) ((((unsigned int) id) >> 8) & 0xFF)
@@ -94,6 +97,7 @@ struct clock {
 	struct stats *delay_stats;
 	struct clockcheck *sanity_check;
 	struct pmc_agent *node;
+	int ha_priority;
 };
 typedef LIST_HEAD(head, clock) clock_list_head_t;
 
@@ -123,6 +127,7 @@ struct phc2sys_private {
 	struct timespec stability_timer;
 	int default_sync;
 	int forced_source_clock;
+	int ha_socket_fd;
 };
 
 static struct config *phc2sys_config;
@@ -1003,7 +1008,6 @@ static int update_needed(struct clock *c)
 /* check configuration if one of the source clocks is force locked to be active */
 static struct clock* ha_forced_source_clock(struct phc2sys_private *priv, struct config *cfg)
 {
-	int clock_priority;
 	struct clock *clock = NULL, *best = NULL;
 
 	LIST_FOREACH(clock, &priv->clocks, list) {
@@ -1012,8 +1016,7 @@ static struct clock* ha_forced_source_clock(struct phc2sys_private *priv, struct
 			continue;
 		}
 
-		clock_priority = config_get_int(cfg, clock->device, "ha_priority");
-		if (FORCED_SOURCE_CLOCK_PRIORITY == clock_priority) {
+		if (FORCED_SOURCE_CLOCK_PRIORITY == clock->ha_priority) {
 			pr_info("HA automatic source selection is disabled by configuration");
 			priv->forced_source_clock = 1;
 			best = clock;
@@ -1025,7 +1028,7 @@ static struct clock* ha_forced_source_clock(struct phc2sys_private *priv, struct
 
 static struct clock* ha_select_clock(struct phc2sys_private *priv, struct config *cfg)
 {
-	int clock_priority, highest_priority;
+	int highest_priority;
 	int clock_class, lowest_clock_class;
 	struct clock *clock = NULL, *best = NULL;
 	clock_list_head_t ha_available_clocks;
@@ -1038,17 +1041,14 @@ static struct clock* ha_select_clock(struct phc2sys_private *priv, struct config
 
 	/* one or more sources match requirements, select highest priority */
 	highest_priority = 0;
-	LIST_FOREACH(clock, &ha_available_clocks, ha_list) {
-		clock_priority = config_get_int(cfg, clock->device, "ha_priority");
-
-		/* select highest priority clock
+	LIST_FOREACH(clock, &ha_available_clocks, ha_list) {/* select highest priority clock
 		   more than one clock with same priority, select first
 		   don't select clocks with ha_priority 0 */
-		if (clock_priority > highest_priority) {
+		if (clock->ha_priority > highest_priority) {
 			pr_notice("new highest ha priority clock %s ha_priority %d",
-				clock->device, clock_priority);
+				clock->device, clock->ha_priority);
 			best = clock;
-			highest_priority = clock_priority;
+			highest_priority = clock->ha_priority;
 		}
 	}
 
@@ -1101,7 +1101,6 @@ static struct clock* check_and_select_clock(struct phc2sys_private *priv, struct
 	struct clock *active = priv->master, *candidate = NULL;
 	int stability_timer = 0;
 	struct timespec now;
-	int active_priority, candidate_priority;
 	int active_clock_class, candidate_clock_class;
 
 	/* Active source degrades - re-run ha_select_clock algorithm */
@@ -1137,11 +1136,9 @@ static struct clock* check_and_select_clock(struct phc2sys_private *priv, struct
 		/* new clock candidate */
 
 		/* candidate has equal priority and clockClass than active - don't change active */
-		active_priority = config_get_int(cfg, active->device, "ha_priority");
-		candidate_priority = config_get_int(cfg, candidate->device, "ha_priority");
 		active_clock_class = active->node->dds.clockQuality.clockClass;
 		candidate_clock_class = candidate->node->dds.clockQuality.clockClass;
-		if ((active_priority == candidate_priority) &&
+		if ((active->ha_priority == candidate->ha_priority) &&
 			(active_clock_class == candidate_clock_class)) {
 			return NULL;
 		}
@@ -1175,6 +1172,196 @@ static void reset_new_dataset_flags(struct phc2sys_private *priv)
 	}
 }
 
+static int ha_com_socket_close(int fd)
+{
+	struct sockaddr_un sa;
+	socklen_t len = sizeof(sa);
+
+	// if (fd < 0)
+	// 	return -1;
+
+	if (!getsockname(fd, (struct sockaddr *) &sa, &len) &&
+		sa.sun_family == AF_LOCAL) {
+		unlink(sa.sun_path);
+	}
+
+	close(fd);
+	return 0;
+}
+
+static int ha_com_socket_open(int *fd_out, struct config *cfg)
+{
+	int fd, err;
+	struct sockaddr_un sa;
+	const char *name = config_get_string(cfg, NULL, "ha_phc2sys_com_socket");
+
+	fd = socket(AF_LOCAL, SOCK_DGRAM, 0);
+	if (fd < 0) {
+		pr_err("ha_com_socket: failed to create socket: %m");
+		return -1;
+	}
+
+	memset(&sa, 0, sizeof(sa));
+	sa.sun_family = AF_LOCAL;
+	strncpy(sa.sun_path, name, sizeof(sa.sun_path) - 1);
+
+	err = bind(fd, (struct sockaddr *) &sa, sizeof(sa));
+	if (err < 0) {
+		pr_err("ha_com_socket: bind failed: %m");
+		close(fd);
+		return -1;
+	}
+
+	*fd_out = fd;
+	chmod(name, HA_SCK_FILEMODE);
+
+	return 0;
+}
+
+static int ha_com_socket_recv(int fd, void *buf, size_t buflen,
+			struct address *addr)
+{
+	int cnt;
+
+	addr->len = sizeof(addr->sun);
+	cnt = recvfrom(fd, buf, buflen, 0, &addr->sa, &addr->len);
+	if (cnt <= 0) {
+		pr_err("ha_com_socket: recvfrom failed: %m");
+		return cnt;
+	}
+
+	((char*)buf)[cnt] = '\0';
+
+	return 0;
+}
+
+static int ha_com_socket_send(int fd, struct address *addr, void *buf,
+			size_t buflen)
+{
+	int cnt;
+
+	cnt = sendto(fd, buf, buflen, 0, &addr->sa, addr->len);
+	if (cnt < 1) {
+		return -errno;
+	}
+	return cnt;
+}
+
+static int ha_handle_status_msg(struct phc2sys_private *priv, char *response,
+			size_t resplen)
+{
+	struct clock *clock;
+	size_t curlen = 0;
+
+	/* header */
+	curlen = snprintf(response, resplen,
+		"act   interface   priority   clockClass   clockAcc   offset   time   freq   "
+		"gm.clockClass\n\n");
+
+	LIST_FOREACH(clock, &priv->clocks, list) {
+
+		/* ignore the dst clock */
+		if (clock->state == PS_MASTER)
+			continue;
+
+		/* sanity check */
+		if (clock->node == NULL)
+			continue;
+
+		curlen += snprintf(response + curlen, resplen - curlen,
+			" %c    %9s   %8d   %10d       0x%2x   0x%4x     %s    %s  %d\n",
+			(priv->master ==  clock) ? '*' :
+				(priv->better == clock) ? '-' : ' ',
+			clock->device, clock->ha_priority,
+			clock->node->dds.clockQuality.clockClass,
+			clock->node->dds.clockQuality.clockAccuracy,
+			clock->node->dds.clockQuality.offsetScaledLogVariance,
+			clock->node->utc_offset_traceable ? "yes" : "no ",
+			clock->node->freq_traceable ? "yes" : "no ",
+			clock->node->pds.grandmasterClockQuality.clockClass);
+	}
+
+	curlen += snprintf(response + curlen, resplen - curlen,
+		"\n\nSource forced? %s\n", priv->forced_source_clock ? "yes" : "no");
+
+	return curlen;
+}
+
+static int ha_com_socket_handle_msg(struct phc2sys_private *priv)
+{
+	struct pollfd pollfd[HA_SCK_N_FD];
+	struct address sender;
+	int cnt, res = 0;
+	int timeout = 0;
+	void * buffer = NULL;
+	void * response = NULL;
+
+	while(1) {
+		pollfd[0].fd = priv->ha_socket_fd;
+		pollfd[0].events = POLLIN|POLLPRI;
+
+		cnt = poll(pollfd, HA_SCK_N_FD, timeout);
+		if (cnt < 0) {
+			pr_err("ha_com_socket: poll failed: %m");
+			res = -1;
+			break;
+		}
+		if (!cnt) {
+			/* timeout and fd wasn't ready */
+			break;
+		}
+
+		if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
+			break;
+
+		buffer = malloc(HA_SCK_BUFFER_SIZE);
+		if (!buffer) {
+			pr_err("ha_com_socket: failed to allocate memory for message");
+			res = -1;
+			break;
+		}
+
+		res = ha_com_socket_recv(pollfd[0].fd, buffer, HA_SCK_BUFFER_SIZE, &sender);
+		if (res < 0)
+			break;
+
+		fprintf(stderr, "ha_com_socket: received: %s\n", (char*)buffer);
+		fprintf(stderr, "ha_com_socket: recvd from: %s\n", ((struct sockaddr_un*)&sender.sa)->sun_path);
+
+		response = malloc(HA_SCK_BUFFER_SIZE);
+		if (!response) {
+			pr_err("ha_com_socket: failed to allocate memory for response message");
+			res = -1;
+			break;
+		}
+
+		/* handle messages and create responses */
+		if (strcmp((const char*)buffer, "status") == 0) {
+			cnt = ha_handle_status_msg(priv, response, HA_SCK_BUFFER_SIZE);
+		} else if (strcmp((const char*)buffer, "clock source") == 0) {
+			if (priv->master) {
+				cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE, "%s",
+					priv->master->device);
+			} else {
+				cnt = snprintf((char*)buffer, HA_SCK_BUFFER_SIZE, "None");
+			}
+		} else if (strcmp((const char*)buffer, "forced lock") == 0) {
+			cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE, "%s",
+				priv->forced_source_clock ? "True" : "False");
+		} else {
+			cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE,  "error: invalid command");
+		}
+
+		fprintf(stderr, "ha_com_socket: response: \n%s", (char*)response);
+
+		res = ha_com_socket_send(pollfd[0].fd, &sender, response, cnt);
+	}
+
+	free(buffer);
+	free(response);
+	return res;
+}
+
 static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscriptions)
 {
 	struct timespec interval;
@@ -1223,6 +1410,8 @@ static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscri
 		}
 
 		if (ha_enabled) {
+			ha_com_socket_handle_msg(priv);
+
 			if (priv->forced_source_clock) {
 				/* HA automatic clock selection is disabled */
 				if (priv->clock_state_changed) {
@@ -1312,6 +1501,7 @@ static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscri
 			update_clock(priv, clock, offset, ts, delay);
 		}
 	}
+
 	return 0;
 }
 
@@ -1597,6 +1787,7 @@ int main(int argc, char *argv[])
 		.better = NULL,
 		.stability_timer.tv_sec = 0,
 		.forced_source_clock = 0,
+		.ha_socket_fd = -1,
 	};
 	struct pmc_agent *node = NULL;
 	unsigned int i, src_cnt = 0;
@@ -1861,7 +2052,7 @@ int main(int argc, char *argv[])
 	ha_enabled = config_get_int(cfg, NULL, "ha_enabled");
 	if (!ha_enabled && src_cnt > 1) {
 		fprintf(stderr, "too many source clocks\n");
-		fprintf(stderr, "Use 'ha_enabled 1' to accept more than one source clocks\n");
+		fprintf(stderr, "Use 'ha_enabled 1' to accept more than one source clock\n");
 		goto bad_usage;
 	}
 
@@ -1877,6 +2068,9 @@ int main(int argc, char *argv[])
 		if (priv.master == NULL) {
 			priv.master = src;
 		}
+		if (ha_enabled) {
+			src->ha_priority = config_get_int(cfg, src->device, "ha_priority");
+		}
 	}
 
 	dst = clock_add(&priv, dst_name ? dst_name : "CLOCK_REALTIME");
@@ -1966,6 +2160,10 @@ int main(int argc, char *argv[])
 		}
 	}
 
+	if (ha_enabled) {
+		ha_com_socket_open(&priv.ha_socket_fd, cfg);
+	}
+
 	if (pps_fd >= 0) {
 		/* only one destination clock allowed with PPS until we
 		 * implement a mean to specify PTP port to PPS mapping */
@@ -1976,6 +2174,7 @@ int main(int argc, char *argv[])
 	}
 
 end:
+	ha_com_socket_close(priv.ha_socket_fd);
 	pmc_agent_cleanup(&priv);
 	clock_cleanup(&priv);
 	port_cleanup(&priv);