diff --git a/msg.c b/msg.c index 0082a90..d24fbc0 100644 --- a/msg.c +++ b/msg.c @@ -76,12 +76,12 @@ static void announce_post_recv(struct announce_msg *m) m->stepsRemoved = ntohs(m->stepsRemoved); } -static int64_t host2net64(int64_t val) +int64_t host2net64(int64_t val) { return __cpu_to_be64(val); } -static int64_t net2host64(int64_t val) +int64_t net2host64(int64_t val) { return __be64_to_cpu(val); } diff --git a/msg.h b/msg.h index 44c6134..b01f086 100644 --- a/msg.h +++ b/msg.h @@ -317,4 +317,14 @@ static inline Boolean one_step(struct ptp_message *m) return !field_is_set(m, 0, TWO_STEP); } +/** + * Convert a 64 bit word into network byte order. + */ +int64_t host2net64(int64_t val); + +/** + * Convert a 64 bit word into host byte order. + */ +int64_t net2host64(int64_t val); + #endif diff --git a/tlv.c b/tlv.c index bde00d6..3e84184 100644 --- a/tlv.c +++ b/tlv.c @@ -17,9 +17,66 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include +#include #include "tlv.h" +uint8_t ieee8021_id[3] = { IEEE_802_1_COMMITTEE }; + +static void scaled_ns_n2h(ScaledNs *sns) +{ + sns->nanoseconds_msb = ntohs(sns->nanoseconds_msb); + sns->nanoseconds_lsb = net2host64(sns->nanoseconds_msb); + sns->fractional_nanoseconds = ntohs(sns->fractional_nanoseconds); +} + +static void scaled_ns_h2n(ScaledNs *sns) +{ + sns->nanoseconds_msb = htons(sns->nanoseconds_msb); + sns->nanoseconds_lsb = host2net64(sns->nanoseconds_msb); + sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds); +} + +static void org_post_recv(struct organization_tlv *org) +{ + struct follow_up_info_tlv *f; + + if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) { + if (org->subtype[0] || org->subtype[1]) { + return; + } + switch (org->subtype[2]) { + case 1: + f = (struct follow_up_info_tlv *) org; + f->cumulativeScaledRateOffset = ntohl(f->cumulativeScaledRateOffset); + f->gmTimeBaseIndicator = ntohs(f->gmTimeBaseIndicator); + scaled_ns_n2h(&f->lastGmPhaseChange); + f->scaledLastGmPhaseChange = ntohl(f->scaledLastGmPhaseChange); + break; + } + } +} + +static void org_pre_send(struct organization_tlv *org) +{ + struct follow_up_info_tlv *f; + + if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) { + if (org->subtype[0] || org->subtype[1]) { + return; + } + switch (org->subtype[2]) { + case 1: + f = (struct follow_up_info_tlv *) org; + f->cumulativeScaledRateOffset = htonl(f->cumulativeScaledRateOffset); + f->gmTimeBaseIndicator = htons(f->gmTimeBaseIndicator); + scaled_ns_h2n(&f->lastGmPhaseChange); + f->scaledLastGmPhaseChange = htonl(f->scaledLastGmPhaseChange); + break; + } + } +} + void tlv_post_recv(struct TLV *tlv) { struct management_tlv *mgt; @@ -37,6 +94,8 @@ void tlv_post_recv(struct TLV *tlv) mes->id = ntohs(mes->id); break; case TLV_ORGANIZATION_EXTENSION: + org_post_recv((struct organization_tlv *) tlv); + break; case TLV_REQUEST_UNICAST_TRANSMISSION: case TLV_GRANT_UNICAST_TRANSMISSION: case TLV_CANCEL_UNICAST_TRANSMISSION: @@ -74,6 +133,8 @@ void tlv_pre_send(struct TLV *tlv) mes->id = htons(mes->id); break; case TLV_ORGANIZATION_EXTENSION: + org_pre_send((struct organization_tlv *) tlv); + break; case TLV_REQUEST_UNICAST_TRANSMISSION: case TLV_GRANT_UNICAST_TRANSMISSION: case TLV_CANCEL_UNICAST_TRANSMISSION: diff --git a/tlv.h b/tlv.h index b6088d2..375b821 100644 --- a/tlv.h +++ b/tlv.h @@ -123,6 +123,17 @@ struct management_error_status { Octet data[0]; } PACKED; +/* Organizationally Unique Identifiers */ +#define IEEE_802_1_COMMITTEE 0x00, 0x80, 0xC2 +extern uint8_t ieee8021_id[3]; + +struct organization_tlv { + Enumeration16 type; + UInteger16 length; + Octet id[3]; + Octet subtype[3]; +} PACKED; + #define PATH_TRACE_MAX \ ((sizeof(struct message_data) - sizeof(struct announce_msg) - sizeof(struct TLV)) / \ sizeof(struct ClockIdentity)) @@ -138,6 +149,23 @@ static inline unsigned int path_length(struct path_trace_tlv *p) return p->length / sizeof(struct ClockIdentity); } +typedef struct Integer96 { + uint16_t nanoseconds_msb; + uint64_t nanoseconds_lsb; + uint16_t fractional_nanoseconds; +} PACKED ScaledNs; + +struct follow_up_info_tlv { + Enumeration16 type; + UInteger16 length; + Octet id[3]; + Octet subtype[3]; + UInteger32 cumulativeScaledRateOffset; + UInteger16 gmTimeBaseIndicator; + ScaledNs lastGmPhaseChange; + Integer32 scaledLastGmPhaseChange; +} PACKED; + /** * Converts recognized value sub-fields into host byte order. * @param tlv Pointer to a Type Length Value field.