From 00a663ca687b96990dcff95c952da58647f68aa7 Mon Sep 17 00:00:00 2001
From: Jiri Benc <jbenc@redhat.com>
Date: Wed, 11 Jun 2014 21:35:20 +0200
Subject: [PATCH] phc2sys: propagate received errors

Recognize errors returned in MANAGEMENT_ERROR_STATUS TLV and return
a distinct value from run_pmc in case such error is received.

Signed-off-by: Jiri Benc <jbenc@redhat.com>
---
 phc2sys.c | 34 +++++++++++++++++++++++++++-------
 1 file changed, 27 insertions(+), 7 deletions(-)

diff --git a/phc2sys.c b/phc2sys.c
index 3af1e51..678235e 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -526,9 +526,11 @@ static int is_msg_mgt(struct ptp_message *msg)
 	if (msg->tlv_count != 1)
 		return 0;
 	tlv = (struct TLV *) msg->management.suffix;
-	if (tlv->type != TLV_MANAGEMENT)
-		return 0;
-	return 1;
+	if (tlv->type == TLV_MANAGEMENT)
+		return 1;
+	if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS)
+		return -1;
+	return 0;
 }
 
 static int get_mgt_id(struct ptp_message *msg)
@@ -543,6 +545,14 @@ static void *get_mgt_data(struct ptp_message *msg)
 	return mgt->data;
 }
 
+static int get_mgt_err_id(struct ptp_message *msg)
+{
+	struct management_error_status *mgt;
+
+	mgt = (struct management_error_status *)msg->management.suffix;
+	return mgt->id;
+}
+
 static int normalize_state(int state)
 {
 	if (state != PS_MASTER && state != PS_SLAVE &&
@@ -630,12 +640,18 @@ static int init_pmc(struct node *node, int domain_number)
 	return 0;
 }
 
+/* Return values:
+ * 1: success
+ * 0: timeout
+ * -1: error reported by the other side
+ * -2: local error, fatal
+ */
 static int run_pmc(struct node *node, int timeout, int ds_id,
 		   struct ptp_message **msg)
 {
 #define N_FD 1
 	struct pollfd pollfd[N_FD];
-	int cnt;
+	int cnt, res;
 
 	while (1) {
 		pollfd[0].fd = pmc_get_transport_fd(node->pmc);
@@ -646,7 +662,7 @@ static int run_pmc(struct node *node, int timeout, int ds_id,
 		cnt = poll(pollfd, N_FD, timeout);
 		if (cnt < 0) {
 			pr_err("poll failed");
-			return -1;
+			return -2;
 		}
 		if (!cnt) {
 			/* Request the data set again in the next run. */
@@ -676,8 +692,12 @@ static int run_pmc(struct node *node, int timeout, int ds_id,
 		if (!*msg)
 			continue;
 
-		if (!is_msg_mgt(*msg) ||
-		    recv_subscribed(node, *msg, ds_id) ||
+		res = is_msg_mgt(*msg);
+		if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
+			node->pmc_ds_requested = 0;
+			return -1;
+		}
+		if (res <= 0 || recv_subscribed(node, *msg, ds_id) ||
 		    get_mgt_id(*msg) != ds_id) {
 			msg_put(*msg);
 			*msg = NULL;