[Linuxptp-devel] [PATCH v4 07/11] Add the ALTERNATE_TIME_OFFSET_PROPERTIES management message.

[Richard C. <ric...@gm...>]

Signed-off-by: Richard Cochran <ric...@gm...>
---
 clock.c      | 70 +++++++++++++++++++++++++++++++++++++++++++++++++---
 pmc.c        | 17 +++++++++++++
 pmc_common.c | 24 +++++++++++++++++-
 tlv.c        | 21 ++++++++++++++++
 util.h       |  7 ++++++
 5 files changed, 134 insertions(+), 5 deletions(-)

diff --git a/clock.c b/clock.c
index 767599b..93f00b1 100644
--- a/clock.c
+++ b/clock.c
@@ -157,6 +157,32 @@ static int clock_resize_pollfd(struct clock *c, int new_nports);
 static void clock_remove_port(struct clock *c, struct port *p);
 static void clock_stats_display(struct clock_stats *s);
 
+uint8_t clock_alttime_offset_get_key(struct ptp_message *req)
+{
+	struct management_tlv_datum *mtd;
+	struct management_tlv *mgt =
+		(struct management_tlv *) req->management.suffix;
+
+	/*
+	 * The data field of incoming management request messages is
+	 * normally ignored.  Indeed it can even be empty.  However
+	 * the ALTERNATE_TIME_OFFSET requests are exceptional because
+	 * the key field selects one of the configured time zones.
+	 *
+	 * Provide the first time zone for an empty GET, and validate
+	 * the length of the request when non-empty.
+	 */
+	if (mgt->length == sizeof(mgt->id)) {
+		return 0;
+	}
+	if (mgt->length < sizeof(mgt->id) + sizeof(*mtd)) {
+		return MAX_TIME_ZONES;
+	}
+	mtd = (struct management_tlv_datum *) mgt->data;
+
+	return mtd->val;
+}
+
 static void remove_subscriber(struct clock_subscriber *s)
 {
 	LIST_REMOVE(s, list);
@@ -354,6 +380,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 					  struct ptp_message *req,
 					  struct ptp_message *rsp, int id)
 {
+	struct alternate_time_offset_properties *atop;
 	struct grandmaster_settings_np *gsn;
 	struct management_tlv_datum *mtd;
 	struct subscribe_events_np *sen;
@@ -363,6 +390,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 	struct PTPText *text;
 	uint16_t duration;
 	int datalen = 0;
+	uint8_t key;
 
 	extra = tlv_extra_alloc();
 	if (!extra) {
@@ -433,6 +461,24 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 		mtd->val = c->tds.flags & PTP_TIMESCALE;
 		datalen = sizeof(*mtd);
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		key = clock_alttime_offset_get_key(req);
+		if (key >= MAX_TIME_ZONES) {
+			break;
+		}
+		atop = (struct alternate_time_offset_properties *) tlv->data;
+		atop->keyField = key;
+		/* Message alignment broken by design. */
+		memcpy(&atop->currentOffset, &c->tz[key].current_offset,
+		       sizeof(atop->currentOffset));
+		memcpy(&atop->jumpSeconds, &c->tz[key].jump_seconds,
+		       sizeof(atop->jumpSeconds));
+		memcpy(&atop->timeOfNextJump.seconds_lsb, &c->tz[key].next_jump_lsb,
+		       sizeof(atop->timeOfNextJump.seconds_lsb));
+		memcpy(&atop->timeOfNextJump.seconds_msb, &c->tz[key].next_jump_msb,
+		       sizeof(atop->timeOfNextJump.seconds_msb));
+		datalen = sizeof(*atop);
+		break;
 	case MID_TIME_STATUS_NP:
 		tsn = (struct time_status_np *) tlv->data;
 		tsn->master_offset = tmv_to_nanoseconds(c->master_offset);
@@ -511,11 +557,12 @@ static int clock_management_get_response(struct clock *c, struct port *p,
 static int clock_management_set(struct clock *c, struct port *p,
 				int id, struct ptp_message *req, int *changed)
 {
-	int respond = 0;
-	struct management_tlv *tlv;
-	struct management_tlv_datum *mtd;
+	struct alternate_time_offset_properties *atop;
 	struct grandmaster_settings_np *gsn;
+	struct management_tlv_datum *mtd;
 	struct subscribe_events_np *sen;
+	struct management_tlv *tlv;
+	int key, respond = 0;
 
 	tlv = (struct management_tlv *) req->management.suffix;
 
@@ -532,6 +579,22 @@ static int clock_management_set(struct clock *c, struct port *p,
 		*changed = 1;
 		respond = 1;
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		atop = (struct alternate_time_offset_properties *) tlv->data;
+		key = atop->keyField;
+		if (key < MAX_TIME_ZONES) {
+			/* Message alignment broken by design. */
+			memcpy(&c->tz[key].current_offset, &atop->currentOffset,
+			       sizeof(c->tz[key].current_offset));
+			memcpy(&c->tz[key].jump_seconds, &atop->jumpSeconds,
+			       sizeof(c->tz[key].jump_seconds));
+			memcpy(&c->tz[key].next_jump_lsb, &atop->timeOfNextJump.seconds_lsb,
+			       sizeof(c->tz[key].next_jump_lsb));
+			memcpy(&c->tz[key].next_jump_msb, &atop->timeOfNextJump.seconds_msb,
+			       sizeof(c->tz[key].next_jump_msb));
+			respond = 1;
+		}
+		break;
 	case MID_GRANDMASTER_SETTINGS_NP:
 		gsn = (struct grandmaster_settings_np *) tlv->data;
 		c->dds.clockQuality = gsn->clockQuality;
@@ -1529,7 +1592,6 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg)
 	case MID_ALTERNATE_TIME_OFFSET_ENABLE:
 	case MID_ALTERNATE_TIME_OFFSET_NAME:
 	case MID_ALTERNATE_TIME_OFFSET_MAX_KEY:
-	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
 	case MID_TRANSPARENT_CLOCK_DEFAULT_DATA_SET:
 	case MID_PRIMARY_DOMAIN:
 	case MID_TIME_STATUS_NP:
diff --git a/pmc.c b/pmc.c
index 793a790..fd8b978 100644
--- a/pmc.c
+++ b/pmc.c
@@ -157,6 +157,7 @@ static void pmc_show_signaling(struct ptp_message *msg, FILE *fp)
 
 static void pmc_show(struct ptp_message *msg, FILE *fp)
 {
+	struct alternate_time_offset_properties *atop;
 	struct ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
@@ -176,6 +177,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 	struct defaultDS *dds;
 	struct currentDS *cds;
 	struct parentDS *pds;
+	uint64_t next_jump;
 	struct portDS *p;
 	struct TLV *tlv;
 	uint8_t *buf;
@@ -359,6 +361,21 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 		fprintf(fp, "TIMESCALE_PROPERTIES "
 			IFMT "ptpTimescale %d", mtd->val & PTP_TIMESCALE ? 1 : 0);
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		atop = (struct alternate_time_offset_properties *) mgt->data;
+		next_jump = atop->timeOfNextJump.seconds_msb;
+		next_jump <<= 32;
+		next_jump |= atop->timeOfNextJump.seconds_lsb;
+		fprintf(fp, "ALTERNATE_TIME_OFFSET_PROPERTIES "
+			IFMT "keyField       %hhu"
+			IFMT "currentOffset  %d"
+			IFMT "jumpSeconds    %d"
+			IFMT "timeOfNextJump %" PRIu64,
+			atop->keyField,
+			align32(&atop->currentOffset),
+			align32(&atop->jumpSeconds),
+			next_jump);
+		break;
 	case MID_MASTER_ONLY:
 		mtd = (struct management_tlv_datum *) mgt->data;
 		fprintf(fp, "MASTER_ONLY "
diff --git a/pmc_common.c b/pmc_common.c
index bb7d087..4ae9db0 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -123,7 +123,7 @@ struct management_id idtab[] = {
 	{ "ALTERNATE_TIME_OFFSET_ENABLE", MID_ALTERNATE_TIME_OFFSET_ENABLE, not_supported },
 	{ "ALTERNATE_TIME_OFFSET_NAME", MID_ALTERNATE_TIME_OFFSET_NAME, not_supported },
 	{ "ALTERNATE_TIME_OFFSET_MAX_KEY", MID_ALTERNATE_TIME_OFFSET_MAX_KEY, not_supported },
-	{ "ALTERNATE_TIME_OFFSET_PROPERTIES", MID_ALTERNATE_TIME_OFFSET_PROPERTIES, not_supported },
+	{ "ALTERNATE_TIME_OFFSET_PROPERTIES", MID_ALTERNATE_TIME_OFFSET_PROPERTIES, do_set_action },
 	{ "MASTER_ONLY", MID_MASTER_ONLY, do_get_action },
 	{ "TRANSPARENT_CLOCK_DEFAULT_DATA_SET", MID_TRANSPARENT_CLOCK_DEFAULT_DATA_SET, not_supported },
 	{ "PRIMARY_DOMAIN", MID_PRIMARY_DOMAIN, not_supported },
@@ -170,6 +170,7 @@ static void do_set_action(struct pmc *pmc, int action, int index, char *str)
 {
 	int cnt, code = idtab[index].code, freq_traceable, leap_59, leap_61,
 		ptp_timescale, time_traceable, utc_off_valid;
+	struct alternate_time_offset_properties atop;
 	struct ieee_c37_238_settings_np pwr;
 	struct grandmaster_settings_np gsn;
 	struct management_tlv_datum mtd;
@@ -205,6 +206,24 @@ static void do_set_action(struct pmc *pmc, int action, int index, char *str)
 		}
 		pmc_send_set_action(pmc, code, &mtd, sizeof(mtd));
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		memset(&atop, 0, sizeof(atop));
+		cnt = sscanf(str, " %*s %*s "
+			     "keyField       %hhu "
+			     "currentOffset  %d "
+			     "jumpSeconds    %d "
+			     "timeOfNextJump %u ",
+			     &atop.keyField,
+			     &atop.currentOffset,
+			     &atop.jumpSeconds,
+			     &atop.timeOfNextJump.seconds_lsb);
+		if (cnt != 4) {
+			fprintf(stderr, "%s SET needs 4 values\n",
+				idtab[index].name);
+			break;
+		}
+		pmc_send_set_action(pmc, code, &atop, sizeof(atop));
+		break;
 	case MID_GRANDMASTER_SETTINGS_NP:
 		cnt = sscanf(str, " %*s %*s "
 			     "clockClass              %hhu "
@@ -575,6 +594,9 @@ static int pmc_tlv_datalen(struct pmc *pmc, int id)
 	case MID_TIME_STATUS_NP:
 		len += sizeof(struct time_status_np);
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		len += sizeof(struct alternate_time_offset_properties);
+		break;
 	case MID_GRANDMASTER_SETTINGS_NP:
 		len += sizeof(struct grandmaster_settings_np);
 		break;
diff --git a/tlv.c b/tlv.c
index 212414f..740d57b 100644
--- a/tlv.c
+++ b/tlv.c
@@ -167,6 +167,7 @@ static void alttime_offset_pre_send(struct tlv_extra *extra)
 static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 			 struct tlv_extra *extra)
 {
+	struct alternate_time_offset_properties *atop;
 	struct ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
@@ -337,6 +338,17 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 		p->portIdentity.portNumber = ntohs(p->portIdentity.portNumber);
 		p->peerMeanPathDelay = net2host64(p->peerMeanPathDelay);
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		atop = (struct alternate_time_offset_properties *) m->data;
+		if (data_len != sizeof(*atop)) {
+			goto bad_length;
+		}
+		/* Message alignment broken by design. */
+		net2host32_unaligned(&atop->currentOffset);
+		net2host32_unaligned(&atop->jumpSeconds);
+		flip16(&atop->timeOfNextJump.seconds_msb);
+		net2host32_unaligned(&atop->timeOfNextJump.seconds_lsb);
+		break;
 	case MID_TIME_STATUS_NP:
 		if (data_len != sizeof(struct time_status_np))
 			goto bad_length;
@@ -486,6 +498,7 @@ bad_length:
 
 static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 {
+	struct alternate_time_offset_properties *atop;
 	struct ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
@@ -548,6 +561,14 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 		p->portIdentity.portNumber = htons(p->portIdentity.portNumber);
 		p->peerMeanPathDelay = host2net64(p->peerMeanPathDelay);
 		break;
+	case MID_ALTERNATE_TIME_OFFSET_PROPERTIES:
+		atop = (struct alternate_time_offset_properties *) m->data;
+		/* Message alignment broken by design. */
+		host2net32_unaligned(&atop->currentOffset);
+		host2net32_unaligned(&atop->jumpSeconds);
+		flip16(&atop->timeOfNextJump.seconds_msb);
+		host2net32_unaligned(&atop->timeOfNextJump.seconds_lsb);
+		break;
 	case MID_TIME_STATUS_NP:
 		tsn = (struct time_status_np *) m->data;
 		tsn->master_offset = host2net64(tsn->master_offset);
diff --git a/util.h b/util.h
index 542f3b5..2bbde71 100644
--- a/util.h
+++ b/util.h
@@ -66,6 +66,13 @@ static inline uint16_t align16(void *p)
 	return v;
 }
 
+static inline uint32_t align32(void *p)
+{
+	uint32_t v;
+	memcpy(&v, p, sizeof(v));
+	return v;
+}
+
 char *bin2str_impl(Octet *data, int len, char *buf, int buf_len);
 
 /**
-- 
2.30.2