From 424bbde8fcca09a07375736c96041dd04467cbf4 Mon Sep 17 00:00:00 2001
From: Jiri Benc <jbenc@redhat.com>
Date: Tue, 6 May 2014 18:41:51 +0200
Subject: [PATCH] Custom management TLV PORT_PROPERTIES_NP

Will be used by phc2sys to find out interfaces corresponding to ports.

Signed-off-by: Jiri Benc <jbenc@redhat.com>
---
 clock.c |  9 +++++++++
 port.c  | 13 +++++++++++++
 tlv.c   | 14 ++++++++++++++
 tlv.h   |  8 ++++++++
 4 files changed, 44 insertions(+)

diff --git a/clock.c b/clock.c
index 32ba490..4378dec 100644
--- a/clock.c
+++ b/clock.c
@@ -1024,6 +1024,15 @@ int clock_manage(struct clock *c, struct port *p, struct ptp_message *msg)
 		return changed;
 	}
 
+	switch (mgt->id) {
+	case PORT_PROPERTIES_NP:
+		if (p != c->port[c->nports]) {
+			/* Only the UDS port allowed. */
+			clock_management_send_error(p, msg, NOT_SUPPORTED);
+			return 0;
+		}
+	}
+
 	switch (mgt->id) {
 	case USER_DESCRIPTION:
 	case SAVE_IN_NON_VOLATILE_STORAGE:
diff --git a/port.c b/port.c
index eca4cd5..63fabc1 100644
--- a/port.c
+++ b/port.c
@@ -611,6 +611,7 @@ static int port_management_fill_response(struct port *target,
 	struct management_tlv_datum *mtd;
 	struct portDS *pds;
 	struct port_ds_np *pdsnp;
+	struct port_properties_np *ppn;
 	struct clock_description *desc;
 	struct mgmt_clock_description *cd;
 	uint8_t *buf;
@@ -760,6 +761,18 @@ static int port_management_fill_response(struct port *target,
 		datalen = sizeof(*pdsnp);
 		respond = 1;
 		break;
+	case PORT_PROPERTIES_NP:
+		ppn = (struct port_properties_np *)tlv->data;
+		ppn->portIdentity = target->portIdentity;
+		if (target->state == PS_GRAND_MASTER)
+			ppn->port_state = PS_MASTER;
+		else
+			ppn->port_state = target->state;
+		ppn->timestamping = target->timestamping;
+		ptp_text_set(&ppn->interface, target->name);
+		datalen = sizeof(*ppn) + ppn->interface.length;
+		respond = 1;
+		break;
 	}
 	if (respond) {
 		if (datalen % 2) {
diff --git a/tlv.c b/tlv.c
index 892e4bc..4593c77 100644
--- a/tlv.c
+++ b/tlv.c
@@ -63,6 +63,7 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 	struct time_status_np *tsn;
 	struct grandmaster_settings_np *gsn;
 	struct subscribe_events_np *sen;
+	struct port_properties_np *ppn;
 	struct mgmt_clock_description *cd;
 	int extra_len = 0, len;
 	uint8_t *buf;
@@ -249,6 +250,14 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 		sen = (struct subscribe_events_np *)m->data;
 		sen->duration = ntohs(sen->duration);
 		break;
+	case PORT_PROPERTIES_NP:
+		if (data_len < sizeof(struct port_properties_np))
+			goto bad_length;
+		ppn = (struct port_properties_np *)m->data;
+		ppn->portIdentity.portNumber = ntohs(ppn->portIdentity.portNumber);
+		extra_len = sizeof(struct port_properties_np);
+		extra_len += ppn->interface.length;
+		break;
 	case SAVE_IN_NON_VOLATILE_STORAGE:
 	case RESET_NON_VOLATILE_STORAGE:
 	case INITIALIZE:
@@ -281,6 +290,7 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 	struct time_status_np *tsn;
 	struct grandmaster_settings_np *gsn;
 	struct subscribe_events_np *sen;
+	struct port_properties_np *ppn;
 	struct mgmt_clock_description *cd;
 	switch (m->id) {
 	case CLOCK_DESCRIPTION:
@@ -349,6 +359,10 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 		sen = (struct subscribe_events_np *)m->data;
 		sen->duration = htons(sen->duration);
 		break;
+	case PORT_PROPERTIES_NP:
+		ppn = (struct port_properties_np *)m->data;
+		ppn->portIdentity.portNumber = htons(ppn->portIdentity.portNumber);
+		break;
 	}
 }
 
diff --git a/tlv.h b/tlv.h
index e5792ae..a4bc2ce 100644
--- a/tlv.h
+++ b/tlv.h
@@ -100,6 +100,7 @@ enum management_action {
 #define DELAY_MECHANISM					0x6000
 #define LOG_MIN_PDELAY_REQ_INTERVAL			0x6001
 #define PORT_DATA_SET_NP					0xC002
+#define PORT_PROPERTIES_NP				0xC004
 
 /* Management error ID values */
 #define RESPONSE_TOO_BIG				0x0001
@@ -205,6 +206,13 @@ struct subscribe_events_np {
 	uint8_t       bitmask[EVENT_BITMASK_CNT];
 } PACKED;
 
+struct port_properties_np {
+	struct PortIdentity portIdentity;
+	uint8_t port_state;
+	uint8_t timestamping;
+	struct PTPText interface;
+} PACKED;
+
 enum clock_type {
 	CLOCK_TYPE_ORDINARY   = 0x8000,
 	CLOCK_TYPE_BOUNDARY   = 0x4000,