diff --git a/tlv.c b/tlv.c index 4811f19..7285af7 100644 --- a/tlv.c +++ b/tlv.c @@ -25,6 +25,11 @@ #include "tlv.h" #include "msg.h" +#define HTONS(x) (x) = htons(x) +#define HTONL(x) (x) = htonl(x) +#define NTOHS(x) (x) = ntohs(x) +#define NTOHL(x) (x) = ntohl(x) + #define TLV_LENGTH_INVALID(tlv, type) \ (tlv->length < sizeof(struct type) - sizeof(struct TLV)) @@ -56,6 +61,24 @@ static uint16_t flip16(uint16_t *p) return v; } +static int64_t host2net64_unaligned(int64_t *p) +{ + int64_t v; + memcpy(&v, p, sizeof(v)); + v = host2net64(v); + memcpy(p, &v, sizeof(v)); + return v; +} + +static int64_t net2host64_unaligned(int64_t *p) +{ + int64_t v; + memcpy(&v, p, sizeof(v)); + v = net2host64(v); + memcpy(p, &v, sizeof(v)); + return v; +} + static int mgt_post_recv(struct management_tlv *m, uint16_t data_len, struct tlv_extra *extra) { @@ -371,6 +394,111 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra) } } +static int nsm_resp_post_recv(struct tlv_extra *extra) +{ + struct nsm_resp_tlv_head *head; + struct TLV *tlv = extra->tlv; + struct timePropertiesDS *tp; + struct PortAddress *paddr; + struct currentDS *cds; + struct parentDS *pds; + unsigned char *ptr; + uint16_t expected; + + if (tlv->length < sizeof(*head) + sizeof(*extra->foot) + - sizeof(head->type) - sizeof(head->length)) { + return -EBADMSG; + } + head = (struct nsm_resp_tlv_head *) tlv; + paddr = &head->parent_addr; + NTOHS(paddr->networkProtocol); + NTOHS(paddr->addressLength); + + switch (paddr->networkProtocol) { + case TRANS_UDP_IPV4: + expected = 4; + break; + case TRANS_UDP_IPV6: + expected = 16; + break; + case TRANS_IEEE_802_3: + expected = 6; + break; + default: + return -EBADMSG; + } + if (paddr->addressLength != expected) { + return -EBADMSG; + } + if (tlv->length != sizeof(*head) + sizeof(*extra->foot) + + paddr->addressLength - sizeof(head->type) - sizeof(head->length)) { + return -EBADMSG; + } + + ptr = (unsigned char *) tlv; + ptr += sizeof(*head) + paddr->addressLength; + extra->foot = (struct nsm_resp_tlv_foot *) ptr; + + pds = &extra->foot->parent; + cds = &extra->foot->current; + tp = &extra->foot->timeprop; + + /* + * At this point the alignment only 2 bytes worst case. + * So we need to be careful with the 64 bit words. + */ + NTOHS(pds->parentPortIdentity.portNumber); + NTOHS(pds->observedParentOffsetScaledLogVariance); + NTOHL(pds->observedParentClockPhaseChangeRate); + NTOHS(pds->grandmasterClockQuality.offsetScaledLogVariance); + + NTOHS(cds->stepsRemoved); + net2host64_unaligned(&cds->offsetFromMaster); + net2host64_unaligned(&cds->meanPathDelay); + + NTOHS(tp->currentUtcOffset); + + NTOHL(extra->foot->lastsync.seconds_lsb); + NTOHS(extra->foot->lastsync.seconds_msb); + NTOHL(extra->foot->lastsync.nanoseconds); + + return 0; +} + +static void nsm_resp_pre_send(struct tlv_extra *extra) +{ + struct nsm_resp_tlv_head *head; + struct timePropertiesDS *tp; + struct PortAddress *paddr; + struct currentDS *cds; + struct parentDS *pds; + + head = (struct nsm_resp_tlv_head *) extra->tlv; + paddr = &head->parent_addr; + + pds = &extra->foot->parent; + cds = &extra->foot->current; + tp = &extra->foot->timeprop; + + NTOHS(paddr->networkProtocol); + NTOHS(paddr->addressLength); + + HTONS(pds->parentPortIdentity.portNumber); + HTONS(pds->observedParentOffsetScaledLogVariance); + HTONL(pds->observedParentClockPhaseChangeRate); + HTONS(pds->grandmasterClockQuality.offsetScaledLogVariance); + + HTONS(cds->stepsRemoved); + host2net64_unaligned(&cds->offsetFromMaster); + host2net64_unaligned(&cds->meanPathDelay); + + HTONS(tp->currentUtcOffset); + + HTONL(extra->foot->lastsync.seconds_lsb); + HTONS(extra->foot->lastsync.seconds_msb); + HTONL(extra->foot->lastsync.nanoseconds); +} + static int org_post_recv(struct organization_tlv *org) { struct follow_up_info_tlv *f; @@ -489,6 +617,11 @@ int tlv_post_recv(struct tlv_extra *extra) case TLV_AUTHENTICATION_CHALLENGE: case TLV_SECURITY_ASSOCIATION_UPDATE: case TLV_CUM_FREQ_SCALE_FACTOR_OFFSET: + case TLV_PTPMON_REQ: + break; + case TLV_PTPMON_RESP: + result = nsm_resp_post_recv(extra); + break; default: break; } @@ -527,6 +660,11 @@ void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra) case TLV_AUTHENTICATION_CHALLENGE: case TLV_SECURITY_ASSOCIATION_UPDATE: case TLV_CUM_FREQ_SCALE_FACTOR_OFFSET: + case TLV_PTPMON_REQ: + break; + case TLV_PTPMON_RESP: + nsm_resp_pre_send(extra); + break; default: break; } diff --git a/tlv.h b/tlv.h index 5a919fb..4ec9173 100644 --- a/tlv.h +++ b/tlv.h @@ -39,6 +39,8 @@ #define TLV_AUTHENTICATION_CHALLENGE 0x2001 #define TLV_SECURITY_ASSOCIATION_UPDATE 0x2002 #define TLV_CUM_FREQ_SCALE_FACTOR_OFFSET 0x2003 +#define TLV_PTPMON_REQ 0x21FE +#define TLV_PTPMON_RESP 0x21FF enum management_action { GET, @@ -134,6 +136,21 @@ struct management_error_status { Octet data[0]; } PACKED; +struct nsm_resp_tlv_head { + Enumeration16 type; + UInteger16 length; + uint8_t port_state; + uint8_t reserved; + struct PortAddress parent_addr; +} PACKED; + +struct nsm_resp_tlv_foot { + struct parentDS parent; + struct currentDS current; + struct timePropertiesDS timeprop; + struct Timestamp lastsync; +} PACKED; + /* Organizationally Unique Identifiers */ #define IEEE_802_1_COMMITTEE 0x00, 0x80, 0xC2 extern uint8_t ieee8021_id[3]; @@ -234,6 +251,7 @@ struct tlv_extra { struct TLV *tlv; union { struct mgmt_clock_description cd; + struct nsm_resp_tlv_foot *foot; }; };