#include <stdio.h> 
#include <stdlib.h> 
#include <string.h> 
#include <sys/socket.h> 
#include <linux/netlink.h>
#include <linux/rtnetlink.h>

#define MAX_PAYLOAD 2048 

/* All globals initialized to 0 */
struct sockaddr_nl src, dst; 
struct iovec iov; 
struct msghdr msg; 
int sock_fd; 
    
struct {
    struct nlmsghdr  n; 
    struct ifinfomsg i; 
    char             buf[MAX_PAYLOAD]; 
} req; 

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

/* Function from libnetlink.h */
int addattr_l(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) {
        fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
        return -1;
    }
    rta = NLMSG_TAIL(n);
    rta->rta_type = type;
    rta->rta_len = len;
    memcpy(RTA_DATA(rta), data, alen);
    n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
    return 0;
}

void usage() 
{
    fprintf(stderr, "Incorrect usage\n"); 
    fprintf(stderr, "./nl <vlan_intf_name> <intf_name> <vlan_id>\n"); 
}

void dumpData(const char *note, const char *msg, unsigned int len)
{
    // no error checks 
    unsigned int i; 

    //fprintf(stderr, "\n%s(%u):\n", note, len);

    for(i=0; i<len; ++i) {
        fprintf(stderr, "%02X ",msg[i]); 
        
        if (i>0 && i%15 == 0) 
            fprintf(stderr, "\n"); 
    }

    fprintf(stderr,"\n"); 
}

void main(int argc, char *argv[]) 
{
    char *type, *intf_name, *vlan_intf_name; 
    __u16   vlan_id; 
    int     ifindex;  

    if (argc != 4) {
        usage(); 
        return; 
    }

    vlan_intf_name = argv[1]; 
    intf_name = argv[2]; 
    vlan_id = atoi(argv[3]); 
    
    fprintf(stderr, "%s, %s, %d\n", vlan_intf_name, intf_name, vlan_id); 
    sock_fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE); 
    if (sock_fd < 0) 
        return;

    memset(&src, 0, sizeof(src)); 
    memset(&dst, 0, sizeof(dst)); 
    memset(&req, 0, sizeof(req)); 

    /* Details of source end of socket */
    src.nl_family   = AF_NETLINK; 
    src.nl_pid      = getpid();

    bind(sock_fd, (struct sockaddr *)&src, sizeof(src)); 

    /* Details of destination side of socket */
    dst.nl_family   = AF_NETLINK; 

    req.n.nlmsg_len = NLMSG_SPACE(sizeof(struct ifinfomsg)); 
    req.n.nlmsg_pid = getpid(); 
    req.n.nlmsg_type = RTM_NEWLINK;  
    req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE ;//| NLM_F_EXCL;
   
    ifindex = if_nametoindex(intf_name);  
    
    req.i.ifi_family = AF_UNSPEC; 
    //req.i.ifi_index  = ifindex;
    struct rtattr *linkinfo = NLMSG_TAIL(&req.n);
    addattr_l(&req.n, sizeof(req), IFLA_LINKINFO, NULL, 0); 
    addattr_l(&req.n, sizeof(req), IFLA_INFO_KIND, "vlan", 4); 
    struct rtattr * data = NLMSG_TAIL(&req.n);
    addattr_l(&req.n, sizeof(req), IFLA_INFO_DATA, NULL, 0);
    addattr_l(&req.n, sizeof(req), IFLA_VLAN_ID, &vlan_id, 2);
    data->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)data;
    linkinfo->rta_len = (void *)NLMSG_TAIL(&req.n) - (void *)linkinfo;
    addattr_l(&req.n, sizeof(req), IFLA_LINK, &ifindex, 4);
    addattr_l(&req.n, sizeof(req), IFLA_IFNAME, vlan_intf_name, strlen(vlan_intf_name)+1);

    /* send msg to kernel */
    if( send(sock_fd, &req, req.n.nlmsg_len, 0) < 0)
       fprintf(stderr, "failed write\n");

    //dumpData("Dst socket", (char *)msg.msg_name, msg.msg_namelen);
    dumpData("Netlink msg", (char *)&req.n, req.n.nlmsg_len);

    close(sock_fd); 
}