104 lines
3.5 KiB
C
104 lines
3.5 KiB
C
#include "linux_timestamping.h"
|
|
#include "latencylog.h"
|
|
|
|
#ifdef __linux__
|
|
#include <linux/errqueue.h>
|
|
#include <linux/net_tstamp.h>
|
|
#include <netinet/in.h>
|
|
#include <string.h>
|
|
|
|
static int64_t linux_timespec_to_ns(const struct timespec *ts) {
|
|
if (ts == NULL) {
|
|
return 0;
|
|
}
|
|
return (int64_t) ts->tv_sec * 1000000000LL + ts->tv_nsec;
|
|
}
|
|
|
|
int linux_timestamping_enable_udp_socket(int fd, int enable_rx) {
|
|
int flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_TX_SCHED | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_OPT_TSONLY;
|
|
if (enable_rx) {
|
|
flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
|
|
}
|
|
return setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof(flags));
|
|
}
|
|
|
|
int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) {
|
|
struct cmsghdr *cmsg;
|
|
const struct scm_timestamping *timestamps;
|
|
if (msg == NULL) {
|
|
return 0;
|
|
}
|
|
for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) {
|
|
if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) {
|
|
timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg);
|
|
if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) {
|
|
return linux_timespec_to_ns(×tamps->ts[0]);
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) {
|
|
struct cmsghdr *cmsg;
|
|
const struct scm_timestamping *timestamps = NULL;
|
|
const struct sock_extended_err *sock_err = NULL;
|
|
int64_t timestamp_ns = 0;
|
|
|
|
if (msg == NULL || out_event == NULL) {
|
|
errno = EINVAL;
|
|
return -1;
|
|
}
|
|
memset(out_event, 0, sizeof(*out_event));
|
|
|
|
for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) {
|
|
if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) {
|
|
timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg);
|
|
} else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) ||
|
|
(cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) {
|
|
sock_err = (const struct sock_extended_err *) CMSG_DATA(cmsg);
|
|
}
|
|
}
|
|
if (timestamps == NULL || sock_err == NULL) {
|
|
errno = EAGAIN;
|
|
return -1;
|
|
}
|
|
if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) {
|
|
timestamp_ns = linux_timespec_to_ns(×tamps->ts[0]);
|
|
snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SOFTWARE);
|
|
} else if (timestamps->ts[1].tv_sec != 0 || timestamps->ts[1].tv_nsec != 0) {
|
|
timestamp_ns = linux_timespec_to_ns(×tamps->ts[1]);
|
|
snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SCHED);
|
|
} else {
|
|
errno = EAGAIN;
|
|
return -1;
|
|
}
|
|
out_event->ts_unix_nano = timestamp_ns;
|
|
out_event->ee_info = sock_err->ee_info;
|
|
out_event->ee_data = sock_err->ee_data;
|
|
return 0;
|
|
}
|
|
|
|
#else
|
|
|
|
int linux_timestamping_enable_udp_socket(int fd, int enable_rx) {
|
|
(void) fd;
|
|
(void) enable_rx;
|
|
errno = ENOTSUP;
|
|
return -1;
|
|
}
|
|
|
|
int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) {
|
|
(void) msg;
|
|
return 0;
|
|
}
|
|
|
|
int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) {
|
|
(void) msg;
|
|
(void) out_event;
|
|
errno = ENOTSUP;
|
|
return -1;
|
|
}
|
|
|
|
#endif
|