#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