// https://syzkaller.appspot.com/bug?id=b4ff8c432c0250e563e3404a2448ce2e2ae70370
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <linux/pkt_sched.h>
#include <time.h>
#include <errno.h>
#include <stdint.h>
#include <net/if.h>

#ifndef CLOCK_TAI
#define CLOCK_TAI 11
#endif

#ifndef TC_TAPRIO_CMD_SET_GATES
#define TC_TAPRIO_CMD_SET_GATES 0
#endif

#ifndef TCA_TAPRIO_ATTR_PRIOMAP
#define TCA_TAPRIO_ATTR_PRIOMAP 1
#endif
#ifndef TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST
#define TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST 2
#endif
#ifndef TCA_TAPRIO_ATTR_SCHED_BASE_TIME
#define TCA_TAPRIO_ATTR_SCHED_BASE_TIME 3
#endif
#ifndef TCA_TAPRIO_ATTR_SCHED_CLOCKID
#define TCA_TAPRIO_ATTR_SCHED_CLOCKID 5
#endif

#ifndef TCA_TAPRIO_SCHED_ENTRY
#define TCA_TAPRIO_SCHED_ENTRY 1
#endif

#ifndef TCA_TAPRIO_SCHED_ENTRY_INDEX
#define TCA_TAPRIO_SCHED_ENTRY_INDEX 1
#endif
#ifndef TCA_TAPRIO_SCHED_ENTRY_CMD
#define TCA_TAPRIO_SCHED_ENTRY_CMD 2
#endif
#ifndef TCA_TAPRIO_SCHED_ENTRY_GATE_MASK
#define TCA_TAPRIO_SCHED_ENTRY_GATE_MASK 3
#endif
#ifndef TCA_TAPRIO_SCHED_ENTRY_INTERVAL
#define TCA_TAPRIO_SCHED_ENTRY_INTERVAL 4
#endif

#ifndef IFLA_LINKINFO
#define IFLA_LINKINFO 18
#endif
#ifndef IFLA_INFO_KIND
#define IFLA_INFO_KIND 1
#endif
#ifndef IFLA_NUM_TX_QUEUES
#define IFLA_NUM_TX_QUEUES 31
#endif

/* Manually define the struct to guarantee the exact 82-byte layout the kernel expects */
struct my_tc_mqprio_qopt {
    uint8_t  num_tc;
    uint8_t  prio_tc_map[16];
    uint8_t  hw;
    uint16_t count[16];
    uint16_t offset[16];
} __attribute__((packed));

struct qdisc_req {
    struct nlmsghdr n;
    struct tcmsg t;
    char buf[2048];
};

static void add_attr(struct nlmsghdr *n, int maxlen, int type, const void *data, int alen) {
    int len = RTA_LENGTH(alen);
    struct rtattr *rta;

    if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
        return;
    }
    rta = (struct rtattr *)(((char *)n) + NLMSG_ALIGN(n->nlmsg_len));
    rta->rta_type = type;
    rta->rta_len = len;
    if (alen > 0 && data) {
        memcpy(RTA_DATA(rta), data, alen);
    }
    n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
}

static struct rtattr *nest_start(struct nlmsghdr *n, int maxlen, int type) {
    struct rtattr *nest = (struct rtattr *)(((char *)n) + NLMSG_ALIGN(n->nlmsg_len));
    add_attr(n, maxlen, type, NULL, 0);
    return nest;
}

static void nest_end(struct nlmsghdr *n, struct rtattr *nest) {
    nest->rta_len = (char *)n + n->nlmsg_len - (char *)nest;
}

int create_dummy_interface(int fd, const char *name) {
    struct {
        struct nlmsghdr n;
        struct ifinfomsg i;
        char buf[1024];
    } req;

    memset(&req, 0, sizeof(req));
    req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
    req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL | NLM_F_ACK;
    req.n.nlmsg_type = RTM_NEWLINK;
    req.i.ifi_family = AF_UNSPEC;

    add_attr(&req.n, sizeof(req), IFLA_IFNAME, name, strlen(name) + 1);

    struct rtattr *linkinfo = nest_start(&req.n, sizeof(req), IFLA_LINKINFO);
    add_attr(&req.n, sizeof(req), IFLA_INFO_KIND, "dummy", 6);
    nest_end(&req.n, linkinfo);

    int numtxqueues = 4;
    add_attr(&req.n, sizeof(req), IFLA_NUM_TX_QUEUES, &numtxqueues, sizeof(numtxqueues));

    struct sockaddr_nl nladdr;
    memset(&nladdr, 0, sizeof(nladdr));
    nladdr.nl_family = AF_NETLINK;

    if (sendto(fd, &req, req.n.nlmsg_len, 0, (struct sockaddr *)&nladdr, sizeof(nladdr)) < 0) {
        return -1;
    }

    char recvbuf[4096];
    int len = recv(fd, recvbuf, sizeof(recvbuf), 0);
    if (len < 0) return -1;

    struct nlmsghdr *h = (struct nlmsghdr *)recvbuf;
    if (h->nlmsg_type == NLMSG_ERROR) {
        struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h);
        if (err->error != 0) {
            errno = -err->error;
            return -1;
        }
    }
    return 0;
}

int set_interface_up(int fd, int ifindex) {
    struct {
        struct nlmsghdr n;
        struct ifinfomsg i;
        char buf[1024];
    } req;

    memset(&req, 0, sizeof(req));
    req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
    req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
    req.n.nlmsg_type = RTM_NEWLINK;
    req.i.ifi_family = AF_UNSPEC;
    req.i.ifi_index = ifindex;
    req.i.ifi_flags = IFF_UP;
    req.i.ifi_change = IFF_UP;

    struct sockaddr_nl nladdr;
    memset(&nladdr, 0, sizeof(nladdr));
    nladdr.nl_family = AF_NETLINK;

    if (sendto(fd, &req, req.n.nlmsg_len, 0, (struct sockaddr *)&nladdr, sizeof(nladdr)) < 0) {
        return -1;
    }

    char recvbuf[4096];
    int len = recv(fd, recvbuf, sizeof(recvbuf), 0);
    if (len < 0) return -1;

    struct nlmsghdr *h = (struct nlmsghdr *)recvbuf;
    if (h->nlmsg_type == NLMSG_ERROR) {
        struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h);
        if (err->error != 0) {
            errno = -err->error;
            return -1;
        }
    }
    return 0;
}

int add_taprio_qdisc(int fd, int ifindex) {
    struct qdisc_req req;
    memset(&req, 0, sizeof(req));

    req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct tcmsg));
    req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_CREATE | NLM_F_EXCL;
    req.n.nlmsg_type = RTM_NEWQDISC;
    req.n.nlmsg_seq = 1;
    req.n.nlmsg_pid = 0;

    req.t.tcm_family = AF_UNSPEC;
    req.t.tcm_ifindex = ifindex;
    req.t.tcm_handle = TC_H_MAKE(0x00010000U, 0); /* handle 1:0 */
    req.t.tcm_parent = TC_H_ROOT;                 /* root parent */

    add_attr(&req.n, sizeof(req), TCA_KIND, "taprio", strlen("taprio") + 1);

    struct rtattr *options = nest_start(&req.n, sizeof(req), TCA_OPTIONS);

    struct my_tc_mqprio_qopt opt;
    memset(&opt, 0, sizeof(opt));
    opt.num_tc = 3;
    
    opt.prio_tc_map[0] = 0;
    opt.prio_tc_map[1] = 1;
    for (int i = 2; i < 16; i++) {
        opt.prio_tc_map[i] = 2;
    }
    
    opt.count[0] = 1;
    opt.offset[0] = 0;
    opt.count[1] = 1;
    opt.offset[1] = 1;
    opt.count[2] = 1;
    opt.offset[2] = 2;

    add_attr(&req.n, sizeof(req), TCA_TAPRIO_ATTR_PRIOMAP, &opt, sizeof(opt));

    int64_t base_time = 0;
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_ATTR_SCHED_BASE_TIME, &base_time, sizeof(base_time));

    int32_t clockid = CLOCK_TAI;
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_ATTR_SCHED_CLOCKID, &clockid, sizeof(clockid));

    struct rtattr *entry_list = nest_start(&req.n, sizeof(req), TCA_TAPRIO_ATTR_SCHED_ENTRY_LIST);

    struct rtattr *entry0 = nest_start(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY);
    uint32_t index0 = 1;
    uint8_t cmd0 = TC_TAPRIO_CMD_SET_GATES;
    uint32_t gate_mask0 = 1;
    uint32_t interval0 = 50000; // 50 us (must be >= 48us for 10Mbps dummy interface)
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_INDEX, &index0, sizeof(index0));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_CMD, &cmd0, sizeof(cmd0));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_GATE_MASK, &gate_mask0, sizeof(gate_mask0));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_INTERVAL, &interval0, sizeof(interval0));
    nest_end(&req.n, entry0);

    struct rtattr *entry1 = nest_start(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY);
    uint32_t index1 = 2;
    uint8_t cmd1 = TC_TAPRIO_CMD_SET_GATES;
    uint32_t gate_mask1 = 2;
    uint32_t interval1 = 50000; // 50 us
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_INDEX, &index1, sizeof(index1));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_CMD, &cmd1, sizeof(cmd1));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_GATE_MASK, &gate_mask1, sizeof(gate_mask1));
    add_attr(&req.n, sizeof(req), TCA_TAPRIO_SCHED_ENTRY_INTERVAL, &interval1, sizeof(interval1));
    nest_end(&req.n, entry1);

    nest_end(&req.n, entry_list);
    nest_end(&req.n, options);

    struct sockaddr_nl daddr;
    memset(&daddr, 0, sizeof(daddr));
    daddr.nl_family = AF_NETLINK;

    struct iovec iov;
    iov.iov_base = &req;
    iov.iov_len = req.n.nlmsg_len;

    struct msghdr msg;
    memset(&msg, 0, sizeof(msg));
    msg.msg_name = &daddr;
    msg.msg_namelen = sizeof(daddr);
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;

    if (sendmsg(fd, &msg, 0) < 0) {
        return -errno;
    }

    char reply_buf[4096];
    struct iovec reply_iov;
    reply_iov.iov_base = reply_buf;
    reply_iov.iov_len = sizeof(reply_buf);

    struct msghdr reply_msg;
    memset(&reply_msg, 0, sizeof(reply_msg));
    reply_msg.msg_name = &daddr;
    reply_msg.msg_namelen = sizeof(daddr);
    reply_msg.msg_iov = &reply_iov;
    reply_msg.msg_iovlen = 1;

    ssize_t len = recvmsg(fd, &reply_msg, 0);
    if (len < 0) {
        return -errno;
    }

    struct nlmsghdr *nh = (struct nlmsghdr *)reply_buf;
    while (NLMSG_OK(nh, len)) {
        if (nh->nlmsg_type == NLMSG_ERROR) {
            struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nh);
            if (nh->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
                return -EIO;
            }
            if (err->error != 0) {
                return err->error;
            }
        }
        nh = NLMSG_NEXT(nh, len);
    }

    return 0;
}

int main(void)
{
    int fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
    if (fd < 0) {
        printf("[-] Failed to create NETLINK_ROUTE socket: %s\n", strerror(errno));
        exit(1);
    }
    printf("[+] NETLINK_ROUTE socket created.\n");

    char ifname[32];
    snprintf(ifname, sizeof(ifname), "dummy_%d", getpid());

    if (create_dummy_interface(fd, ifname) < 0) {
        printf("[-] Failed to create dummy interface %s: %s\n", ifname, strerror(errno));
        exit(1);
    }
    printf("[+] Dummy interface %s created.\n", ifname);

    int ifindex = if_nametoindex(ifname);
    if (ifindex == 0) {
        printf("[-] Failed to get ifindex for %s: %s\n", ifname, strerror(errno));
        exit(1);
    }
    printf("[+] Got ifindex %d for %s.\n", ifindex, ifname);

    if (set_interface_up(fd, ifindex) < 0) {
        printf("[-] Failed to set %s UP: %s\n", ifname, strerror(errno));
        exit(1);
    }
    printf("[+] %s is UP.\n", ifname);

    int ret = add_taprio_qdisc(fd, ifindex);
    if (ret != 0) {
        printf("[-] Failed to configure taprio qdisc: %s\n", strerror(-ret));
        exit(1);
    }
    printf("[+] Successfully configured taprio qdisc.\n");

    struct timespec ts;
    if (clock_gettime(CLOCK_REALTIME, &ts) == 0) {
        ts.tv_sec += 10000;
        if (clock_settime(CLOCK_REALTIME, &ts) != 0) {
            printf("[-] clock_settime failed: %s\n", strerror(errno));
            exit(1);
        } else {
            printf("[+] Clock jumped forward by 10000 seconds.\n");
        }
    } else {
        printf("[-] clock_gettime failed: %s\n", strerror(errno));
        exit(1);
    }

    int sock = socket(AF_UNIX, SOCK_DGRAM, 0);
    if (sock < 0) {
        printf("[-] socket failed: %s\n", strerror(errno));
        exit(1);
    }
    
    struct mmsghdr msgs[1];
    memset(msgs, 0, sizeof(msgs));
    struct iovec iov2;
    char buf2[1];
    iov2.iov_base = buf2;
    iov2.iov_len = 1;
    msgs[0].msg_hdr.msg_iov = &iov2;
    msgs[0].msg_hdr.msg_iovlen = 1;
    struct timespec timeout = { .tv_sec = 1, .tv_nsec = 0 };
    
    printf("[*] Waiting for RCU stall in recvmmsg...\n");
    while (1) {
        recvmmsg(sock, msgs, 1, 0, &timeout);
    }

    return 0;
}