#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <netdb.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <linux/mpls.h>
#include <sys/ioctl.h>
#include <linux/atm.h>
#include <asm/types.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>

#include "netlink.h"

int fd = 0;
int netlink = 0;
char verbose = 0;

unsigned int resolve_it(char *host);
int finish_out_label(struct mpls_out_label_req *mol,char **arg,int start);
int fill_label(struct mpls_label *lbl,char **args,int start);
int fill_instructions(struct mpls_instruction_req *mir, char** arg,int start, int num);

void usage() {
  fprintf(stderr,"usage: mplsadm [ADBUdhvT:f:L:I:O:i:o:]\n\n");
  fprintf(stderr,"-A add modifier\n");
  fprintf(stderr,"-B bind modifier\n");
  fprintf(stderr,"-D delete modifier\n");
  fprintf(stderr,"-U unbind modifier\n");
  fprintf(stderr,"-d toggle debug\n");
  fprintf(stderr,"-h this message\n");
  fprintf(stderr,"-v verbose info\n");
  fprintf(stderr,"-T <tunnel name>:<dest addr>\n");
  fprintf(stderr,"-f <prefix/len>\n");
  fprintf(stderr,"-L <interface name>:<label space> set the label space for"
                 " an interface (-1 disables)\n");
  fprintf(stderr,"-I <gen|atm|fr>:<label>:<label space> create|delete an"
                 " incoming label\n");
  fprintf(stderr,"-O <gen|atm|fr>:<label>:<out interface>[:<next type>:"
                 "<next hop>]\n");
  fprintf(stderr,"-i <opcode:opcode_data>+\n");
  fprintf(stderr,"-o <opcode:opcode_data>+\n");
  fprintf(stderr,"\n\n");
}

int finish_out_label(struct mpls_out_label_req *mol,char **arg,int start) {
  struct ifreq ifr;
  int result;
  int len;
  int i = 0;

  fprintf(stderr,"out: %s\n",arg[start]);
  len = strlen(arg[start]);
  i++;
  if(len > 0 && len < 10) {
    memset(&ifr,0,sizeof(struct ifreq));
    strncpy(ifr.ifr_name,arg[start],len+1);
    if((result = ioctl(fd,SIOCGIFINDEX,&ifr)) == 0) {
      mol->mol_ifindex = ifr.ifr_ifindex;
      if(arg[start+1] && !strcmp(arg[start+1],"ipv4")) {
        struct sockaddr_in sin;
        i++;

        if(verbose) fprintf(stderr,"ipv4\n");

        sin.sin_addr.s_addr = resolve_it(arg[start+2]);
        sin.sin_family = AF_INET;
        memcpy(&mol->mol_nh,&sin,sizeof(struct sockaddr));
        i++;
      }
    } else {
      fprintf(stderr,"Unable to resolve ifindex for %s\n",ifr.ifr_name);
    }
  }
  return i;
}

int fill_label(struct mpls_label *lbl,char **args,int start) {
  int result = -1;

  if(!strncmp(args[start],"atm",3)) {
    char *vpi_str,*vci_str;
    int vpi,vci;

    vpi_str = strtok(args[start+1],"/");
    vci_str = strtok(NULL,"\0");
    if(vpi_str && vci_str) {
      vpi = atoi(vpi_str);
      vci = atoi(vci_str);
      if(vpi > -1 && vpi < ATM_MAX_VPI && vci > -1 && vci < ATM_MAX_VCI) {
        lbl->ml_type = MPLS_LABEL_ATM;
        lbl->u.ml_atm.mla_vpi = vpi;
        lbl->u.ml_atm.mla_vci = vci;
      }
    }
    result = 0;
  } else if(!strncmp(args[start],"gen",3)) {
    lbl->ml_type = MPLS_LABEL_GEN;
    lbl->u.ml_gen = atol(args[start+1]);
    result = 0;
  } else if(!strncmp(args[start],"fr",2)) {
    lbl->ml_type = MPLS_LABEL_FR;
    lbl->u.ml_fr = atol(args[start+1]);
    result = 0;
  }
  return result;
}

int fill_instructions(struct mpls_instruction_req *mir, char** arg,int start,
  int num) {
  struct mpls_out_label_req mol;
  struct ifreq ifr;
  int length = 0;
  int result;
  int i;

  for(i=start;i<(num+start);i++) {
    fprintf(stderr,"%s\n",arg[i]);
    if(!strncmp("pop",arg[i],3)) {
      mir->mir_instruction[length].mir_opcode = MPLS_OP_POP;
    } else if(!strncmp("push",arg[i],4)) {
      i++;
      mir->mir_instruction[length].mir_opcode = MPLS_OP_PUSH;
      if(fill_label(&(mir->mir_instruction[length].mir_data.push.label),
        arg,i) < 0) {
        return -1;
      }
    } else if(!strncmp("dlv",arg[i],3)) {
      mir->mir_instruction[length].mir_opcode = MPLS_OP_DLV;
    } else if(!strncmp("set",arg[i],3)) {
      mir->mir_instruction[length].mir_opcode = MPLS_OP_SET;
      if(mir->mir_direction == MPLS_IN) {
        i++;
        memset(&ifr,0,sizeof(struct ifreq));
        strcpy(ifr.ifr_name,arg[i]);
        if((result = ioctl(fd,SIOCGIFINDEX,&ifr)) != 0) {
          perror("SET-SIOCGIFINDEX");
          exit(result);
        }
        mir->mir_instruction[length].mir_data.set = ifr.ifr_ifindex;
      }
    } else if(!strncmp("peek",arg[i],4)) {
      mir->mir_instruction[length].mir_opcode = MPLS_OP_PEEK;
    } else if(!strncmp("fwd",arg[i],3)) {
      mir->mir_instruction[length].mir_opcode = MPLS_OP_FWD;
      i++;
      if(fill_label(&(mir->mir_instruction[length].mir_data.fwd.label),
        arg,i) < 0) {
        return -1;
      }
      i += 2;
      i += finish_out_label(&mol,arg,i);
      mir->mir_instruction[length].mir_data.fwd.index = mol.mol_ifindex;
    }
    length++;
  }
  mir->mir_instruction_length = length;
  return 0;
}

unsigned int resolve_it(char *host) {
  unsigned int result = -1;
  struct hostent *hp;

  if(isdigit(host[0])) {
    result = inet_addr(host);
  } else {
    if((hp = gethostbyname(host))) {
      memcpy(&result,hp->h_addr,sizeof(unsigned int));
    }
  }
  return result;
}

int parse_args(int argc,char **argv,char *args) {
  int i = 1;

  argv[0] = strtok(args,":");
  while((argv[i] = strtok(NULL,":"))) {
    i++;
    if(i > (argc - 1)) {
      return -i;
    }
  }
  return i;
}

int main(int argc, char **argv) {

  struct mpls_instruction_req mir_req;
  struct mpls_labelspace_req mls_req;
  struct mpls_out_label_req mol_req;
  struct mpls_in_label_req mil_req;
  struct mpls_bind_fec_req mbf_req;
  struct mpls_xconnect_req mx_req;
  struct ifreq ifr;

  char *in_instr_str = NULL;
  char *out_instr_str = NULL;
  char *in_label_str = NULL;
  char *out_label_str = NULL;
  char *tunnel_str = NULL;
  char *label_space_str = NULL;
  char *fec_str = NULL;
  char *larg[1024];
  int result = -1;
  int num = 0;
  int opt;

  char delete = 0;
  char add = 0;
  char bind = 0;
  char unbind = 0;
  char debug = 0;

  fd = socket(AF_INET,SOCK_DGRAM,0);
  if(fd < 0) {
    perror("Socket");
    exit(fd);
  }

  netlink = rtnl_open();
  if(netlink < 0) {
    perror("Netlink");
    exit(netlink);
  }

  memset(&mil_req,0,sizeof(struct mpls_in_label_req));
  memset(&mls_req,0,sizeof(struct mpls_labelspace_req));
  memset(&mol_req,0,sizeof(struct mpls_out_label_req));
  memset(&mx_req,0,sizeof(struct mpls_xconnect_req));
  memset(&mbf_req,0,sizeof(struct mpls_bind_fec_req));
  memset(&mir_req,0,sizeof(struct mpls_instruction_req));

  while((opt = getopt(argc,argv,"ADBUdhvT:f:L:I:O:i:o:")) != EOF) {
    switch(opt) {
      case 'A':
        add = 1;
        break;
      case 'B':
        bind = 1;
        break;
      case 'D':
        delete = 1;
        break;
      case 'U':
        unbind = 1;
        break;
      case 'd':
        debug = 1;
        break;
      case 'v':
        verbose = 1;
        break;
      case 'I':
        in_label_str = optarg;
        if(verbose) fprintf(stderr,"In label input: %s\n",in_label_str);
        break;
      case 'L':
        label_space_str = optarg;
        if(verbose) fprintf(stderr,"Label Space input: %s\n",label_space_str);
        break;
      case 'i':
        in_instr_str = optarg;
        if(verbose) fprintf(stderr,"In instr input: %s\n",in_instr_str);
        break;
      case 'o':
        out_instr_str = optarg;
        if(verbose) fprintf(stderr,"Out instr input: %s\n",out_instr_str);
        break;
      case 'O':
        out_label_str = optarg;
        if(verbose) fprintf(stderr,"Out label input: %s\n",out_label_str);
        break;
      case 'f':
        fec_str = optarg;
        if(verbose) fprintf(stderr,"FEC input: %s\n",fec_str);
        break;
      case 'T':
        tunnel_str = optarg;
        if(verbose) fprintf(stderr,"Tunnel input: %s\n",tunnel_str);
        break;
      case 'h':
      default:
        usage();
        exit(result);
        break;
    }
  }

  if(debug) {
    result = ioctl(fd,SIOCMPLSDEBUG,&ifr);
    perror("Debug");
  }

  if(label_space_str) {
    int len;

    num = parse_args(1024,larg,label_space_str);
    if(num == 2) {
      len = strlen(larg[0]);
      if(verbose) fprintf(stderr,"If: %s LS: %s\n",larg[0],larg[1]);
      if(len > 0 && len < IFNAMSIZ) {
        memset(&ifr,0,sizeof(struct ifreq));
        strcpy(ifr.ifr_name,larg[0]);
        if((result = ioctl(fd,SIOCGIFINDEX,&ifr)) != 0) {
          perror("SIOCGIFINDEX");
          exit(result);
        }
        mls_req.mls_ifindex = ifr.ifr_ifindex;
        mls_req.mls_labelspace = atoi(larg[1]);
        result = ioctl(fd,SIOCSLABELSPACEMPLS,&mls_req);
      }
    }
    perror("Label Space");
  }

  if(tunnel_str) {
    num = parse_args(1024,larg,tunnel_str);
    if(add && num == 1) {
      strncpy(ifr.ifr_name,larg[0],IFNAMSIZ);
      result = ioctl(fd,SIOCMPLSTUNNELADD,&ifr);
      if(bind) {
        result = ioctl(fd,SIOCGIFINDEX,&ifr);
      }
    } else if((delete && num == 1) || (bind && num == 1)) {
      strcpy(ifr.ifr_name,larg[0]);
      if(delete) {
        result = ioctl(fd,SIOCMPLSTUNNELDEL,&ifr);
      } else {
        result = ioctl(fd,SIOCGIFINDEX,&ifr);
      }
    } else {
      fprintf(stderr,"Tunnel: wrong number of paramters(%d)\n",num);
      fprintf(stderr,"Tunnel: -A -T <name>\n");
      fprintf(stderr,"Tunnel: -D -T <name>\n");
      fprintf(stderr,"Tunnel: -B ... -T <name>\n");
      tunnel_str = NULL;
    }
  }

  if(in_label_str) {
    num = parse_args(1024,larg,in_label_str);
    if(num == 3) {
      fill_label(&mil_req.mil_label,larg,0);
      mil_req.mil_labelspace = atoi(larg[2]);
      if(delete) {
        result = send_ilm(netlink,&mil_req,RTM_DELILM);
        perror("In Label del");
      } else if(add) {
        result = send_ilm(netlink,&mil_req,RTM_NEWILM);
        perror("In Label add");
      }
    } else {
      fprintf(stderr,"In Label: wrong number of paramters(%d)\n",num);
      in_label_str = NULL;
    }
  }

  if(out_label_str) {
    num = parse_args(1024,larg,out_label_str);
    if(num >= 3) {
      fill_label(&mol_req.mol_label,larg,0);
      finish_out_label(&mol_req,larg,2);

      if(delete) {
        result = send_nhlfe(netlink,&mol_req,RTM_DELNHLFE);
        perror("Out Label del");
      } else if(add) {
        fprintf(stderr,"out: %d\n",mol_req.mol_ifindex);
        fprintf(stderr,"out: %d\n",mol_req.mol_label.ml_type);
        fprintf(stderr,"out: %d\n",mol_req.mol_label.u.ml_gen);
        fprintf(stderr,"out: %s\n",inet_ntoa(((struct sockaddr_in*)&(mol_req.mol_nh))->sin_addr));
        result = send_nhlfe(netlink,&mol_req,RTM_NEWNHLFE);
        perror("Out Label add");
      }
    } else {
      fprintf(stderr,"Out Label: wrong number of paramters(%d)\n",num);
      out_label_str = NULL;
    }
  }

  if(fec_str) {
    num = parse_args(1024,larg,fec_str);
    if(num == 1) {
      char *prefix_str = strtok(larg[0],"/");
      char *len_str = strtok(NULL,"\0");

      if(prefix_str && len_str) {
        mbf_req.mbf_fec.prefix = resolve_it(prefix_str);
        mbf_req.mbf_fec.len = atoi(len_str);
        mbf_req.mbf_ifindex = mol_req.mol_ifindex;

        memcpy(&mbf_req.mbf_label,&mol_req.mol_label,
          sizeof(struct mpls_label));
      } else {
        fprintf(stderr,"FEC: wrong format\n");
        fec_str = NULL;
      }
    } else {
      fprintf(stderr,"FEC: wrong number of paramters(%d)\n",num);
      fec_str = NULL;
    }
  }

  if(fec_str && out_label_str) {
    if(bind && !delete) {
      result = send_ftn(netlink,&mbf_req,RTM_NEWFTN);
      perror("FTN add");
    } else if(unbind && !add) {
      result = send_ftn(netlink,&mbf_req,RTM_DELFTN);
      perror("FTN del");
    } else {
      fprintf(stderr,"FTN: No modifer specified\n");
    } 
  }

  if(in_label_str && in_instr_str) {
    num = parse_args(1024,larg,in_instr_str);
    if(num >= 1) {
      mir_req.mir_direction = MPLS_IN;
      if((result = fill_instructions(&mir_req,larg,0,num)) >= 0) {
        memcpy(&mir_req.mir_label,&mil_req.mil_label,sizeof(struct mpls_label));
        mir_req.mir_index = mil_req.mil_labelspace;
        result = send_instr(netlink,&mir_req,RTM_SETININSTR);
      }
      perror("In Instr");
    } else {
      fprintf(stderr,"In Instr: wrong number of parameters(%d)\n",num);
    }
  }

  if(out_label_str && out_instr_str) {
    num = parse_args(1024,larg,out_instr_str);
    if(num >= 1) {
      mir_req.mir_direction = MPLS_OUT;
      if((result = fill_instructions(&mir_req,larg,0,num)) >= 0) {
        memcpy(&mir_req.mir_label,&mol_req.mol_label,sizeof(struct mpls_label));
        mir_req.mir_index = mol_req.mol_ifindex;
        result = send_instr(netlink,&mir_req,RTM_SETOUTINSTR);
      }
      perror("Out Instr");
    } else {
      fprintf(stderr,"Out Instr: wrong number of parameters(%d)\n",num);
    }
  }

  if(out_label_str && tunnel_str) {
    if(ifr.ifr_ifindex == 0) {
      fprintf(stderr,"%s: no such interface\n",tunnel_str);
    } else {
      memcpy(&ifr.ifr_data,&mol_req.mol_label,sizeof(struct mpls_label));

      if(bind && !delete) {
        result = ioctl(fd,SIOCMPLSTUNNELADDOUT,&ifr);
        perror("Bind Tunnel2Out add");
      } else if(unbind && !add) {
        result = ioctl(fd,SIOCMPLSTUNNELDELOUT,&ifr);
        perror("Bind Tunnel2Out del");
      } else {
        fprintf(stderr,"Bind Tunnel2Out: No modifer specified\n");
      }
    }
  }

  if(out_label_str && in_label_str) {
    if(in_instr_str || out_instr_str) {
      fprintf(stderr,"Bind In2Out: not at same time as In or Out Instr\n");
    } else {
      memcpy(&mx_req.mx_in,&mil_req.mil_label,sizeof(struct mpls_label));
      memcpy(&mx_req.mx_out,&mol_req.mol_label,sizeof(struct mpls_label));
      mx_req.mx_ilabelspace = mil_req.mil_labelspace;
      mx_req.mx_oifindex = mol_req.mol_ifindex;

      if(bind && !delete) {
        result = send_xc(netlink,&mx_req,RTM_NEWXC);
        perror("Bind In2Out add");
      } else if(unbind && !add) {
        result = send_xc(netlink,&mx_req,RTM_DELXC);
        perror("Bind In2Out del");
      } else {
        fprintf(stderr,"Bind In2Out: No modifer specified\n");
      }
    }
  }
  return result;
}