Organize host and robot streaming releases

robot/ros2/OmniSocketGo_robot_ros/src/linux_timestamping.c

@@ -0,0 +1,103 @@
+#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(&timestamps->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(&timestamps->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(&timestamps->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