integ/base/linuxptp/debian/patches/0042-Commands-enable-lock-and-disable-lock.patch

From 509fb3bfe38d8fafc6d4d14855d34bb985a15493 Mon Sep 17 00:00:00 2001
From: Andre Mauricio Zelak <andre.zelak@windriver.com>
Date: Fri, 4 Aug 2023 15:44:12 -0300
Subject: [PATCH 42/58] Commands 'enable lock' and 'disable lock.

The 'enable lock' command is used to lock to a single clock
source and disable the HA clock selection algorithm. The
interface of the clock source must be specified in the
command. For example:

'enable lock <interface-name>'

It returns "Success" or an error message. The error message
"Error: Usage 'enable lock <interface>'" is returned when
no interface was provided in the command. The error message
"Error: Interface not found!" is returned when the interface
provided is not found in the phc2sys configuration.

The command 'disable lock' is used to unlock the clock source
and re-enable the HA clock selection algorithm. It returns
"Success" even when the clock source was not locked.

Test plan: enable lock and disable lock commands
PASS: Verify the enable lock changes the clock source to the given
interface.
PASS: Verify that regardless the interface state, the clock source
remains locked.
PASS: Verify that disable lock command makes the better available
clock to be selected again.

[commit 704d9ed2e22b89308c7f0149d7fde86d456bc4e3 upstream]

Signed-off-by: Andre Mauricio Zelak <andre.zelak@windriver.com>
---
 phc2sys.c | 110 +++++++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 93 insertions(+), 17 deletions(-)

diff --git a/phc2sys.c b/phc2sys.c
index 0bc3709..f89dc23 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -259,13 +259,21 @@ static void clock_cleanup(struct phc2sys_private *priv)
 	}
 }
 
-static struct clock *clock_get(struct phc2sys_private *priv, struct pmc_agent *node)
+static struct clock * clock_get_by_device(struct phc2sys_private *priv,
+	const char * device)
 {
 	struct clock * clock = NULL;
 	LIST_FOREACH(clock, &priv->clocks, list) {
-		if (clock->node == node) {
+		/* ignore the dst clock */
+		if (clock->state == PS_MASTER)
+			continue;
+
+		/* sanity check */
+		if (!clock->device)
+			continue;
+
+		if (strcmp(device, clock->device) == 0)
 			break;
-		}
 	}
 	return clock;
 }
@@ -508,18 +516,6 @@ static struct port *port_get(struct phc2sys_private *priv, unsigned int number)
 	return NULL;
 }
 
-static struct port *port_get_by_clock(struct phc2sys_private *priv, struct clock * clock)
-{
-	struct port *p, *port = NULL;
-	LIST_FOREACH(p, &priv->ports, list) {
-		if (p->clock == clock) {
-			port = p;
-			break;
-		}
-	}
-	return port;
-}
-
 static struct port *port_add(struct phc2sys_private *priv, unsigned int number,
 			     char *device)
 {
@@ -1287,7 +1283,82 @@ static int ha_handle_status_msg(struct phc2sys_private *priv, char *response,
 	return curlen;
 }
 
-static int ha_com_socket_handle_msg(struct phc2sys_private *priv)
+static bool startsWith(const char *prefix, const char *str)
+{
+	return 0 == strncmp(prefix, str, strlen(prefix) - 1);
+}
+
+static char * strAtColumn(char *msg, size_t column)
+{
+	int i;
+	char * str = NULL;
+
+	/* split and walk over the columns */
+	strtok(msg, " ");
+	for (i = 1; i < column; i++) {
+		str = strtok(NULL, " ");
+	}
+
+	return str;
+}
+
+static int ha_handle_enable_lock_msg(struct phc2sys_private *priv, char *msg,
+			char *response, size_t resplen)
+{
+	size_t curlen = 0;
+	char *interface = NULL;
+	struct clock *clock = NULL;
+
+	interface = strAtColumn(msg, 3);
+	if (strlen(interface) == 0) {
+		return snprintf(response, resplen, "Error: Usage 'enable lock <interface>'");
+	}
+
+	clock = clock_get_by_device(priv, interface);
+	if (!clock) {
+		return snprintf(response, resplen, "Error: Interface not found!");
+	}
+
+	pr_info("HA automatic source selection is disabled by command");
+	pr_info("Only interface %s will be used as source clock", clock->device);
+
+	priv->master = clock;
+	priv->better = NULL;
+	priv->stability_timer.tv_sec = 0;
+	priv->stability_timer.tv_nsec = 0;
+
+	priv->forced_source_clock = 1;
+
+	curlen = snprintf(response, resplen, "Success");
+
+	return curlen;
+}
+
+static int ha_handle_disable_lock_msg(struct phc2sys_private *priv,
+			struct config *cfg, char *response, size_t resplen)
+{
+	size_t curlen = 0;
+	struct clock *clock = NULL;
+
+	if (priv->forced_source_clock) {
+		pr_info("HA automatic source selection is enabled by command");
+		/* re-enable HA source selection algorithm */
+		priv->forced_source_clock = 0;
+		/* select the best clock available */
+		clock = ha_select_clock(priv, cfg);
+		if (clock && clock != priv->master) {
+			priv->master = clock;
+			pr_notice("new source clock selected %s", clock->device);
+		}
+	}
+
+	curlen = snprintf(response, resplen, "Success");
+
+	return curlen;
+}
+
+static int ha_com_socket_handle_msg(struct phc2sys_private *priv,
+			struct config *cfg)
 {
 	struct pollfd pollfd[HA_SCK_N_FD];
 	struct address sender;
@@ -1348,6 +1419,11 @@ static int ha_com_socket_handle_msg(struct phc2sys_private *priv)
 		} 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 if (startsWith("enable lock", buffer)) {
+			cnt = ha_handle_enable_lock_msg(priv, buffer, response,
+					HA_SCK_BUFFER_SIZE);
+		} else if (strcmp((const char*)buffer, "disable lock") == 0) {
+			cnt = ha_handle_disable_lock_msg(priv, cfg, response, HA_SCK_BUFFER_SIZE);
 		} else {
 			cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE,  "error: invalid command");
 		}
@@ -1410,7 +1486,7 @@ static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscri
 		}
 
 		if (ha_enabled) {
-			ha_com_socket_handle_msg(priv);
+			ha_com_socket_handle_msg(priv, cfg);
 
 			if (priv->forced_source_clock) {
 				/* HA automatic clock selection is disabled */
-- 
2.30.2