integ/base/linuxptp/debian/patches/0033-Implement-push-notification-for-TIME_STATUS_NP.patch

From: Andre Mauricio Zelak <andre.zelak@windriver.com>
Date: Mon, 12 Jun 2023 17:57:11 -0300
Subject: [PATCH 33/59] Implement push notification for TIME_STATUS_NP

Subscribers to NOTIFY_TIME_SYNC will be notified on every clock
synchronization.

[ RC:
   - Don't subscribe this in pmc_agent.
   - Use stdbool/stdint types in event_bitmask_get/set. ]

Signed-off-by: Juergen Werner <pogojotz@gmx.net>
Signed-off-by: Richard Cochran <richardcochran@gmail.com>

[commit 6d7c090706e76af334185ffcec9cc56d0570e215 upstream]
Signed-off-by: Andre Mauricio Zelak <andre.zelak@windriver.com>
---
 clock.c        | 14 +++++++++-----
 notification.h | 22 ++++++++++++++++++++++
 pmc.c          |  6 ++++--
 pmc_agent.c    |  2 +-
 pmc_common.c   | 23 +++++++++++++++--------
 5 files changed, 51 insertions(+), 16 deletions(-)

diff --git a/clock.c b/clock.c
index 437cd1c..f048771 100644
--- a/clock.c
+++ b/clock.c
@@ -243,13 +243,11 @@ static void clock_prune_subscriptions(struct clock *c)
 void clock_send_notification(struct clock *c, struct ptp_message *msg,
 			     enum notification event)
 {
-	unsigned int event_pos = event / 8;
-	uint8_t mask = 1 << (event % 8);
 	struct port *uds = c->uds_port;
 	struct clock_subscriber *s;
 
 	LIST_FOREACH(s, &c->subscribers, list) {
-		if (!(s->events[event_pos] & mask))
+		if (!event_bitmask_get(s->events, event))
 			continue;
 		/* send event */
 		msg->header.sequenceId = htons(s->sequenceId);
@@ -1501,7 +1499,9 @@ void clock_notify_event(struct clock *c, enum notification event)
 	int id;
 
 	switch (event) {
-	/* set id */
+	case NOTIFY_TIME_SYNC:
+		id = TLV_TIME_STATUS_NP;
+		break;
 	default:
 		return;
 	}
@@ -1731,7 +1731,9 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 	c->cur.offsetFromMaster = tmv_to_TimeInterval(c->master_offset);
 
 	if (c->free_running) {
-		return clock_no_adjust(c, ingress, origin);
+		state = clock_no_adjust(c, ingress, origin);
+		clock_notify_event(c, NOTIFY_TIME_SYNC);
+		return state;
 	}
 
 	offset = tmv_to_nanoseconds(c->master_offset);
@@ -1777,6 +1779,8 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 			tmv_to_nanoseconds(c->path_delay));
 	}
 
+	clock_notify_event(c, NOTIFY_TIME_SYNC);
+
 	return state;
 }
 
diff --git a/notification.h b/notification.h
index 47c9b56..115f864 100644
--- a/notification.h
+++ b/notification.h
@@ -20,8 +20,30 @@
 #ifndef HAVE_NOTIFICATION_H
 #define HAVE_NOTIFICATION_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
+static inline void event_bitmask_set(uint8_t *bitmask, unsigned int event,
+				     bool value)
+{
+	unsigned int event_pos = event / 8;
+	uint8_t event_bit = 1 << (event % 8);
+
+	if (value) {
+		bitmask[event_pos] |= event_bit;
+	} else {
+		bitmask[event_pos] &= ~(event_bit);
+	}
+}
+
+static inline bool event_bitmask_get(uint8_t *bitmask, unsigned int event)
+{
+	return (bitmask[event / 8] & (1 << (event % 8))) ? true : false;
+}
+
 enum notification {
 	NOTIFY_PORT_STATE,
+	NOTIFY_TIME_SYNC,
 };
 
 #endif
diff --git a/pmc.c b/pmc.c
index 65d1d61..3678800 100644
--- a/pmc.c
+++ b/pmc.c
@@ -387,9 +387,11 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 		sen = (struct subscribe_events_np *) mgt->data;
 		fprintf(fp, "SUBSCRIBE_EVENTS_NP "
 			IFMT "duration          %hu"
-			IFMT "NOTIFY_PORT_STATE %s",
+			IFMT "NOTIFY_PORT_STATE %s"
+			IFMT "NOTIFY_TIME_SYNC  %s",
 			sen->duration,
-			(sen->bitmask[0] & 1 << NOTIFY_PORT_STATE) ? "on" : "off");
+			event_bitmask_get(sen->bitmask, NOTIFY_PORT_STATE) ? "on" : "off",
+			event_bitmask_get(sen->bitmask, NOTIFY_TIME_SYNC) ? "on" : "off");
 		break;
 	case TLV_SYNCHRONIZATION_UNCERTAIN_NP:
 		mtd = (struct management_tlv_datum *) mgt->data;
diff --git a/pmc_agent.c b/pmc_agent.c
index 623f300..37910b3 100644
--- a/pmc_agent.c
+++ b/pmc_agent.c
@@ -57,7 +57,7 @@ static void send_subscription(struct pmc_agent *node)
 
 	memset(&sen, 0, sizeof(sen));
 	sen.duration = PMC_SUBSCRIBE_DURATION;
-	sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
+	event_bitmask_set(sen.bitmask, NOTIFY_PORT_STATE, TRUE);
 	pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen));
 }
 
diff --git a/pmc_common.c b/pmc_common.c
index a117904..c5cd992 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -149,7 +149,8 @@ static void do_set_action(struct pmc *pmc, int action, int index, char *str)
 	struct management_tlv_datum mtd;
 	struct subscribe_events_np sen;
 	struct port_ds_np pnp;
-	char onoff[4] = {0};
+	char onoff_port_state[4] = "off";
+	char onoff_time_status[4] = "off";
 
 	switch (action) {
 	case GET:
@@ -223,16 +224,22 @@ static void do_set_action(struct pmc *pmc, int action, int index, char *str)
 	case TLV_SUBSCRIBE_EVENTS_NP:
 		memset(&sen, 0, sizeof(sen));
 		cnt = sscanf(str, " %*s %*s "
-			     "duration %hu "
-			     "NOTIFY_PORT_STATE %3s ",
-			     &sen.duration, onoff);
-		if (cnt != 2) {
-			fprintf(stderr, "%s SET needs 2 values\n",
+			     "duration          %hu "
+			     "NOTIFY_PORT_STATE %3s "
+			     "NOTIFY_TIME_SYNC  %3s ",
+			     &sen.duration,
+			     onoff_port_state,
+			     onoff_time_status);
+		if (cnt != 3) {
+			fprintf(stderr, "%s SET needs 3 values\n",
 				idtab[index].name);
 			break;
 		}
-		if (!strcasecmp(onoff, "on")) {
-			sen.bitmask[0] = 1 << NOTIFY_PORT_STATE;
+		if (!strcasecmp(onoff_port_state, "on")) {
+			event_bitmask_set(sen.bitmask, NOTIFY_PORT_STATE, TRUE);
+		}
+		if (!strcasecmp(onoff_time_status, "on")) {
+			event_bitmask_set(sen.bitmask, NOTIFY_TIME_SYNC, TRUE);
 		}
 		pmc_send_set_action(pmc, code, &sen, sizeof(sen));
 		break;