From e0005dac945cf012b2ccb7549aee9684f073d9ce Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Fri, 23 Feb 2018 22:47:17 -0800 Subject: [PATCH] port: Convert to the API for appending TLVs on the transmit path. The current code uses an ad hoc method of appending TLVs. When constructing a message, the code computes the total PDU length by adding the message size to the TLV size. By using the new API, this patch simplifies message construction, letting each TLV add its own length to the total. As a result of the this change, the return value for the helper functions, follow_up_info_append() and path_trace_append(), has changed meaning. Instead of returning the TLV length, these functions now provide an error code. Signed-off-by: Richard Cochran --- port.c | 91 +++++++++++++++++++++++++++++++++++----------------------- 1 file changed, 55 insertions(+), 36 deletions(-) diff --git a/port.c b/port.c index 60a28be..9c0b4ef 100644 --- a/port.c +++ b/port.c @@ -435,13 +435,19 @@ static int add_foreign_master(struct port *p, struct ptp_message *m) static int follow_up_info_append(struct port *p, struct ptp_message *m) { struct follow_up_info_tlv *fui; - fui = (struct follow_up_info_tlv *) m->follow_up.suffix; + struct tlv_extra *extra; + + extra = msg_tlv_append(m, sizeof(*fui)); + if (!extra) { + return -1; + } + fui = (struct follow_up_info_tlv *) extra->tlv; fui->type = TLV_ORGANIZATION_EXTENSION; fui->length = sizeof(*fui) - sizeof(fui->type) - sizeof(fui->length); memcpy(fui->id, ieee8021_id, sizeof(ieee8021_id)); fui->subtype[2] = 1; - m->tlv_count = 1; - return sizeof(*fui); + + return 0; } static struct follow_up_info_tlv *follow_up_info_extract(struct ptp_message *m) @@ -498,19 +504,28 @@ static int incapable_ignore(struct port *p, struct ptp_message *m) static int path_trace_append(struct port *p, struct ptp_message *m, struct parent_ds *dad) { + int length = 1 + dad->path_length, ptt_len, tlv_len; struct path_trace_tlv *ptt; - int length = 1 + dad->path_length; + struct tlv_extra *extra; if (length > PATH_TRACE_MAX) { - return 0; + return -1; } - ptt = (struct path_trace_tlv *) m->announce.suffix; + + ptt_len = length * sizeof(struct ClockIdentity); + tlv_len = ptt_len + sizeof(ptt->type) + sizeof(ptt->length); + + extra = msg_tlv_append(m, tlv_len); + if (!extra) { + return -1; + } + ptt = (struct path_trace_tlv *) extra->tlv; ptt->type = TLV_PATH_TRACE; - ptt->length = length * sizeof(struct ClockIdentity); + ptt->length = ptt_len; memcpy(ptt->cid, dad->ptl, ptt->length); ptt->cid[length - 1] = clock_identity(p->clock); - m->tlv_count = 1; - return ptt->length + sizeof(ptt->type) + sizeof(ptt->length); + + return 0; } static int path_trace_ignore(struct port *p, struct ptp_message *m) @@ -1272,10 +1287,10 @@ out: static int port_tx_announce(struct port *p) { - struct parent_ds *dad = clock_parent_ds(p->clock); struct timePropertiesDS *tp = clock_time_properties(p->clock); + struct parent_ds *dad = clock_parent_ds(p->clock); struct ptp_message *msg; - int err, pdulen; + int err; if (!port_capable(p)) { return 0; @@ -1284,15 +1299,11 @@ static int port_tx_announce(struct port *p) if (!msg) return -1; - pdulen = sizeof(struct announce_msg); msg->hwts.type = p->timestamping; - if (p->path_trace_enabled) - pdulen += path_trace_append(p, msg, dad); - msg->header.tsmt = ANNOUNCE | p->transportSpecific; msg->header.ver = PTP_VERSION; - msg->header.messageLength = pdulen; + msg->header.messageLength = sizeof(struct announce_msg); msg->header.domainNumber = clock_domain_number(p->clock); msg->header.sourcePortIdentity = p->portIdentity; msg->header.sequenceId = p->seqnum.announce++; @@ -1309,6 +1320,10 @@ static int port_tx_announce(struct port *p) msg->announce.stepsRemoved = clock_steps_removed(p->clock); msg->announce.timeSource = tp->timeSource; + if (p->path_trace_enabled && path_trace_append(p, msg, dad)) { + pr_err("port %hu: append path trace failed", portnum(p)); + } + err = port_prepare_and_send(p, msg, 0); if (err) pr_err("port %hu: send announce failed", portnum(p)); @@ -1319,8 +1334,9 @@ static int port_tx_announce(struct port *p) static int port_tx_sync(struct port *p) { struct ptp_message *msg, *fup; - int err, pdulen; - int event = p->timestamping == TS_ONESTEP ? TRANS_ONESTEP : TRANS_EVENT; + int err, event; + + event = p->timestamping == TS_ONESTEP ? TRANS_ONESTEP : TRANS_EVENT; if (!port_capable(p)) { return 0; @@ -1337,12 +1353,11 @@ static int port_tx_sync(struct port *p) return -1; } - pdulen = sizeof(struct sync_msg); msg->hwts.type = p->timestamping; msg->header.tsmt = SYNC | p->transportSpecific; msg->header.ver = PTP_VERSION; - msg->header.messageLength = pdulen; + msg->header.messageLength = sizeof(struct sync_msg); msg->header.domainNumber = clock_domain_number(p->clock); msg->header.sourcePortIdentity = p->portIdentity; msg->header.sequenceId = p->seqnum.sync++; @@ -1368,15 +1383,11 @@ static int port_tx_sync(struct port *p) /* * Send the follow up message right away. */ - pdulen = sizeof(struct follow_up_msg); fup->hwts.type = p->timestamping; - if (p->follow_up_info) - pdulen += follow_up_info_append(p, fup); - fup->header.tsmt = FOLLOW_UP | p->transportSpecific; fup->header.ver = PTP_VERSION; - fup->header.messageLength = pdulen; + fup->header.messageLength = sizeof(struct follow_up_msg); fup->header.domainNumber = clock_domain_number(p->clock); fup->header.sourcePortIdentity = p->portIdentity; fup->header.sequenceId = p->seqnum.sync - 1; @@ -1385,6 +1396,12 @@ static int port_tx_sync(struct port *p) ts_to_timestamp(&msg->hwts.ts, &fup->follow_up.preciseOriginTimestamp); + if (p->follow_up_info && follow_up_info_append(p, fup)) { + pr_err("port %hu: append fup info failed", portnum(p)); + err = -1; + goto out; + } + err = port_prepare_and_send(p, fup, 0); if (err) pr_err("port %hu: send follow up failed", portnum(p)); @@ -2524,24 +2541,28 @@ int port_manage(struct port *p, struct port *ingress, struct ptp_message *msg) int port_management_error(struct PortIdentity pid, struct port *ingress, struct ptp_message *req, Enumeration16 error_id) { - struct ptp_message *msg; - struct management_tlv *mgt; struct management_error_status *mes; - int err = 0, pdulen; + struct management_tlv *mgt; + struct ptp_message *msg; + struct tlv_extra *extra; + int err = 0; + mgt = (struct management_tlv *) req->management.suffix; msg = port_management_reply(pid, ingress, req); if (!msg) { return -1; } - mgt = (struct management_tlv *) req->management.suffix; - mes = (struct management_error_status *) msg->management.suffix; + + extra = msg_tlv_append(msg, sizeof(*mes)); + if (!extra) { + msg_put(msg); + return -ENOMEM; + } + mes = (struct management_error_status *) extra->tlv; mes->type = TLV_MANAGEMENT_ERROR_STATUS; mes->length = 8; mes->error = error_id; mes->id = mgt->id; - pdulen = msg->header.messageLength + sizeof(*mes); - msg->header.messageLength = pdulen; - msg->tlv_count = 1; err = port_prepare_and_send(ingress, msg, 0); msg_put(msg); @@ -2555,18 +2576,16 @@ port_management_construct(struct PortIdentity pid, struct port *ingress, UInteger8 boundaryHops, uint8_t action) { 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.messageLength = sizeof(struct management_msg); msg->header.domainNumber = clock_domain_number(ingress->clock); msg->header.sourcePortIdentity = pid; msg->header.sequenceId = sequenceId;