diff --git a/clock.c b/clock.c index 89cdd78..5f13a5a 100644 --- a/clock.c +++ b/clock.c @@ -340,9 +340,17 @@ static int clock_management_fill_response(struct clock *c, struct port *p, struct subscribe_events_np *sen; struct management_tlv *tlv; struct time_status_np *tsn; + struct tlv_extra *extra; struct PTPText *text; int datalen = 0; + extra = tlv_extra_alloc(); + if (!extra) { + pr_err("failed to allocate TLV descriptor"); + return 0; + } + extra->tlv = (struct TLV *) rsp->management.suffix; + tlv = (struct management_tlv *) rsp->management.suffix; tlv->type = TLV_MANAGEMENT; tlv->id = id; @@ -448,7 +456,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p, } tlv->length = sizeof(tlv->id) + datalen; rsp->header.messageLength += sizeof(*tlv) + datalen; - rsp->tlv_count = 1; + msg_tlv_attach(rsp, extra); /* The caller can respond to this message. */ return 1; diff --git a/msg.c b/msg.c index adab384..345de7e 100644 --- a/msg.c +++ b/msg.c @@ -120,33 +120,43 @@ static void port_id_pre_send(struct PortIdentity *pid) pid->portNumber = htons(pid->portNumber); } -static int suffix_post_recv(uint8_t *ptr, int len, struct tlv_extra *last) +static int suffix_post_recv(struct ptp_message *msg, uint8_t *ptr, int len) { - int cnt, err; - struct TLV *tlv; + struct tlv_extra *extra; + int err; if (!ptr) return 0; - for (cnt = 0; len > sizeof(struct TLV); cnt++) { - tlv = (struct TLV *) ptr; - tlv->type = ntohs(tlv->type); - tlv->length = ntohs(tlv->length); - if (tlv->length % 2) { + while (len > sizeof(struct TLV)) { + extra = tlv_extra_alloc(); + if (!extra) { + pr_err("failed to allocate TLV descriptor"); + return -ENOMEM; + } + extra->tlv = (struct TLV *) ptr; + extra->tlv->type = ntohs(extra->tlv->type); + extra->tlv->length = ntohs(extra->tlv->length); + if (extra->tlv->length % 2) { + tlv_extra_recycle(extra); return -EBADMSG; } len -= sizeof(struct TLV); ptr += sizeof(struct TLV); - if (tlv->length > len) { + if (extra->tlv->length > len) { + tlv_extra_recycle(extra); return -EBADMSG; } - len -= tlv->length; - ptr += tlv->length; - err = tlv_post_recv(tlv, len > sizeof(struct TLV) ? NULL : last); - if (err) + len -= extra->tlv->length; + ptr += extra->tlv->length; + err = tlv_post_recv(extra); + if (err) { + tlv_extra_recycle(extra); return err; + } + msg_tlv_attach(msg, extra); } - return cnt; + return 0; } static void suffix_pre_send(uint8_t *ptr, int cnt, struct tlv_extra *last) @@ -324,9 +334,9 @@ int msg_post_recv(struct ptp_message *m, int cnt) if (msg_sots_missing(m)) return -ETIME; - m->tlv_count = suffix_post_recv(suffix, cnt - pdulen, &m->last_tlv); - if (m->tlv_count < 0) - return m->tlv_count; + err = suffix_post_recv(m, suffix, cnt - pdulen); + if (err) + return err; return 0; } @@ -381,10 +391,17 @@ int msg_pre_send(struct ptp_message *m) default: return -1; } - suffix_pre_send(suffix, m->tlv_count, &m->last_tlv); + suffix_pre_send(suffix, m->tlv_count, m->last_tlv); return 0; } +void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra) +{ + TAILQ_INSERT_TAIL(&msg->tlv_list, extra, list); + msg->tlv_count++; + msg->last_tlv = extra; +} + const char *msg_type_string(int type) { switch (type) { diff --git a/msg.h b/msg.h index 3d67c23..7d7b64e 100644 --- a/msg.h +++ b/msg.h @@ -233,7 +233,7 @@ struct ptp_message { * the layout of the TLV makes it difficult to access the data * directly from the message's buffer. */ - struct tlv_extra last_tlv; + struct tlv_extra *last_tlv; }; /** @@ -258,6 +258,14 @@ static inline Boolean field_is_set(struct ptp_message *m, int index, Octet bit) return m->header.flagField[index] & bit ? TRUE : FALSE; } +/** + * Place a TLV descriptor into a message's list of TLVs. + * + * @param msg A message obtained using msg_allocate(). + * @param extra The TLV to be added to the list. + */ +void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra); + /** * Obtain the transportSpecific field from a message. * @param m Message to test. diff --git a/pmc.c b/pmc.c index 49282a6..fde2449 100644 --- a/pmc.c +++ b/pmc.c @@ -194,8 +194,10 @@ static void pmc_show(struct ptp_message *msg, FILE *fp) struct time_status_np *tsn; struct grandmaster_settings_np *gsn; struct mgmt_clock_description *cd; + struct tlv_extra *extra; struct portDS *p; struct port_ds_np *pnp; + if (msg_type(msg) != MANAGEMENT) { return; } @@ -209,6 +211,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp) if (msg->tlv_count != 1) { goto out; } + extra = TAILQ_FIRST(&msg->tlv_list); tlv = (struct TLV *) msg->management.suffix; if (tlv->type == TLV_MANAGEMENT) { fprintf(fp, "MANAGEMENT "); @@ -226,7 +229,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp) } switch (mgt->id) { case TLV_CLOCK_DESCRIPTION: - cd = &msg->last_tlv.cd; + cd = &extra->cd; fprintf(fp, "CLOCK_DESCRIPTION " IFMT "clockType 0x%hx" IFMT "physicalLayerProtocol %s" @@ -252,7 +255,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp) case TLV_USER_DESCRIPTION: fprintf(fp, "USER_DESCRIPTION " IFMT "userDescription %s", - text2str(msg->last_tlv.cd.userDescription)); + text2str(extra->cd.userDescription)); break; case TLV_DEFAULT_DATA_SET: dds = (struct defaultDS *) mgt->data; diff --git a/pmc_common.c b/pmc_common.c index c1b0d3e..2a58081 100644 --- a/pmc_common.c +++ b/pmc_common.c @@ -228,6 +228,8 @@ int pmc_send_get_action(struct pmc *pmc, int id) int datalen, pdulen; struct ptp_message *msg; struct management_tlv *mgt; + struct tlv_extra *extra; + msg = pmc_message(pmc, GET); if (!msg) { return -1; @@ -239,14 +241,21 @@ int pmc_send_get_action(struct pmc *pmc, int id) mgt->id = id; pdulen = msg->header.messageLength + sizeof(*mgt) + datalen; msg->header.messageLength = pdulen; - msg->tlv_count = 1; + + extra = tlv_extra_alloc(); + if (!extra) { + pr_err("failed to allocate TLV descriptor"); + return -ENOMEM; + } + extra->tlv = (struct TLV *) msg->management.suffix; + msg_tlv_attach(msg, extra); if (id == TLV_CLOCK_DESCRIPTION && !pmc->zero_length_gets) { /* * Make sure the tlv_extra pointers dereferenced in * mgt_pre_send() do point to something. */ - struct mgmt_clock_description *cd = &msg->last_tlv.cd; + struct mgmt_clock_description *cd = &extra->cd; uint8_t *buf = mgt->data; cd->clockType = (UInteger16 *) buf; buf += sizeof(*cd->clockType); diff --git a/port.c b/port.c index 59e8b5e..60a28be 100644 --- a/port.c +++ b/port.c @@ -711,11 +711,19 @@ static int port_management_fill_response(struct port *target, struct port_properties_np *ppn; struct management_tlv *tlv; struct port_ds_np *pdsnp; + struct tlv_extra *extra; struct portDS *pds; uint16_t u16; uint8_t *buf; int datalen; + extra = tlv_extra_alloc(); + if (!extra) { + pr_err("failed to allocate TLV descriptor"); + return 0; + } + extra->tlv = (struct TLV *) rsp->management.suffix; + tlv = (struct management_tlv *) rsp->management.suffix; tlv->type = TLV_MANAGEMENT; tlv->id = id; @@ -725,7 +733,7 @@ static int port_management_fill_response(struct port *target, datalen = 0; break; case TLV_CLOCK_DESCRIPTION: - cd = &rsp->last_tlv.cd; + cd = &extra->cd; buf = tlv->data; cd->clockType = (UInteger16 *) buf; buf += sizeof(*cd->clockType); @@ -867,7 +875,7 @@ static int port_management_fill_response(struct port *target, } tlv->length = sizeof(tlv->id) + datalen; rsp->header.messageLength += sizeof(*tlv) + datalen; - rsp->tlv_count = 1; + msg_tlv_attach(rsp, extra); /* The caller can respond to this message. */ return 1; diff --git a/tlv.c b/tlv.c index aa4dafa..4811f19 100644 --- a/tlv.c +++ b/tlv.c @@ -444,15 +444,13 @@ void tlv_extra_recycle(struct tlv_extra *extra) TAILQ_INSERT_HEAD(&tlv_pool, extra, list); } -int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra) +int tlv_post_recv(struct tlv_extra *extra) { int result = 0; struct management_tlv *mgt; struct management_error_status *mes; + struct TLV *tlv = extra->tlv; struct path_trace_tlv *ptt; - struct tlv_extra dummy_extra; - if (!extra) - extra = &dummy_extra; switch (tlv->type) { case TLV_MANAGEMENT: diff --git a/tlv.h b/tlv.h index fd41022..5a919fb 100644 --- a/tlv.h +++ b/tlv.h @@ -256,12 +256,10 @@ void tlv_extra_recycle(struct tlv_extra *extra); /** * Converts recognized value sub-fields into host byte order. - * @param tlv Pointer to a Type Length Value field. - * @param extra Additional struct where data from tlv will be saved, - * can be NULL. + * @param extra TLV descriptor pointing to the protocol data. * @return Zero if successful, otherwise non-zero */ -int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra); +int tlv_post_recv(struct tlv_extra *extra); /** * Converts recognized value sub-fields into network byte order.