diff options
Diffstat (limited to 'daemons/gptp/linux/src/linux_hal_generic.cpp')
| -rw-r--r-- | daemons/gptp/linux/src/linux_hal_generic.cpp | 21 |
1 files changed, 18 insertions, 3 deletions
diff --git a/daemons/gptp/linux/src/linux_hal_generic.cpp b/daemons/gptp/linux/src/linux_hal_generic.cpp index 5364fa6a..9eeba46f 100644 --- a/daemons/gptp/linux/src/linux_hal_generic.cpp +++ b/daemons/gptp/linux/src/linux_hal_generic.cpp @@ -30,9 +30,10 @@ POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ - #include <linux_hal_generic.hpp> #include <linux_hal_generic_tsprivate.hpp> +#include <platform.hpp> +#include <avbts_message.hpp> #include <sys/select.h> #include <sys/socket.h> #include <netpacket/packet.h> @@ -285,6 +286,10 @@ int LinuxTimestamperGeneric::HWTimestamper_txtimestamp struct cmsghdr *cmsg; struct sockaddr_ll remote; struct iovec sgentry; + PTPMessageId reflectedMessageId; + uint8_t reflected_bytes[ETHER_HDR_LEN + PTP_COMMON_HDR_LENGTH]; + uint8_t *gptpCommonHeader; + uint16_t sequenceId; struct { struct cmsghdr cm; char control[256]; @@ -296,8 +301,10 @@ int LinuxTimestamperGeneric::HWTimestamper_txtimestamp msg.msg_iov = &sgentry; msg.msg_iovlen = 1; - sgentry.iov_base = NULL; - sgentry.iov_len = 0; + sgentry.iov_base = reflected_bytes; + sgentry.iov_len = sizeof(reflected_bytes); + + gptpCommonHeader = reflected_bytes + ETHER_HDR_LEN; memset( &remote, 0, sizeof(remote)); msg.msg_name = (caddr_t) &remote; @@ -316,6 +323,14 @@ int LinuxTimestamperGeneric::HWTimestamper_txtimestamp goto done; } } + sequenceId = PLAT_ntohs(*((uint16_t*)(PTP_COMMON_HDR_SEQUENCE_ID(gptpCommonHeader)))); + reflectedMessageId.setSequenceId(sequenceId); + reflectedMessageId.setMessageType((MessageType)(*PTP_COMMON_HDR_TRANSSPEC_MSGTYPE(gptpCommonHeader) & 0xF)); + if (messageId != reflectedMessageId) { + GPTP_LOG_WARNING("Timestamp discarded due to wrong message id"); + ret = GPTP_EC_EAGAIN; + goto done; + } // Retrieve the timestamp cmsg = CMSG_FIRSTHDR(&msg); |