diff --git a/clock.c b/clock.c index c999c836..14f35232 100644 --- a/clock.c +++ b/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); diff --git a/e2e_tc.c b/e2e_tc.c index 2f8e8211..82b454a4 100644 --- a/e2e_tc.c +++ b/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,6 +62,7 @@ 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); @@ -67,6 +70,7 @@ void e2e_dispatch(struct port *p, enum fsm_event event, int mdiff) 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)) { diff --git a/msg.c b/msg.c index 50e5549a..7c236c38 100644 --- a/msg.c +++ b/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) { diff --git a/msg.h b/msg.h index 06ba0c3b..4a61f909 100644 --- a/msg.h +++ b/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(). diff --git a/p2p_tc.c b/p2p_tc.c index 75cb3b96..a8ec63b7 100644 --- a/p2p_tc.c +++ b/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)) { diff --git a/sad.c b/sad.c index c30ef0c0..0169d562 100644 --- a/sad.c +++ b/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 diff --git a/sad.h b/sad.h index 3ee27b7a..3b16690e 100644 --- a/sad.h +++ b/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 diff --git a/tc.c b/tc.c index 20717c1a..442a698a 100644 --- a/tc.c +++ b/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) {