[Linuxptp-devel] [PATCH RFC v3 03/10] Add a custom management message for power profile settings.

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

Signed-off-by: Richard Cochran <ric...@gm...>
---
 pmc.c        | 15 +++++++++++++++
 pmc_common.c | 39 ++++++++++++++++++++++++++++++++++++++-
 port.c       | 20 +++++++++++++++++++-
 tlv.c        | 20 ++++++++++++++++++++
 tlv.h        |  1 +
 5 files changed, 93 insertions(+), 2 deletions(-)

diff --git a/pmc.c b/pmc.c
index a1ee787..28d96a8 100644
--- a/pmc.c
+++ b/pmc.c
@@ -139,6 +139,7 @@ static void pmc_show_signaling(struct ptp_message *msg, FILE *fp)
 
 static void pmc_show(struct ptp_message *msg, FILE *fp)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct grandmaster_settings_np *gsn;
 	struct mgmt_clock_description *cd;
 	struct subscribe_events_np *sen;
@@ -495,6 +496,20 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 			pcp->stats.txMsgType[SIGNALING],
 			pcp->stats.txMsgType[MANAGEMENT]);
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		pwr = (struct ieee_c37_238_settings_np *) mgt->data;
+		fprintf(fp, "POWER_PROFILE_SETTINGS_NP "
+			IFMT "version                   %hu"
+			IFMT "grandmasterID             0x%04hx"
+			IFMT "grandmasterTimeInaccuracy %u"
+			IFMT "networkTimeInaccuracy     %u"
+			IFMT "totalTimeInaccuracy       %u",
+			pwr->version,
+			pwr->grandmasterID,
+			pwr->grandmasterTimeInaccuracy,
+			pwr->networkTimeInaccuracy,
+			pwr->totalTimeInaccuracy);
+		break;
 	case MID_LOG_ANNOUNCE_INTERVAL:
 		mtd = (struct management_tlv_datum *) mgt->data;
 		fprintf(fp, "LOG_ANNOUNCE_INTERVAL "
diff --git a/pmc_common.c b/pmc_common.c
index 7a1dbb4..e40c3d8 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -28,6 +28,7 @@
 #include "tlv.h"
 #include "transport.h"
 #include "pmc_common.h"
+#include "power_profile.h"
 
 #define BAD_ACTION   -1
 #define BAD_ID       -1
@@ -130,8 +131,9 @@ struct management_id idtab[] = {
 	{ "DELAY_MECHANISM", MID_DELAY_MECHANISM, do_get_action },
 	{ "LOG_MIN_PDELAY_REQ_INTERVAL", MID_LOG_MIN_PDELAY_REQ_INTERVAL, do_get_action },
 	{ "PORT_DATA_SET_NP", MID_PORT_DATA_SET_NP, do_set_action },
-	{ "PORT_STATS_NP", MID_PORT_STATS_NP, do_get_action },
 	{ "PORT_PROPERTIES_NP", MID_PORT_PROPERTIES_NP, do_get_action },
+	{ "PORT_STATS_NP", MID_PORT_STATS_NP, do_get_action },
+	{ "POWER_PROFILE_SETTINGS_NP", MID_POWER_PROFILE_SETTINGS_NP, do_set_action },
 };
 
 static void do_get_action(struct pmc *pmc, int action, int index, char *str)
@@ -146,6 +148,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 ieee_c37_238_settings_np pwr;
 	struct grandmaster_settings_np gsn;
 	struct management_tlv_datum mtd;
 	struct subscribe_events_np sen;
@@ -278,6 +281,37 @@ static void do_set_action(struct pmc *pmc, int action, int index, char *str)
 		}
 		pmc_send_set_action(pmc, code, &pnp, sizeof(pnp));
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		cnt = sscanf(str, " %*s %*s "
+			     "version                   %hu "
+			     "grandmasterID             %hx "
+			     "grandmasterTimeInaccuracy %u "
+			     "networkTimeInaccuracy     %u "
+			     "totalTimeInaccuracy       %u ",
+			     &pwr.version,
+			     &pwr.grandmasterID,
+			     &pwr.grandmasterTimeInaccuracy,
+			     &pwr.networkTimeInaccuracy,
+			     &pwr.totalTimeInaccuracy);
+		if (cnt != 5) {
+			fprintf(stderr, "%s SET needs 5 values\n",
+				idtab[index].name);
+			break;
+		}
+		switch (pwr.version) {
+		case IEEE_C37_238_VERSION_NONE:
+		case IEEE_C37_238_VERSION_2011:
+		case IEEE_C37_238_VERSION_2017:
+			pmc_send_set_action(pmc, code, &pwr, sizeof(pwr));
+			break;
+		default:
+			fprintf(stderr, "\nusage: set PROFILE_SETTINGS_NP version "
+				"%hu (none), %hu (2011), or %hu (2017)\n\n",
+				IEEE_C37_238_VERSION_NONE,
+				IEEE_C37_238_VERSION_2011,
+				IEEE_C37_238_VERSION_2017);
+		}
+		break;
 	}
 }
 
@@ -531,6 +565,9 @@ static int pmc_tlv_datalen(struct pmc *pmc, int id)
 	case MID_PORT_DATA_SET_NP:
 		len += sizeof(struct port_ds_np);
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		len += sizeof(struct ieee_c37_238_settings_np);
+		break;
 	case MID_LOG_ANNOUNCE_INTERVAL:
 	case MID_ANNOUNCE_RECEIPT_TIMEOUT:
 	case MID_LOG_SYNC_INTERVAL:
diff --git a/port.c b/port.c
index acd142f..d204dab 100644
--- a/port.c
+++ b/port.c
@@ -833,6 +833,7 @@ static const Octet profile_id_p2p[] = {0x00, 0x1B, 0x19, 0x00, 0x02, 0x00};
 static int port_management_fill_response(struct port *target,
 					 struct ptp_message *rsp, int id)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct mgmt_clock_description *cd;
 	struct management_tlv_datum *mtd;
 	struct clock_description *desc;
@@ -1006,6 +1007,11 @@ static int port_management_fill_response(struct port *target,
 		psn->stats = target->stats;
 		datalen = sizeof(*psn);
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		pwr = (struct ieee_c37_238_settings_np *)tlv->data;
+		memcpy(pwr, &target->pwr, sizeof(*pwr));
+		datalen = sizeof(*pwr);
+		break;
 	default:
 		/* The caller should *not* respond to this message. */
 		tlv_extra_recycle(extra);
@@ -1047,9 +1053,10 @@ static int port_management_set(struct port *target,
 			       struct port *ingress, int id,
 			       struct ptp_message *req)
 {
-	int respond = 0;
+	struct ieee_c37_238_settings_np *pwr;
 	struct management_tlv *tlv;
 	struct port_ds_np *pdsnp;
+	int respond = 0;
 
 	tlv = (struct management_tlv *) req->management.suffix;
 
@@ -1059,6 +1066,17 @@ static int port_management_set(struct port *target,
 		target->neighborPropDelayThresh = pdsnp->neighborPropDelayThresh;
 		respond = 1;
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		pwr = (struct ieee_c37_238_settings_np *) tlv->data;
+		switch (pwr->version) {
+		case IEEE_C37_238_VERSION_NONE:
+		case IEEE_C37_238_VERSION_2011:
+		case IEEE_C37_238_VERSION_2017:
+			target->pwr = *pwr;
+			respond = 1;
+			break;
+		}
+		break;
 	}
 	if (respond && !port_management_get_response(target, ingress, id, req))
 		pr_err("%s: failed to send management set response", target->log_name);
diff --git a/tlv.c b/tlv.c
index aadfe0a..d9d2846 100644
--- a/tlv.c
+++ b/tlv.c
@@ -114,6 +114,7 @@ static bool tlv_array_invalid(struct TLV *tlv, size_t base_size, size_t item_siz
 static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 			 struct tlv_extra *extra)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct grandmaster_settings_np *gsn;
 	struct mgmt_clock_description *cd;
 	struct subscribe_events_np *sen;
@@ -332,6 +333,16 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 		}
 		extra_len = sizeof(struct port_stats_np);
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		if (data_len < sizeof(struct ieee_c37_238_settings_np))
+			goto bad_length;
+		pwr = (struct ieee_c37_238_settings_np *)m->data;
+		NTOHS(pwr->version);
+		NTOHS(pwr->grandmasterID);
+		NTOHL(pwr->grandmasterTimeInaccuracy);
+		NTOHL(pwr->networkTimeInaccuracy);
+		NTOHL(pwr->totalTimeInaccuracy);
+		break;
 	case MID_SAVE_IN_NON_VOLATILE_STORAGE:
 	case MID_RESET_NON_VOLATILE_STORAGE:
 	case MID_INITIALIZE:
@@ -355,6 +366,7 @@ bad_length:
 
 static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct grandmaster_settings_np *gsn;
 	struct mgmt_clock_description *cd;
 	struct subscribe_events_np *sen;
@@ -449,6 +461,14 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 			psn->stats.txMsgType[i] = __cpu_to_le64(psn->stats.txMsgType[i]);
 		}
 		break;
+	case MID_POWER_PROFILE_SETTINGS_NP:
+		pwr = (struct ieee_c37_238_settings_np *)m->data;
+		HTONS(pwr->version);
+		HTONS(pwr->grandmasterID);
+		HTONL(pwr->grandmasterTimeInaccuracy);
+		HTONL(pwr->networkTimeInaccuracy);
+		HTONL(pwr->totalTimeInaccuracy);
+		break;
 	}
 }
 
diff --git a/tlv.h b/tlv.h
index c15c49b..e7d2a2d 100644
--- a/tlv.h
+++ b/tlv.h
@@ -125,6 +125,7 @@ enum management_action {
 #define MID_PORT_DATA_SET_NP				0xC002
 #define MID_PORT_PROPERTIES_NP				0xC004
 #define MID_PORT_STATS_NP				0xC005
+#define MID_POWER_PROFILE_SETTINGS_NP			0xC007
 
 /* Management error ID values */
 #define MID_RESPONSE_TOO_BIG				0x0001
-- 
2.20.1