[Linuxptp-devel] [PATCH v4 04/11] 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 | 37 +++++++++++++++++++++++++++++++++++++
 port.c       | 20 +++++++++++++++++++-
 tlv.c        | 20 ++++++++++++++++++++
 tlv.h        |  1 +
 5 files changed, 92 insertions(+), 1 deletion(-)

diff --git a/pmc.c b/pmc.c
index e218ca4..793a790 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 ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
 	struct port_service_stats_np *pssp;
@@ -572,6 +573,20 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 			phn->phc_index,
 			phn->flags);
 		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 1dd89f6..bb7d087 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
@@ -154,6 +155,7 @@ struct management_id idtab[] = {
 	{ "PORT_SERVICE_STATS_NP", MID_PORT_SERVICE_STATS_NP, do_get_action },
 	{ "UNICAST_MASTER_TABLE_NP", MID_UNICAST_MASTER_TABLE_NP, do_get_action },
 	{ "PORT_HWCLOCK_NP", MID_PORT_HWCLOCK_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)
@@ -168,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 ieee_c37_238_settings_np pwr;
 	struct grandmaster_settings_np gsn;
 	struct management_tlv_datum mtd;
 	struct subscribe_events_np sen;
@@ -302,6 +305,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;
 	}
 }
 
@@ -573,6 +607,9 @@ static int pmc_tlv_datalen(struct pmc *pmc, int id)
 	case MID_PORT_HWCLOCK_NP:
 		len += sizeof(struct port_hwclock_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 7bcf88c..7200a87 100644
--- a/port.c
+++ b/port.c
@@ -877,6 +877,7 @@ static const Octet profile_id_8275_2[] = {0x00, 0x19, 0xA7, 0x02, 0x01, 0x02};
 static int port_management_fill_response(struct port *target,
 					 struct ptp_message *rsp, int id)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct unicast_master_address *ucma;
 	struct port_service_stats_np *pssn;
@@ -1123,6 +1124,11 @@ static int port_management_fill_response(struct port *target,
 			PORT_HWCLOCK_VCLOCK : 0;
 		datalen = sizeof(*phn);
 		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);
@@ -1164,9 +1170,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;
 
@@ -1176,6 +1183,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 29d0556..2e421ed 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 unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
 	struct port_service_stats_np *pssn;
@@ -399,6 +400,16 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 		phn->phc_index = ntohl(phn->phc_index);
 		extra_len = sizeof(struct port_hwclock_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:
@@ -422,6 +433,7 @@ bad_length:
 
 static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 {
+	struct ieee_c37_238_settings_np *pwr;
 	struct unicast_master_table_np *umtn;
 	struct grandmaster_settings_np *gsn;
 	struct port_service_stats_np *pssn;
@@ -570,6 +582,14 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 		phn->portIdentity.portNumber = htons(phn->portIdentity.portNumber);
 		phn->phc_index = htonl(phn->phc_index);
 		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 a435588..3dbce4f 100644
--- a/tlv.h
+++ b/tlv.h
@@ -128,6 +128,7 @@ enum management_action {
 #define MID_PORT_SERVICE_STATS_NP			0xC007
 #define MID_UNICAST_MASTER_TABLE_NP			0xC008
 #define MID_PORT_HWCLOCK_NP				0xC009
+#define MID_POWER_PROFILE_SETTINGS_NP			0xC00A
 
 /* Management error ID values */
 #define MID_RESPONSE_TOO_BIG				0x0001
-- 
2.30.2