From f233528ea4ba9960845a9b705aec9cc9d3deb2b1 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Thu, 2 Aug 2012 06:33:32 +0200
Subject: [PATCH] Provide a port method to allocate a management message reply.

This function will be needed for both positive replies and error status
messages.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 msg.h  | 10 ++++++++++
 port.c | 39 +++++++++++++++++++++++++++++++++++++++
 port.h | 16 ++++++++++++++++
 3 files changed, 65 insertions(+)

diff --git a/msg.h b/msg.h
index 8147383..f9ae85a 100644
--- a/msg.h
+++ b/msg.h
@@ -204,6 +204,16 @@ struct ptp_message {
 	int tlv_count;
 };
 
+/**
+ * Obtain the action field from a management message.
+ * @param m  A management message.
+ * @return   The value of the action field.
+ */
+static inline uint8_t management_action(struct ptp_message *m)
+{
+	return m->management.flags & 0x0f;
+}
+
 /**
  * Test a given bit in a message's flag field.
  * @param m      Message to test.
diff --git a/port.c b/port.c
index ae4b9dd..e9e2cf9 100644
--- a/port.c
+++ b/port.c
@@ -1390,6 +1390,45 @@ int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg)
 	return 0;
 }
 
+struct ptp_message *port_management_reply(struct PortIdentity pid,
+					  struct port *ingress,
+					  struct ptp_message *req)
+{
+	struct ptp_message *msg;
+	int pdulen;
+
+	msg = msg_allocate();
+	if (!msg)
+		return NULL;
+
+	pdulen = sizeof(struct management_msg);
+	msg->hwts.type = ingress->timestamping;
+
+	msg->header.tsmt               = MANAGEMENT | ingress->transportSpecific;
+	msg->header.ver                = PTP_VERSION;
+	msg->header.messageLength      = pdulen;
+	msg->header.domainNumber       = clock_domain_number(ingress->clock);
+	msg->header.sourcePortIdentity = pid;
+	msg->header.sequenceId         = req->header.sequenceId;
+	msg->header.control            = CTL_MANAGEMENT;
+	msg->header.logMessageInterval = 0x7f;
+
+	msg->management.targetPortIdentity = req->header.sourcePortIdentity;
+	msg->management.startingBoundaryHops =
+		req->management.startingBoundaryHops - req->management.boundaryHops;
+	msg->management.boundaryHops = msg->management.startingBoundaryHops;
+
+	switch (management_action(req)) {
+	case GET: case SET:
+		msg->management.flags = RESPONSE;
+		break;
+	case COMMAND:
+		msg->management.flags = ACKNOWLEDGE;
+		break;
+	}
+	return msg;
+}
+
 struct port *port_open(struct port_defaults *pod,
 		       int phc_index,
 		       char *name,
diff --git a/port.h b/port.h
index bc9c3df..6e7c25e 100644
--- a/port.h
+++ b/port.h
@@ -97,6 +97,22 @@ int port_forward(struct port *p, struct ptp_message *msg, int msglen);
  */
 int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg);
 
+/**
+ * Allocate a reply to a management message.
+ *
+ * Messages are reference counted, and newly allocated messages have a
+ * reference count of one. Allocated messages are freed using the
+ * function @ref msg_put().
+ *
+ * @param pid      The id of the responding port.
+ * @param ingress  The port on which 'req' was received.
+ * @param req      A management message.
+ * @return         Pointer to a message on success, NULL otherwise.
+ */
+struct ptp_message *port_management_reply(struct PortIdentity pid,
+					  struct port *ingress,
+					  struct ptp_message *req);
+
 /**
  * Open a network port.
  * @param pod           A pointer to a default port data set for this port.