#include <stdio.h>
#include <stdarg.h>

#include <netlink/route/qdisc.h>
#include <netlink/route/class.h>
#include <netlink/route/link.h>
#include <netlink/netlink-kernel.h>

#include <linux/pkt_cls.h>          /* TC_H_ROOT definition */

#define INTF "eth0"
#define PRIOMAP_SATEM {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define BANDS_SATEM 2

enum KIND {PRIO,HTB,NETEM,BFIFO};


void configure_htb_qdisc(struct rtnl_qdisc *qdisc,
			 int def)
{
  rtnl_qdisc_set_kind(qdisc, "htb");
  rtnl_htb_set_rate2quantum(qdisc, 10);
  rtnl_htb_set_defcls(qdisc, def);
/*   rtnl_htb_set_prio(qdisc, prio); */
}


void configure_netem_qdisc(struct rtnl_qdisc *qdisc, int delay,
			   int jitter, int loss)
{
  rtnl_qdisc_set_kind(qdisc, "netem");
  if (delay){
    fprintf(stderr, "Setting delay = %dms...", delay, nl_geterror());
    if (rtnl_netem_set_delay(qdisc, delay)<0)
      fprintf(stderr, "%s\n", nl_geterror());
    else
      fprintf(stderr, "%s", nl_geterror());
  }
  if (jitter){
    fprintf(stderr, "Setting jitter = %dms...", jitter, nl_geterror());
    if (rtnl_netem_set_jitter(qdisc, jitter)<0)
      fprintf(stderr, "%s\n", nl_geterror());
    else
      fprintf(stderr, "%s", nl_geterror());
  }
  if (loss){
    fprintf(stderr, "Setting loss = %d...", loss, nl_geterror());
    if (rtnl_netem_set_loss(qdisc, loss)<0)
      fprintf(stderr, "%s\n", nl_geterror());
    else
      fprintf(stderr, "%s", nl_geterror());
  }
}


int configure_prio_qdisc(struct rtnl_qdisc *qdisc, int bands)
{
  uint8_t map[] = PRIOMAP_SATEM;
  rtnl_qdisc_set_kind(qdisc, "prio");

  fprintf(stderr, "Setting %d bands...", bands, nl_geterror());
  if (rtnl_qdisc_prio_set_bands(qdisc, bands)<0){
    fprintf(stderr, "%s\n", nl_geterror());
    return -1;
  }
  fprintf(stderr, "%s", nl_geterror());
  
  fprintf(stderr, "Setting priomap...", nl_geterror());
  if (rtnl_qdisc_prio_set_priomap(qdisc, map, sizeof(map))<0){
    fprintf(stderr, "%s\n", nl_geterror());
    return -1;
  }
  fprintf(stderr, "%s", nl_geterror());
  return 0;
}


void configure_bfifo_qdisc(struct rtnl_qdisc *qdisc, int buffer)
{
  rtnl_qdisc_set_kind(qdisc, "bfifo");
  rtnl_qdisc_fifo_set_limit(qdisc, buffer);
}


struct rtnl_qdisc *add_qdisc(struct nl_handle *nl_handle, int ifindex, uint32_t handle,
			     uint32_t parent, int kind, ...)
{
  int def, prio, delay, jitter, loss, bands, buffer;
  va_list arg_list;
  va_start(arg_list, kind);

  struct rtnl_qdisc *qdisc = rtnl_qdisc_alloc();

  /* Specify the device the qdisc should be attached to
     and the parent and handler */
  rtnl_qdisc_set_ifindex(qdisc, ifindex);
  rtnl_qdisc_set_parent(qdisc, parent);
  rtnl_qdisc_set_handle(qdisc, handle);

  switch(kind)
    {
    case HTB:
      def = va_arg(arg_list, int);
      configure_htb_qdisc(qdisc, def);
      break;
    case NETEM:
      delay = va_arg(arg_list, int);
      jitter = va_arg(arg_list, int);
      loss = va_arg(arg_list, int);
      configure_netem_qdisc(qdisc, delay, jitter, loss);
      break;
    case PRIO:
      bands = va_arg(arg_list, int);
      configure_prio_qdisc(qdisc, bands);
      break;
    case BFIFO:
      buffer = va_arg(arg_list, int);
      configure_bfifo_qdisc(qdisc, buffer);
      break;
    default:
      fprintf(stderr, "No such kind\n");      
      goto err_free_qdisc;
    }
  va_end(arg_list);
  fprintf(stderr, "Creating Qdisc... ");  
  if (rtnl_qdisc_add(nl_handle, qdisc, NLM_F_REPLACE)){
    fprintf(stderr, "%s\n", nl_geterror());
    goto err_free_qdisc;
  }
  fprintf(stderr, "%s", nl_geterror());

  return qdisc;

 err_free_qdisc:
  rtnl_qdisc_put(qdisc);
  return NULL;
}


int main(main){
/*  uint32_t def; */
/*  uint32_t parent; */
  uint32_t handle;
  struct rtnl_qdisc *qdisc;
/*  struct rtnl_class *class; */

  struct nl_handle *nl_handle = nl_handle_alloc();
  nl_connect(nl_handle, NETLINK_ROUTE);
  
  struct nl_cache *cache = rtnl_link_alloc_cache(nl_handle);
  
  int ifindex = rtnl_link_name2i(cache, INTF);
  if (ifindex<0){
    fprintf(stderr, "No such interface: %s\n", INTF);
    return -1;
  }  

  /*************************
   * DEFINE THE ROOT QDISC
   *************************/
  fprintf(stderr, "1: NETEM\n", nl_geterror());
  if (rtnl_tc_str2handle("1:", &handle)){
    fprintf(stderr, "Bad qdisc handle: %s\n", nl_geterror());
    return -1;
  }
  qdisc = add_qdisc(nl_handle, ifindex, handle, TC_H_ROOT, NETEM, 10, 0, 0);
  if (!qdisc)
    return -1;

  return 0;
}