This patch adds support for packing and unpacking the NSM TLVs. In
addition, it introduces macros to make the ntoh/htoh boilerplate easier
to read. The idea is to reduce the number of monstrous muti-line
assignments like:
pds->grandmasterClockQuality.offsetScaledLogVariance =
htons(pds->grandmasterClockQuality.offsetScaledLogVariance);
Signed-off-by: Richard Cochran <richardcoch...@gmail.com>
---
tlv.c | 138 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tlv.h | 18 +++++++++
2 files changed, 156 insertions(+)
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;
};
};
--
2.11.0
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
