[Linuxptp-devel] [PATCH v4 03/11] Introduce the power profile.

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

Signed-off-by: Richard Cochran <ric...@gm...>
---
 config.c        | 15 +++++++++++++-
 port.c          | 53 +++++++++++++++++++++++++++++++++++++++++++++++++
 port_private.h  |  3 +++
 power_profile.h | 31 +++++++++++++++++++++++++++++
 tlv.h           |  8 ++++++++
 5 files changed, 109 insertions(+), 1 deletion(-)
 create mode 100644 power_profile.h

diff --git a/config.c b/config.c
index 6684c9f..4c58a0a 100644
--- a/config.c
+++ b/config.c
@@ -31,6 +31,7 @@
 #include "config.h"
 #include "ether.h"
 #include "hash.h"
+#include "power_profile.h"
 #include "print.h"
 #include "util.h"
 
@@ -66,7 +67,7 @@ typedef union {
 	char *s;
 } any_t;
 
-#define CONFIG_LABEL_SIZE 32
+#define CONFIG_LABEL_SIZE 64
 
 #define CFG_ITEM_STATIC (1 << 0) /* statically allocated, not to be freed */
 #define CFG_ITEM_LOCKED (1 << 1) /* command line value, may not be changed */
@@ -189,6 +190,13 @@ static struct config_enum hwts_filter_enu[] = {
 	{ NULL, 0 },
 };
 
+static struct config_enum ieee_c37_238_enu[] = {
+	{ "none", IEEE_C37_238_VERSION_NONE },
+	{ "2011", IEEE_C37_238_VERSION_2011 },
+	{ "2017", IEEE_C37_238_VERSION_2017 },
+	{ NULL, 0 },
+};
+
 static struct config_enum nw_trans_enu[] = {
 	{ "L2",    TRANS_IEEE_802_3 },
 	{ "UDPv4", TRANS_UDP_IPV4   },
@@ -298,6 +306,11 @@ struct config_item config_tab[] = {
 	GLOB_ITEM_DBL("pi_proportional_exponent", -0.3, -DBL_MAX, DBL_MAX),
 	GLOB_ITEM_DBL("pi_proportional_norm_max", 0.7, DBL_MIN, 1.0),
 	GLOB_ITEM_DBL("pi_proportional_scale", 0.0, 0.0, DBL_MAX),
+	PORT_ITEM_ENU("power_profile.version", IEEE_C37_238_VERSION_NONE, ieee_c37_238_enu),
+	PORT_ITEM_INT("power_profile.2011.grandmasterTimeInaccuracy", 0xFFFFFFFF, 0, INT_MAX),
+	PORT_ITEM_INT("power_profile.2011.networkTimeInaccuracy", 0, 0, INT_MAX),
+	PORT_ITEM_INT("power_profile.2017.totalTimeInaccuracy", 0xFFFFFFFF, 0, INT_MAX),
+	PORT_ITEM_INT("power_profile.grandmasterID", 0, 0, 0xFFFF),
 	GLOB_ITEM_INT("priority1", 128, 0, UINT8_MAX),
 	GLOB_ITEM_INT("priority2", 128, 0, UINT8_MAX),
 	GLOB_ITEM_STR("productDescription", ";;"),
diff --git a/port.c b/port.c
index 6baf5c8..7bcf88c 100644
--- a/port.c
+++ b/port.c
@@ -453,6 +453,46 @@ static int follow_up_info_append(struct ptp_message *m)
 	return 0;
 }
 
+static int ieee_c37_238_append(struct port *p, struct ptp_message *m)
+{
+	struct ieee_c37_238_2017_tlv *p17;
+	struct ieee_c37_238_2011_tlv *p11;
+	struct tlv_extra *extra;
+
+	switch (p->pwr.version) {
+	case IEEE_C37_238_VERSION_NONE:
+		return 0;
+	case IEEE_C37_238_VERSION_2011:
+		extra = msg_tlv_append(m, sizeof(*p11));
+		if (!extra) {
+			return -1;
+		}
+		p11 = (struct ieee_c37_238_2011_tlv *) extra->tlv;
+		p11->type = TLV_ORGANIZATION_EXTENSION;
+		p11->length = sizeof(*p11) - sizeof(p11->type) - sizeof(p11->length);
+		memcpy(p11->id, ieeec37_238_id, sizeof(ieeec37_238_id));
+		p11->subtype[2] = 1;
+		p11->grandmasterID = p->pwr.grandmasterID;
+		p11->grandmasterTimeInaccuracy = p->pwr.grandmasterTimeInaccuracy;
+		p11->networkTimeInaccuracy = p->pwr.networkTimeInaccuracy;
+		break;
+	case IEEE_C37_238_VERSION_2017:
+		extra = msg_tlv_append(m, sizeof(*p17));
+		if (!extra) {
+			return -1;
+		}
+		p17 = (struct ieee_c37_238_2017_tlv *) extra->tlv;
+		p17->type = TLV_ORGANIZATION_EXTENSION;
+		p17->length = sizeof(*p17) - sizeof(p17->type) - sizeof(p17->length);
+		memcpy(p17->id, ieeec37_238_id, sizeof(ieeec37_238_id));
+		p17->subtype[2] = 2;
+		p17->grandmasterID = p->pwr.grandmasterID;
+		p17->totalTimeInaccuracy = p->pwr.totalTimeInaccuracy;
+		break;
+	}
+	return 0;
+}
+
 static int net_sync_resp_append(struct port *p, struct ptp_message *m)
 {
 	struct timePropertiesDS tp = clock_time_properties(p->clock);
@@ -1622,6 +1662,9 @@ int port_tx_announce(struct port *p, struct address *dst, uint16_t sequence_id)
 	if (p->path_trace_enabled && path_trace_append(p, msg, dad)) {
 		pr_err("%s: append path trace failed", p->log_name);
 	}
+	if (ieee_c37_238_append(p, msg)) {
+		pr_err("%s: append power profile failed", p->log_name);
+	}
 
 	err = port_prepare_and_send(p, msg, TRANS_GENERAL);
 	if (err) {
@@ -3326,6 +3369,16 @@ struct port *port_open(const char *phc_device,
 	p->state = PS_INITIALIZING;
 	p->delayMechanism = config_get_int(cfg, p->name, "delay_mechanism");
 	p->versionNumber = PTP_MAJOR_VERSION;
+	p->pwr.version =
+		config_get_int(cfg, p->name, "power_profile.version");
+	p->pwr.grandmasterID =
+		config_get_int(cfg, p->name, "power_profile.grandmasterID");
+	p->pwr.grandmasterTimeInaccuracy =
+		config_get_int(cfg, p->name, "power_profile.2011.grandmasterTimeInaccuracy");
+	p->pwr.networkTimeInaccuracy =
+		config_get_int(cfg, p->name, "power_profile.2011.networkTimeInaccuracy");
+	p->pwr.totalTimeInaccuracy =
+		config_get_int(cfg, p->name, "power_profile.2017.totalTimeInaccuracy");
 	p->slave_event_monitor = clock_slave_monitor(clock);
 
 	if (!port_is_uds(p) && unicast_client_initialize(p)) {
diff --git a/port_private.h b/port_private.h
index d27dceb..204ec61 100644
--- a/port_private.h
+++ b/port_private.h
@@ -26,6 +26,7 @@
 #include "fsm.h"
 #include "monitor.h"
 #include "msg.h"
+#include "power_profile.h"
 #include "tmv.h"
 
 #define NSEC2SEC 1000000000LL
@@ -151,6 +152,8 @@ struct port {
 	LIST_HEAD(fm, foreign_clock) foreign_masters;
 	/* TC book keeping */
 	TAILQ_HEAD(tct, tc_txd) tc_transmitted;
+	/* power profile */
+	struct ieee_c37_238_settings_np pwr;
 	/* unicast client mode */
 	struct unicast_master_table *unicast_master_table;
 	/* unicast service mode */
diff --git a/power_profile.h b/power_profile.h
new file mode 100644
index 0000000..6a7c8a4
--- /dev/null
+++ b/power_profile.h
@@ -0,0 +1,31 @@
+/**
+ * @file power_profile.h
+ * @brief Implements power profile features.
+ * @note Copyright (C) 2021 Richard Cochran <ric...@gm...>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef HAVE_POWER_PROFILE_H
+#define HAVE_POWER_PROFILE_H
+
+#include "pdt.h"
+
+enum power_profile_version {
+	IEEE_C37_238_VERSION_NONE,
+	IEEE_C37_238_VERSION_2011,
+	IEEE_C37_238_VERSION_2017,
+};
+
+#endif
diff --git a/tlv.h b/tlv.h
index 409c8d0..a435588 100644
--- a/tlv.h
+++ b/tlv.h
@@ -324,6 +324,14 @@ struct ieee_c37_238_2017_tlv {
 	Octet         pad[2];
 } PACKED;
 
+struct ieee_c37_238_settings_np {
+	Enumeration16 version;
+	UInteger16    grandmasterID;
+	UInteger32    grandmasterTimeInaccuracy;
+	UInteger32    networkTimeInaccuracy;
+	UInteger32    totalTimeInaccuracy;
+} PACKED;
+
 struct msg_interval_req_tlv {
 	Enumeration16 type;
 	UInteger16    length;
-- 
2.30.2