Signed-off-by: Richard Cochran <[email protected]>
---
tlv.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
tlv.h | 28 ++++++++++++++++++---
2 files changed, 101 insertions(+), 5 deletions(-)
diff --git a/tlv.c b/tlv.c
index 0cf6d5c..e12e5ae 100644
--- a/tlv.c
+++ b/tlv.c
@@ -53,6 +53,20 @@ static void scaled_ns_h2n(ScaledNs *sns)
sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds);
}
+static void timestamp_host2net(struct Timestamp *t)
+{
+ HTONL(t->seconds_lsb);
+ HTONS(t->seconds_msb);
+ HTONL(t->nanoseconds);
+}
+
+static void timestamp_net2host(struct Timestamp *t)
+{
+ NTOHL(t->seconds_lsb);
+ NTOHS(t->seconds_msb);
+ NTOHL(t->nanoseconds);
+}
+
static uint16_t flip16(uint16_t *p)
{
uint16_t v;
@@ -80,11 +94,16 @@ static int64_t net2host64_unaligned(int64_t *p)
return v;
}
+static size_t tlv_array_count(struct TLV *tlv, size_t base_size, size_t
item_size)
+{
+ return (tlv->length - base_size) / item_size;
+}
+
static bool tlv_array_invalid(struct TLV *tlv, size_t base_size, size_t
item_size)
{
size_t expected_length, n_items;
- n_items = (tlv->length - base_size) / item_size;
+ n_items = tlv_array_count(tlv, base_size, item_size);
expected_length = base_size + n_items * item_size;
@@ -575,6 +594,57 @@ static void org_pre_send(struct organization_tlv *org)
}
}
+static int slave_rx_sync_timing_data_post_revc(struct tlv_extra *extra)
+{
+ struct slave_rx_sync_timing_data_tlv *slave_data =
+ (struct slave_rx_sync_timing_data_tlv *) extra->tlv;
+ size_t base_size = sizeof(slave_data->sourcePortIdentity), n_items;
+ struct slave_rx_sync_timing_record *record;
+
+ if (tlv_array_invalid(extra->tlv, base_size, sizeof(*record))) {
+ return -EBADMSG;
+ }
+ n_items = tlv_array_count(extra->tlv, base_size, sizeof(*record));
+ record = slave_data->record;
+
+ NTOHS(slave_data->sourcePortIdentity.portNumber);
+
+ while (n_items) {
+ NTOHS(record->sequenceId);
+ timestamp_net2host(&record->syncOriginTimestamp);
+ net2host64_unaligned(&record->totalCorrectionField);
+ NTOHL(record->scaledCumulativeRateOffset);
+ timestamp_net2host(&record->syncEventIngressTimestamp);
+ n_items--;
+ record++;
+ }
+
+ return 0;
+}
+
+static void slave_rx_sync_timing_data_pre_send(struct tlv_extra *extra)
+{
+ struct slave_rx_sync_timing_data_tlv *slave_data =
+ (struct slave_rx_sync_timing_data_tlv *) extra->tlv;
+ size_t base_size = sizeof(slave_data->sourcePortIdentity), n_items;
+ struct slave_rx_sync_timing_record *record;
+
+ n_items = tlv_array_count(extra->tlv, base_size, sizeof(*record));
+ record = slave_data->record;
+
+ HTONS(slave_data->sourcePortIdentity.portNumber);
+
+ while (n_items) {
+ HTONS(record->sequenceId);
+ timestamp_host2net(&record->syncOriginTimestamp);
+ host2net64_unaligned(&record->totalCorrectionField);
+ HTONL(record->scaledCumulativeRateOffset);
+ timestamp_host2net(&record->syncEventIngressTimestamp);
+ n_items--;
+ record++;
+ }
+}
+
static int unicast_message_type_valid(uint8_t message_type)
{
message_type >>= 4;
@@ -743,7 +813,10 @@ int tlv_post_recv(struct tlv_extra *extra)
case TLV_L1_SYNC:
case TLV_PORT_COMMUNICATION_AVAILABILITY:
case TLV_PROTOCOL_ADDRESS:
+ break;
case TLV_SLAVE_RX_SYNC_TIMING_DATA:
+ result = slave_rx_sync_timing_data_post_revc(extra);
+ break;
case TLV_SLAVE_RX_SYNC_COMPUTED_DATA:
case TLV_SLAVE_TX_EVENT_TIMESTAMPS:
case TLV_CUMULATIVE_RATE_RATIO:
@@ -800,7 +873,10 @@ void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra)
case TLV_L1_SYNC:
case TLV_PORT_COMMUNICATION_AVAILABILITY:
case TLV_PROTOCOL_ADDRESS:
+ break;
case TLV_SLAVE_RX_SYNC_TIMING_DATA:
+ slave_rx_sync_timing_data_pre_send(extra);
+ break;
case TLV_SLAVE_RX_SYNC_COMPUTED_DATA:
case TLV_SLAVE_TX_EVENT_TIMESTAMPS:
case TLV_CUMULATIVE_RATE_RATIO:
diff --git a/tlv.h b/tlv.h
index f46a3b8..8212325 100644
--- a/tlv.h
+++ b/tlv.h
@@ -224,6 +224,11 @@ struct path_trace_tlv {
struct ClockIdentity cid[0];
} PACKED;
+static inline unsigned int path_length(struct path_trace_tlv *p)
+{
+ return p->length / sizeof(struct ClockIdentity);
+}
+
struct request_unicast_xmit_tlv {
Enumeration16 type;
UInteger16 length;
@@ -232,10 +237,25 @@ struct request_unicast_xmit_tlv {
UInteger32 durationField;
} PACKED;
-static inline unsigned int path_length(struct path_trace_tlv *p)
-{
- return p->length / sizeof(struct ClockIdentity);
-}
+struct slave_rx_sync_timing_record {
+ UInteger16 sequenceId;
+ struct Timestamp syncOriginTimestamp;
+ TimeInterval totalCorrectionField;
+ Integer32 scaledCumulativeRateOffset;
+ struct Timestamp syncEventIngressTimestamp;
+} PACKED;
+
+struct slave_rx_sync_timing_data_tlv {
+ Enumeration16 type;
+ UInteger16 length;
+ struct PortIdentity sourcePortIdentity;
+ struct slave_rx_sync_timing_record record[0];
+} PACKED;
+
+#define SLAVE_RX_SYNC_TIMING_MAX \
+ ((sizeof(struct message_data) - sizeof(struct signaling_msg) - \
+ sizeof(struct slave_rx_sync_timing_data_tlv)) / \
+ sizeof(struct slave_rx_sync_timing_record))
typedef struct Integer96 {
uint16_t nanoseconds_msb;
--
2.20.1
_______________________________________________
Linuxptp-devel mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
