diff --git a/msg.c b/msg.c index 24bfe26..6ded539 100644 --- a/msg.c +++ b/msg.c @@ -142,6 +142,32 @@ static void port_id_pre_send(struct PortIdentity *pid) pid->portNumber = htons(pid->portNumber); } +static int suffix_post_recv(uint8_t *ptr, int len) +{ + int cnt; + struct TLV *tlv; + + 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) { + break; + } + len -= sizeof(struct TLV); + ptr += sizeof(struct TLV); + if (tlv->length > len) { + break; + } + len -= tlv->length; + ptr += tlv->length; + } + return cnt; +} + static void timestamp_post_recv(struct ptp_message *m, struct Timestamp *ts) { uint32_t lsb = ntohl(ts->seconds_lsb); @@ -193,6 +219,7 @@ void msg_get(struct ptp_message *m) int msg_post_recv(struct ptp_message *m, int cnt) { int pdulen, type; + uint8_t *suffix = NULL; if (cnt < sizeof(struct ptp_header)) return -1; @@ -254,21 +281,28 @@ int msg_post_recv(struct ptp_message *m, int cnt) break; case FOLLOW_UP: timestamp_post_recv(m, &m->follow_up.preciseOriginTimestamp); + suffix = m->follow_up.suffix; break; case DELAY_RESP: timestamp_post_recv(m, &m->delay_resp.receiveTimestamp); + suffix = m->delay_resp.suffix; break; case PDELAY_RESP_FOLLOW_UP: timestamp_post_recv(m, &m->pdelay_resp_fup.responseOriginTimestamp); port_id_post_recv(&m->pdelay_resp_fup.requestingPortIdentity); + suffix = m->pdelay_resp_fup.suffix; break; case ANNOUNCE: clock_gettime(CLOCK_MONOTONIC, &m->ts.host); timestamp_post_recv(m, &m->announce.originTimestamp); announce_post_recv(&m->announce); + suffix = m->announce.suffix; break; case SIGNALING: + suffix = m->signaling.suffix; + break; case MANAGEMENT: + suffix = m->management.suffix; break; } @@ -277,6 +311,8 @@ int msg_post_recv(struct ptp_message *m, int cnt) return -1; } + m->tlv_count = suffix_post_recv(suffix, cnt - pdulen); + return 0; } diff --git a/msg.h b/msg.h index d737ee2..8147383 100644 --- a/msg.h +++ b/msg.h @@ -198,6 +198,10 @@ struct ptp_message { * SO_TIMESTAMPING socket option. */ struct hw_timestamp hwts; + /** + * Contains the number of TLVs in the suffix. + */ + int tlv_count; }; /**