sad: introduce sad_update_auth_tlv()

Some messages do not need a new authentication tlv attached but instead
only require the existing tlv to be updated because a protected field
was edited. These include messages being forward via a transparent clock
and management messages.

Introduce sad_update_auth_tlv() to search for existing authentication
tlvs, confirm their spp/security parameters are known, and regenerate
the icv.

Because this function still requires valid tlv pointers, add
msg_tlv_copy() to copy the pointers from the formatted message to the
raw message duplicate.

In addition add various calls to e2e_tc/p2p_tc to match port.c

Signed-off-by: Clay Kaiser <Clay.Kaiser@ibm.com>
Reviewed-by: Erez Geva <ErezGeva2@gmail.com>
Reviewed-by: Miroslav Lichvar <mlichvar@redhat.com>

clock.c

@@ -36,6 +36,7 @@
 #include "msg.h"
 #include "phc.h"
 #include "port.h"
+#include "sad.h"
 #include "servo.h"
 #include "stats.h"
 #include "print.h"
@@ -335,6 +336,7 @@ void clock_send_notification(struct clock *c, struct ptp_message *msg,
 		msg->management.targetPortIdentity.portNumber =
 			htons(s->targetPortIdentity.portNumber);
 		msg->address = s->addr;
+		sad_update_auth_tlv(clock_config(c), msg);
 		port_forward_to(uds, msg);
 	}
 }
@@ -1576,6 +1578,7 @@ static int clock_do_forward_mgmt(struct clock *c,
 		/* delay calling msg_pre_send until
 		 * actually forwarding */
 		msg_pre_send(msg);
+		sad_update_auth_tlv(clock_config(c), msg);
 		*pre_sent = 1;
 	}
 	return port_forward(out, msg);

e2e_tc.c

@@ -22,6 +22,7 @@
 #include "port_private.h"
 #include "print.h"
 #include "rtnl.h"
+#include "sad.h"
 #include "tc.h"
 
 void e2e_dispatch(struct port *p, enum fsm_event event, int mdiff)
@@ -50,6 +51,7 @@ void e2e_dispatch(struct port *p, enum fsm_event event, int mdiff)
 	case PS_FAULTY:
 	case PS_DISABLED:
 		port_disable(p);
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
 		break;
 	case PS_LISTENING:
 		port_set_announce_tmo(p);
@@ -60,13 +62,15 @@ void e2e_dispatch(struct port *p, enum fsm_event event, int mdiff)
 		break;
 	case PS_MASTER:
 	case PS_GRAND_MASTER:
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
 		break;
 	case PS_PASSIVE:
 		port_set_announce_tmo(p);
 		break;
 	case PS_UNCALIBRATED:
 		flush_last_sync(p);
 		flush_delay_req(p);
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
 		/* fall through */
 	case PS_SLAVE:
 		port_set_announce_tmo(p);
@@ -76,7 +80,7 @@ void e2e_dispatch(struct port *p, enum fsm_event event, int mdiff)
 
 enum fsm_event e2e_event(struct port *p, int fd_index)
 {
-	int cnt, fd = p->fda.fd[fd_index];
+	int cnt, err, fd = p->fda.fd[fd_index];
 	enum fsm_event event = EV_NONE;
 	struct ptp_message *msg, *dup;
 
@@ -160,9 +164,27 @@ enum fsm_event e2e_event(struct port *p, int fd_index)
 		msg_put(msg);
 		return EV_NONE;
 	}
+	msg_tlv_copy(dup, msg);
 	if (tc_ignore(p, dup)) {
 		msg_put(dup);
 		dup = NULL;
+	} else {
+		err = sad_process_auth(clock_config(p->clock), p->spp, dup, msg);
+		if (err) {
+			switch (err) {
+			case -EBADMSG:
+				pr_err("%s: auth: bad message", p->log_name);
+				break;
+			case -EPROTO:
+				pr_debug("%s: auth: ignoring message", p->log_name);
+				break;
+			}
+			msg_put(msg);
+			if (dup) {
+				msg_put(dup);
+			}
+			return EV_NONE;
+		}
 	}
 
 	switch (msg_type(msg)) {

msg.c

@@ -190,6 +190,8 @@ static int suffix_post_recv(struct ptp_message *msg, int len)
 	if (!ptr)
 		return 0;
 
+	msg_tlv_recycle(msg);
+
 	while (len >= sizeof(struct TLV)) {
 		extra = tlv_extra_alloc();
 		if (!extra) {
@@ -234,7 +236,6 @@ static void suffix_pre_send(struct ptp_message *msg)
 		tlv->type = htons(tlv->type);
 		tlv->length = htons(tlv->length);
 	}
-	msg_tlv_recycle(msg);
 }
 
 static void timestamp_post_recv(struct ptp_message *m, struct Timestamp *ts)
@@ -517,6 +518,27 @@ int msg_tlv_count(struct ptp_message *msg)
 	return count;
 }
 
+int msg_tlv_copy(struct ptp_message *msg, struct ptp_message *dup) {
+	struct tlv_extra *extra, *dup_extra;
+	struct TLV *tlv;
+
+	if (msg_type(msg) != msg_type(dup)) {
+		return -1;
+	}
+	if (msg->header.messageLength != ntohs(dup->header.messageLength)) {
+		return -1;
+	}
+
+	TAILQ_FOREACH(extra, &msg->tlv_list, list) {
+		tlv = (void *) extra->tlv - (void *) msg + (void *) dup;
+		dup_extra = tlv_extra_alloc();
+		dup_extra->tlv = tlv;
+		msg_tlv_attach(dup, dup_extra);
+	}
+
+	return 0;
+}
+
 const char *msg_type_string(int type)
 {
 	switch (type) {

msg.h

@@ -309,6 +309,18 @@ struct tlv_extra *msg_tlv_append(struct ptp_message *msg, int length);
  */
 void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra);
 
+/**
+ * Copy list of TLVs from message that has gone through @ref msg_post_recv()
+ * to a network byte order duplicate message. This is useful for TC applications
+ * where any auth tlvs must be updated on the raw forwarded messages.
+ * @param msg  A message obtained using @ref msg_allocate().
+ *             The passed message must have been passed to @ref msg_post_recv()
+ *             in order to have tlv pointers attached.
+ * @param dup  A duplicate of msg that is still in network byte order.
+ * @return     -1 if the messages do not match, otherwise 0
+ */
+int msg_tlv_copy(struct ptp_message *msg, struct ptp_message *dup);
+
 /*
  * Return the number of TLVs attached to a message.
  * @param msg  A message obtained using @ref msg_allocate().

p2p_tc.c

@@ -22,6 +22,7 @@
 #include "port_private.h"
 #include "print.h"
 #include "rtnl.h"
+#include "sad.h"
 #include "tc.h"
 
 static int p2p_delay_request(struct port *p)
@@ -69,6 +70,7 @@ void p2p_dispatch(struct port *p, enum fsm_event event, int mdiff)
 	case PS_FAULTY:
 	case PS_DISABLED:
 		port_disable(p);
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
 		break;
 	case PS_LISTENING:
 		port_set_announce_tmo(p);
@@ -79,11 +81,14 @@ void p2p_dispatch(struct port *p, enum fsm_event event, int mdiff)
 		break;
 	case PS_MASTER:
 	case PS_GRAND_MASTER:
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
 		break;
 	case PS_PASSIVE:
 		port_set_announce_tmo(p);
 		break;
 	case PS_UNCALIBRATED:
+		sad_set_last_seqid(clock_config(p->clock), p->spp, -1);
+		/* fall through */
 	case PS_SLAVE:
 		port_set_announce_tmo(p);
 		break;
@@ -92,7 +97,7 @@ void p2p_dispatch(struct port *p, enum fsm_event event, int mdiff)
 
 enum fsm_event p2p_event(struct port *p, int fd_index)
 {
-	int cnt, fd = p->fda.fd[fd_index];
+	int cnt, err, fd = p->fda.fd[fd_index];
 	enum fsm_event event = EV_NONE;
 	struct ptp_message *msg, *dup;
 
@@ -163,9 +168,27 @@ enum fsm_event p2p_event(struct port *p, int fd_index)
 		msg_put(msg);
 		return EV_NONE;
 	}
+	msg_tlv_copy(dup, msg);
 	if (tc_ignore(p, dup)) {
 		msg_put(dup);
 		dup = NULL;
+	} else {
+		err = sad_process_auth(clock_config(p->clock), p->spp, dup, msg);
+		if (err) {
+			switch (err) {
+			case -EBADMSG:
+				pr_err("%s: auth: bad message", p->log_name);
+				break;
+			case -EPROTO:
+				pr_debug("%s: auth: ignoring message", p->log_name);
+				break;
+			}
+			msg_put(msg);
+			if (dup) {
+				msg_put(dup);
+			}
+			return EV_NONE;
+		}
 	}
 
 	switch (msg_type(msg)) {

sad.c

@@ -99,6 +99,60 @@ static int sad_generate_icv(struct security_association *sa,
 	return icv_len;
 }
 
+int sad_update_auth_tlv(struct config *cfg,
+			struct ptp_message *msg)
+{
+	struct tlv_extra *extra;
+	struct authentication_tlv *auth;
+	struct security_association *sa;
+	struct security_association_key *key;
+	void *sequenceNo, *res, *icv;
+	/* update any/all authentication tlvs now */
+	TAILQ_FOREACH(extra, &msg->tlv_list, list) {
+		if (ntohs(extra->tlv->type) != TLV_AUTHENTICATION) {
+			continue;
+		}
+		auth = (struct authentication_tlv *) extra->tlv;
+		/* retrieve sa specified by spp in tlv */
+		sa = sad_get_association(cfg, auth->spp);
+		if (!sa) {
+			return -1;
+		}
+		/* verify res, seqnum, disclosedKey field indicators match expectations */
+		if ((sa->res_ind != ((auth->secParamIndicator & 0x1) != 0)) ||
+		    (sa->seqnum_ind != ((auth->secParamIndicator & 0x2) != 0)) ||
+		    (sa->immediate_ind == ((auth->secParamIndicator & 0x4) != 0))) {
+			pr_debug("sa %u: unable to update auth tlv,"
+				 " sec param %d does not match",
+				 sa->spp, auth->secParamIndicator);
+			return -1;
+		}
+		/* retrieve key specified by keyid in tlv */
+		key = sad_get_key(sa, ntohl(auth->keyID));
+		if (!key) {
+			pr_debug("sa %u: unable to update auth tlv,"
+				 " unable to retrieve key %u",
+				 sa->spp, ntohl(auth->keyID));
+			return -1;
+		}
+		/* confirm tlv length to be generated matches what already exists */
+		if (ntohs(auth->length) != sad_get_auth_tlv_len(sa, key->icv->digest_len) - 4) {
+			pr_debug("sa %u: unable to update auth tlv,"
+				 " length is not maintained", sa->spp);
+			return -1;
+		}
+		/* set pointers to extra data used for icv generation */
+		sequenceNo = auth->data;
+		res = sequenceNo + (sa->seqnum_ind ? sa->seqnum_len : 0);
+		icv = res + (sa->res_ind ? sa->res_len : 0);
+		if (!sad_generate_icv(sa, key, msg, icv)) {
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
 /**
  * append auth tlv to outbound messages. This includes:
  * 1. retrieve security association, key

sad.h

@@ -25,6 +25,17 @@ struct security_association {
 	Integer32  last_seqid;
 };
 
+/**
+ * Update authentication tlvs for message passing through (tc). Pass
+ * a network byte order message that still contains tlv pointers.
+ * Attempt to recalculate icv for each authentication tlv attached.
+ * @param cfg  pointer to config that contains sad
+ * @param msg  msg that the authentication tlvs should be updated for
+ * @return     -1 if sa/key is unknown, otherwise 0
+ */
+int sad_update_auth_tlv(struct config *cfg,
+			struct ptp_message *msg);
+
 /**
  * Append authentication tlv to outbound messages. Includes
  * msg_pre_send() to put message in network byte order so the icv can

tc.c

@@ -20,6 +20,7 @@
 
 #include "port.h"
 #include "print.h"
+#include "sad.h"
 #include "tc.h"
 #include "tmv.h"
 
@@ -162,6 +163,7 @@ static void tc_complete_response(struct port *q, struct port *p,
 	c1 = net2host64(resp->header.correction);
 	c2 = c1 + tmv_to_TimeInterval(residence);
 	resp->header.correction = host2net64(c2);
+	sad_update_auth_tlv(clock_config(q->clock), resp);
 	cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, resp);
 	if (cnt <= 0) {
 		pr_err("tc failed to forward response on %s", p->log_name);
@@ -223,6 +225,7 @@ static void tc_complete_syfup(struct port *q, struct port *p,
 	c2 += tmv_to_TimeInterval(q->peer_delay);
 	c2 += q->asymmetry;
 	fup->header.correction = host2net64(c2);
+	sad_update_auth_tlv(clock_config(q->clock), fup);
 	cnt = transport_send(p->trp, &p->fda, TRANS_GENERAL, fup);
 	if (cnt <= 0) {
 		pr_err("tc failed to forward follow up on %s", p->log_name);
@@ -388,6 +391,7 @@ int tc_forward(struct port *q, struct ptp_message *msg)
 	if (q->tc_spanning_tree && msg_type(msg) == ANNOUNCE) {
 		steps_removed = ntohs(msg->announce.stepsRemoved);
 		msg->announce.stepsRemoved = htons(1 + steps_removed);
+		sad_update_auth_tlv(clock_config(q->clock), msg);
 	}
 
 	for (p = clock_first_port(q->clock); p; p = LIST_NEXT(p, list)) {
@@ -457,7 +461,10 @@ int tc_fwd_sync(struct port *q, struct ptp_message *msg)
 		fup->header.sequenceId         = msg->header.sequenceId;
 		fup->header.logMessageInterval = msg->header.logMessageInterval;
 		fup->follow_up.preciseOriginTimestamp = msg->sync.originTimestamp;
+		sad_append_auth_tlv(clock_config(q->clock), q->spp,
+				    q->active_key_id, fup);
 		msg->header.flagField[0]      |= TWO_STEP;
+		sad_update_auth_tlv(clock_config(q->clock), msg);
 	}
 	err = tc_fwd_event(q, msg);
 	if (err) {