integ/base/linuxptp/debian/patches/0043-Commands-enable-source-and-disable-source.patch

From 27b5c6afff470053b30ade14537be43f1c1c376d Mon Sep 17 00:00:00 2001
From: Andre Mauricio Zelak <andre.zelak@windriver.com>
Date: Fri, 4 Aug 2023 19:01:57 -0300
Subject: [PATCH 43/54] Commands 'enable source' and 'disable source'.

These commands controls the list of clocks available to clock
selection algorithm.

At startup all sources are enabled and can be selected as clock
source. The 'disable source' command removes a given interface
from the available list and it can't be selected any more. The
'enable source' command re-enables the interface.

The last interface can't be disable. The disable command fails and
returns an error indicating the given interface is the last one.

If the active clock source interface is disabled than a new one
will be selected.

Every time the enable command is executed the clock selection
algorithm is executed and the best available clock is selected.

The enable and disable source commands won't affect the active
clock if one interface is forced lock as active.

The disabled interface is market with 'x' sign in the status
command.

Test plan: enable source and disable source commands
PASS: Verify a new interface is selected when the active one
is disabled.
PASS: Verify the primary interface is re-selected active after
it is enabled back.
PASS: Verify the disable source command fails when attempt to
disable the last enabled interface.
PASS: Verify the active interface don't change while one of them
are forced lock as active.
PASS: Verify the active interface dont't change after enabling
an interface while in forced lock mode.

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

[commit 55ac3f4131aaa999b1b7b9eec50b7cb7cebbf0d4 upstream]
[commit c77de0acd3641833d2705e3929be2152bd5fb519 upstream]

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

diff --git a/phc2sys.c b/phc2sys.c
index f89dc23..035ee21 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -98,6 +98,7 @@ struct clock {
 	struct clockcheck *sanity_check;
 	struct pmc_agent *node;
 	int ha_priority;
+	int enabled;
 };
 typedef LIST_HEAD(head, clock) clock_list_head_t;
 
@@ -228,6 +229,8 @@ static struct clock *clock_add(struct phc2sys_private *priv, char *device)
 		c->sysoff_method = sysoff_probe(CLOCKID_TO_FD(clkid),
 						priv->phc_readings);
 
+	c->enabled = 1;
+
 	LIST_INSERT_HEAD(&priv->clocks, c, list);
 	return c;
 }
@@ -278,6 +281,28 @@ static struct clock * clock_get_by_device(struct phc2sys_private *priv,
 	return clock;
 }
 
+static size_t clock_count_enabled_sources(struct phc2sys_private *priv,
+	struct clock *ignore)
+{
+	size_t count = 0;
+	struct clock * clock = NULL;
+
+	LIST_FOREACH(clock, &priv->clocks, list) {
+		/* ignore the dst clock */
+		if (clock->state == PS_MASTER)
+			continue;
+
+		if (clock == ignore)
+			continue;
+
+		if (!clock->enabled)
+			continue;
+
+		count++;
+	}
+	return count;
+}
+
 static bool clock_match_ha_dds_requirements(struct clock *clock, struct config *cfg)
 {
 	/* get requirements */
@@ -404,6 +429,11 @@ static int clock_available_ha_src_clocks(struct phc2sys_private *priv, struct co
 			continue;
 		}
 
+		if (!clock->enabled) {
+			pr_debug("clock %s is disabled", clock->device);
+			continue;
+		}
+
 		/* get Default Data Set */
 		if (!clock->node->dds_valid) {
 			retries = 0;
@@ -1267,7 +1297,8 @@ static int ha_handle_status_msg(struct phc2sys_private *priv, char *response,
 		curlen += snprintf(response + curlen, resplen - curlen,
 			" %c    %9s   %8d   %10d       0x%2x   0x%4x     %s    %s  %d\n",
 			(priv->master ==  clock) ? '*' :
-				(priv->better == clock) ? '-' : ' ',
+				(priv->better == clock) ? '-' :
+					(!clock->enabled) ? 'x' : ' ',
 			clock->device, clock->ha_priority,
 			clock->node->dds.clockQuality.clockClass,
 			clock->node->dds.clockQuality.clockAccuracy,
@@ -1302,6 +1333,16 @@ static char * strAtColumn(char *msg, size_t column)
 	return str;
 }
 
+static void ha_set_clock_source(struct phc2sys_private *priv, struct clock *clock)
+{
+	pr_notice("new clock source selected %s", clock->device);
+
+	priv->master = clock;
+	priv->better = NULL;
+	priv->stability_timer.tv_sec = 0;
+	priv->stability_timer.tv_nsec = 0;
+}
+
 static int ha_handle_enable_lock_msg(struct phc2sys_private *priv, char *msg,
 			char *response, size_t resplen)
 {
@@ -1316,16 +1357,13 @@ static int ha_handle_enable_lock_msg(struct phc2sys_private *priv, char *msg,
 
 	clock = clock_get_by_device(priv, interface);
 	if (!clock) {
-		return snprintf(response, resplen, "Error: Interface not found!");
+		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;
+	ha_set_clock_source(priv, clock);
 
 	priv->forced_source_clock = 1;
 
@@ -1347,8 +1385,77 @@ static int ha_handle_disable_lock_msg(struct phc2sys_private *priv,
 		/* 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);
+			ha_set_clock_source(priv, clock);
+		}
+	}
+
+	curlen = snprintf(response, resplen, "Success");
+
+	return curlen;
+}
+
+static int ha_handle_enable_source_msg(struct phc2sys_private *priv,
+			struct config *cfg, char *msg, char *response, size_t resplen)
+{
+	size_t curlen;
+	char *interface = NULL;
+	struct clock *clock = NULL;
+
+	interface = strAtColumn(msg, 3);
+	if (strlen(interface) == 0) {
+		return snprintf(response, resplen, "Error: Usage 'enable source <interface>'");
+	}
+
+	clock = clock_get_by_device(priv, interface);
+	if (!clock) {
+		return snprintf(response, resplen, "Error: Interface not found");
+	}
+
+	clock->enabled = 1;
+
+	if (!priv->forced_source_clock) {
+		/* select the best clock available */
+		clock = ha_select_clock(priv, cfg);
+		if (clock && clock != priv->master) {
+			ha_set_clock_source(priv, clock);
+		}
+	}
+
+	curlen = snprintf(response, resplen, "Success");
+
+	return curlen;
+}
+
+static int ha_handle_disable_source_msg(struct phc2sys_private *priv,
+			struct config *cfg, char *msg, char *response, size_t resplen)
+{
+	size_t curlen;
+	char *interface = NULL;
+	struct clock *clock = NULL;
+
+	interface = strAtColumn(msg, 3);
+	if (strlen(interface) == 0) {
+		return snprintf(response, resplen, "Error: Usage 'disable source <interface>'");
+	}
+
+	clock = clock_get_by_device(priv, interface);
+	if (!clock) {
+		return snprintf(response, resplen, "Error: Interface not found");
+	}
+
+	/* check if is the last clock enabled */
+	if (clock_count_enabled_sources(priv, clock) == 0) {
+		return snprintf(response, resplen, "Error: Last interface enabled");
+	}
+
+	clock->enabled = 0;
+
+	/* disabling clock source */
+	if (clock == priv->master && !priv->forced_source_clock) {
+		/* select the best clock available */
+		clock = ha_select_clock(priv, cfg);
+		if (clock && clock != priv->master) {
+			ha_set_clock_source(priv, clock);
 		}
 	}
 
@@ -1423,9 +1530,17 @@ static int ha_com_socket_handle_msg(struct phc2sys_private *priv,
 			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);
+			cnt = ha_handle_disable_lock_msg(priv, cfg, response,
+					HA_SCK_BUFFER_SIZE);
+		} else if (startsWith("enable source", buffer)) {
+			cnt = ha_handle_enable_source_msg(priv, cfg, buffer, response,
+					HA_SCK_BUFFER_SIZE);
+		} else if (startsWith("disable source", buffer)) {
+			cnt = ha_handle_disable_source_msg(priv, cfg, buffer, response,
+					HA_SCK_BUFFER_SIZE);
 		} else {
-			cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE,  "error: invalid command");
+			cnt = snprintf((char*)response, HA_SCK_BUFFER_SIZE,
+					"Error: Invalid command");
 		}
 
 		fprintf(stderr, "ha_com_socket: response: \n%s", (char*)response);
@@ -1498,10 +1613,7 @@ static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscri
 				if (priv->clock_state_changed) {
 					clock = check_and_select_clock(priv, cfg);
 					if (clock && clock != priv->master) {
-						priv->master = clock;
-						priv->better = NULL;
-						priv->stability_timer.tv_sec = 0;
-						priv->stability_timer.tv_nsec = 0;
+						ha_set_clock_source(priv, clock);
 					}
 
 					priv->clock_state_changed = 0;
@@ -1514,11 +1626,7 @@ static int do_loop(struct phc2sys_private *priv, struct config *cfg, int subscri
 					if ((now.tv_sec > priv->stability_timer.tv_sec) ||
 						(now.tv_sec == priv->stability_timer.tv_sec &&
 						now.tv_nsec > priv->stability_timer.tv_nsec)) {
-						pr_notice("new source clock selected %s", priv->better->device);
-						priv->master = priv->better;
-						priv->better = NULL;
-						priv->stability_timer.tv_sec = 0;
-						priv->stability_timer.tv_nsec = 0;
+						ha_set_clock_source(priv, priv->better);
 					}
 				}
 			}
-- 
2.25.1