diff --git a/msg.c b/msg.c index 79fee6d..8fd2008 100644 --- a/msg.c +++ b/msg.c @@ -144,7 +144,7 @@ static void port_id_pre_send(struct PortIdentity *pid) pid->portNumber = htons(pid->portNumber); } -static int suffix_post_recv(uint8_t *ptr, int len) +static int suffix_post_recv(uint8_t *ptr, int len, struct tlv_extra *last) { int cnt; struct TLV *tlv; @@ -166,14 +166,14 @@ static int suffix_post_recv(uint8_t *ptr, int len) } len -= tlv->length; ptr += tlv->length; - if (tlv_post_recv(tlv)) { + if (tlv_post_recv(tlv, len ? NULL : last)) { return -1; } } return cnt; } -static void suffix_pre_send(uint8_t *ptr, int cnt) +static void suffix_pre_send(uint8_t *ptr, int cnt, struct tlv_extra *last) { int i; struct TLV *tlv; @@ -183,7 +183,7 @@ static void suffix_pre_send(uint8_t *ptr, int cnt) for (i = 0; i < cnt; i++) { tlv = (struct TLV *) ptr; - tlv_pre_send(tlv); + tlv_pre_send(tlv, i == cnt - 1 ? last : NULL); ptr += sizeof(struct TLV) + tlv->length; tlv->type = htons(tlv->type); tlv->length = htons(tlv->length); @@ -344,7 +344,7 @@ int msg_post_recv(struct ptp_message *m, int cnt) return -1; } - m->tlv_count = suffix_post_recv(suffix, cnt - pdulen); + m->tlv_count = suffix_post_recv(suffix, cnt - pdulen, &m->last_tlv); if (m->tlv_count == -1) { return -1; } @@ -401,7 +401,7 @@ int msg_pre_send(struct ptp_message *m) default: return -1; } - suffix_pre_send(suffix, m->tlv_count); + suffix_pre_send(suffix, m->tlv_count, &m->last_tlv); return 0; } diff --git a/msg.h b/msg.h index 2feb804..279f3e6 100644 --- a/msg.h +++ b/msg.h @@ -26,6 +26,7 @@ #include "ddt.h" #include "transport.h" +#include "tlv.h" #define PTP_VERSION 2 @@ -204,6 +205,12 @@ struct ptp_message { * Contains the number of TLVs in the suffix. */ int tlv_count; + /** + * Used to hold the data of the last TLV in the message when + * the layout of the TLV makes it difficult to access the data + * directly from the message's buffer. + */ + struct tlv_extra last_tlv; }; /** diff --git a/tlv.c b/tlv.c index d1093ee..5471d4b 100644 --- a/tlv.c +++ b/tlv.c @@ -21,6 +21,7 @@ #include "port.h" #include "tlv.h" +#include "msg.h" #define TLV_LENGTH_INVALID(tlv, type) \ (tlv->length < sizeof(struct type) - sizeof(struct TLV)) @@ -41,7 +42,8 @@ static void scaled_ns_h2n(ScaledNs *sns) sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds); } -static int mgt_post_recv(struct management_tlv *m, uint16_t data_len) +static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, + struct tlv_extra *extra) { struct defaultDS *dds; struct currentDS *cds; @@ -49,6 +51,7 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len) struct timePropertiesDS *tp; struct portDS *p; struct time_status_np *tsn; + int extra_len = 0; switch (m->id) { case DEFAULT_DATA_SET: if (data_len != sizeof(struct defaultDS)) @@ -105,12 +108,18 @@ static int mgt_post_recv(struct management_tlv *m, uint16_t data_len) tsn->gmPresent = ntohl(tsn->gmPresent); break; } + if (extra_len) { + if (extra_len % 2) + extra_len++; + if (extra_len + sizeof(m->id) != m->length) + goto bad_length; + } return 0; bad_length: return -1; } -static void mgt_pre_send(struct management_tlv *m) +static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra) { struct defaultDS *dds; struct currentDS *cds; @@ -209,12 +218,15 @@ static void org_pre_send(struct organization_tlv *org) } } -int tlv_post_recv(struct TLV *tlv) +int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra) { int result = 0; struct management_tlv *mgt; struct management_error_status *mes; struct path_trace_tlv *ptt; + struct tlv_extra dummy_extra; + if (!extra) + extra = &dummy_extra; switch (tlv->type) { case TLV_MANAGEMENT: @@ -223,7 +235,7 @@ int tlv_post_recv(struct TLV *tlv) mgt = (struct management_tlv *) tlv; mgt->id = ntohs(mgt->id); if (tlv->length > sizeof(mgt->id)) - result = mgt_post_recv(mgt, tlv->length - sizeof(mgt->id)); + result = mgt_post_recv(mgt, tlv->length - sizeof(mgt->id), extra); break; case TLV_MANAGEMENT_ERROR_STATUS: if (TLV_LENGTH_INVALID(tlv, management_error_status)) @@ -261,7 +273,7 @@ bad_length: return -1; } -void tlv_pre_send(struct TLV *tlv) +void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra) { struct management_tlv *mgt; struct management_error_status *mes; @@ -270,7 +282,7 @@ void tlv_pre_send(struct TLV *tlv) case TLV_MANAGEMENT: mgt = (struct management_tlv *) tlv; if (tlv->length > sizeof(mgt->id)) - mgt_pre_send(mgt); + mgt_pre_send(mgt, extra); mgt->id = htons(mgt->id); break; case TLV_MANAGEMENT_ERROR_STATUS: diff --git a/tlv.h b/tlv.h index 8cce615..cfcab68 100644 --- a/tlv.h +++ b/tlv.h @@ -21,7 +21,6 @@ #define HAVE_TLV_H #include "ddt.h" -#include "msg.h" /* TLV types */ #define TLV_MANAGEMENT 0x0001 @@ -177,17 +176,28 @@ struct time_status_np { struct ClockIdentity gmIdentity; } PACKED; +struct tlv_extra { + union { + /* Empty for now, but will contain structs for the + * TLVs that use the tlv_extra support. */ + }; +}; + /** * 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. * @return Zero if successful, otherwise non-zero */ -int tlv_post_recv(struct TLV *tlv); +int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra); /** * Converts recognized value sub-fields into network byte order. * @param tlv Pointer to a Type Length Value field. + * @param extra Additional struct containing tlv data to send, can be + * NULL. */ -void tlv_pre_send(struct TLV *tlv); +void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra); #endif