netlink.c

#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <net/if.h>
#include <linux/if_tun.h>
#include <linux/if_packet.h>

#include <arpa/inet.h>
#include <netdb.h>

#include <asm/types.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>

#include <errno.h>

static void hexdump_into_buffer(const void *data, const size_t len,
                         void *buffer, const size_t capacity,
                         const size_t line_bytes) {
    const size_t lines = len/line_bytes + !!(len%line_bytes);
    const char *prefix_format = "\n0x%04X: ";

    char *begin = buffer;
    const char *end = buffer + capacity;

    unsigned char *byte = (unsigned char*)data;
    unsigned char *byte_end = byte + len;

    unsigned int i;
    for(i=0;begin < end && i < lines;++i) {
        begin += snprintf(begin,end-begin,prefix_format,i*line_bytes);

        unsigned char *curr_end = byte + line_bytes;
        if(curr_end > byte_end) curr_end = byte_end;
        for(;byte < curr_end && begin < end;++byte) {
            begin += snprintf(begin,end-begin," %02hhX",*byte);
        }
    }
}

static void hexdump(const void *data, const size_t len) {
    const size_t line_bytes = 16;
    const size_t lines = len/line_bytes + !!(len%line_bytes);

    size_t capacity = 3*line_bytes*lines + 10*lines;
    char *buffer = malloc(capacity);
    if(!buffer) {
        fprintf(stderr,"hexdump memory allocation failed[%lu]\n",capacity);
        return;
    }
    
    hexdump_into_buffer(data,len,buffer,capacity,line_bytes);

    printf("%s\n",buffer+1);

    free(buffer);
}

int tun_alloc(char *dev)
{
    struct ifreq ifr;
    int fd, err;

    if( (fd = open("/dev/net/tun", O_RDWR)) < 0 ) {
        perror("open");
        return -1;
    }

    memset(&ifr, 0, sizeof(ifr));

    /* Flags: IFF_TUN   - TUN device (no Ethernet headers) 
    *        IFF_TAP   - TAP device  
    *
    *        IFF_NO_PI - Do not provide packet information  
    */ 
    ifr.ifr_flags = IFF_TUN | IFF_NO_PI; 
    if( *dev )
        strncpy(ifr.ifr_name, dev, IFNAMSIZ);

    if( (err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0 ){
        perror("ioctl");
        close(fd);
        return err;
    }
    if(!*dev)
        strcpy(dev, ifr.ifr_name);
    return fd;
}

int if_mod_flags(const char *ifname, int set_flags, int unset_flags) {
    int sockfd = socket(AF_INET,SOCK_DGRAM,IPPROTO_IP);
    if(sockfd < 0) {
        perror("socket");
        return -1;
    }

    struct ifreq ifr;
    strcpy(ifr.ifr_name,ifname);

    int ret = ioctl(sockfd,SIOCGIFFLAGS,&ifr);
    if(ret < 0) {
        perror("ioctl SIOCGIFFLAGS");
        close(sockfd);
        return -1;
    }

    ifr.ifr_flags |= set_flags;
    ifr.ifr_flags &= ~unset_flags;

    ret = ioctl(sockfd,SIOCSIFFLAGS,&ifr);
    if(ret < 0) {
        perror("ioctl SIOCSIFFLAGS");
        close(sockfd);
        return -1;
    }

    return close(sockfd);
}

int netlink_connect(int family, int groups) {
    int fd = socket(PF_NETLINK, SOCK_RAW|SOCK_CLOEXEC, family);
    if(fd < 0) {
        perror("netlink socket");
        return -1;
    }

    struct sockaddr_nl addr;
    memset(&addr, 0, sizeof(addr));
    addr.nl_family = AF_NETLINK;
    addr.nl_groups = groups;

    int ret = bind(fd, (struct sockaddr *) &addr, sizeof(addr));
    if(ret < 0) {
        perror("netlink bind");
        close(fd);
        return -1;
    }

    return fd;
}

int netlink_tx(int fd, struct nlmsghdr *header) {
    /* Request an ack from kernel by setting NLM_F_ACK */
    header->nlmsg_flags |= NLM_F_ACK;

    struct iovec iov = { header, header->nlmsg_len }; 
    
    struct sockaddr_nl addr;    
    memset(&addr, 0, sizeof(addr));
    addr.nl_family = AF_NETLINK;

    struct msghdr message = { &addr, sizeof(addr), &iov, 1, NULL, 0, 0 };    

    fprintf(stderr,"%s -> [%d] size[%d]:\n",__func__,fd,header->nlmsg_len);
    hexdump(header,header->nlmsg_len);
    int ret = sendmsg(fd, &message, 0);
    if(ret < 0) {
        perror("netlink sendmsg");
        return -1;
    }

    return 0;
}

static int netlink_rx(int fd, void *buf, int *len) {
    struct iovec iov = { buf, *len };
    struct sockaddr_nl addr;
    struct msghdr msg = { &addr, sizeof(addr), &iov, 1, NULL, 0, 0 };
        
    *len = recvmsg(fd, &msg, 0);
    if(*len < 0) {
        perror("netlink recvmsg");
        return -1;
    }
    fprintf(stderr,"%s <- [%d] size[%d]:\n",__func__,fd,*len);
    hexdump(buf,*len);

    return 0;
}

static struct rtattr * netlink_add_attribute(struct nlmsghdr *header,int maxlen,int type, void *data, int datalen) {
    int len = RTA_SPACE(datalen);
    
    if ((int)NLMSG_ALIGN(header->nlmsg_len) + len > maxlen)
        return 0;
    struct rtattr *attr = (struct rtattr*)(((char*)header) + NLMSG_ALIGN(header->nlmsg_len));
    attr->rta_type = type;
    attr->rta_len = len;
    memcpy(RTA_DATA(attr), data, datalen);
    header->nlmsg_len = NLMSG_ALIGN(header->nlmsg_len) + len;
    return attr;
}

static const char * nlmsg_type_to_str(int nlmsg_type) {
    static const struct {
        int nlmsg_type;
        const char* nlmsg_str;
    } nlmsg_type_str[] = {
        {NLMSG_NOOP,   "NLMSG_NOOP"},
        {NLMSG_ERROR,  "NLMSG_ERROR"},
        {NLMSG_DONE,   "NLMSG_DONE"},
        {NLMSG_OVERRUN,"NLMSG_OVERRUN"},
    };

    size_t i;
    for(i=0;i<sizeof(nlmsg_type_str)/sizeof(*nlmsg_type_str);++i) {
        if(nlmsg_type_str[i].nlmsg_type == nlmsg_type) {
            return nlmsg_type_str[i].nlmsg_str;
        }
    }

    return "Unknown";
}


typedef int (*netlink_response_handler)(struct nlmsghdr *,size_t len, void*);

int netlink_check_response(struct nlmsghdr *nh, size_t len, void *arg) {
    int rc = 0;
    
    (void)arg;

    for (;NLMSG_OK(nh, len);nh = NLMSG_NEXT(nh, len)) {
        fprintf(stderr,"%s: nlmsg_type[%s]\n",
                       __func__,nlmsg_type_to_str(nh->nlmsg_type));
        if (nh->nlmsg_type == NLMSG_ERROR) {
            struct nlmsgerr *e = (struct nlmsgerr*)NLMSG_DATA(nh);
            fprintf(stderr,"NLMSG_ERROR[%d]\n",e->error);
            if(e->error) {
                rc = -1;
                fprintf(stderr,"nlmsghdr:\n");
                hexdump(&e->msg,e->msg.nlmsg_len);
            }
        }
    }

    return rc;
}

int netlink_transceive(int fd, struct nlmsghdr *header,
                       netlink_response_handler handler,void *arg) {
    if(netlink_tx(fd,header) < 0) {
        return -1;
    }

    char buf[4096];
    int len = sizeof(buf);

    if(netlink_rx(fd,buf,&len) < 0) {
        return -2;
    }

    if(handler) {
        return handler((struct nlmsghdr *)buf,len,arg);
    }

    return 0;
}

typedef enum {
    ADDR_DEL = 0,
    ADDR_ADD
} ADDR_ACTION;

static int if_mod_address(const char *ifname,
                          ADDR_ACTION action,
                          int address_type,
                          void *address,
                          int address_len,
                          int prefix) {
    int fd = netlink_connect(NETLINK_ROUTE,0);
    if(fd < 0) {
        return -1;
    }

    // structure of the netlink packet. 
    struct {
        struct nlmsghdr   header;
        struct ifaddrmsg  message;
        char              attrbuf[512];
    } request;

    memset(&request, 0, sizeof(request));
    request.header.nlmsg_flags = NLM_F_REQUEST;
    request.header.nlmsg_len = NLMSG_LENGTH(sizeof(request.message));
    request.header.nlmsg_seq = time(0);

    request.message.ifa_family = address_type;
    request.message.ifa_prefixlen = prefix;
    request.message.ifa_flags = 0;
    request.message.ifa_scope = 0;

    request.message.ifa_index = if_nametoindex(ifname);
    if(!request.message.ifa_index) {
        fprintf(stderr,"%s: if_nametoindex[%s] -> 0\n",__func__,ifname);
        close(fd);
        return -1;
    }

    switch(action) {
        case ADDR_ADD:
            request.header.nlmsg_type = RTM_NEWADDR;
            request.header.nlmsg_flags |= NLM_F_CREATE | NLM_F_EXCL;
            break;
        case ADDR_DEL:
            request.header.nlmsg_type = RTM_DELADDR;
            break;
        default:
            close(fd);
            return -1;
    }

    if(!netlink_add_attribute(&request.header,sizeof(request),
                              IFA_LOCAL,address,address_len)) {
        close(fd);
        return -1;
    }

    if(netlink_transceive(fd,&request.header,netlink_check_response,0) < 0) {
        close(fd);
        return -1;
    }

    close(fd);

    return 0;
}

static int if_mod_address_str(const char *ifname,
                              ADDR_ACTION action,
                              const char *address,
                              int prefix) {
    struct addrinfo hint;
    memset(&hint, '\0', sizeof(hint));
    hint.ai_family = PF_UNSPEC;
    hint.ai_flags = AI_NUMERICHOST;

    struct addrinfo *result = 0;
    int ret = getaddrinfo(address, 0, &hint, &result);
    if(ret) {
        fprintf(stderr,"%s: getaddrinfo for [%s] -> [%d][%s]\n",
                       __func__,address,ret,gai_strerror(ret));
        return -1;
    }

    struct sockaddr_in *addr_in;
    struct sockaddr_in6 *addr_in6;
    switch(result->ai_family) {
        case AF_INET:
            addr_in = (struct sockaddr_in*)result->ai_addr;
            ret = if_mod_address(ifname,action,AF_INET,
                                 &addr_in->sin_addr,sizeof(addr_in->sin_addr),
                                 prefix);
            break;
        case AF_INET6:
            addr_in6 = (struct sockaddr_in6*)result->ai_addr;
            ret = if_mod_address(ifname,action,AF_INET6,
                                 &addr_in6->sin6_addr,sizeof(addr_in6->sin6_addr),
                                 prefix);
            break;
        default:
            fprintf(stderr,"%s: unknown address[%s] family[%d]\n",
                           __func__,address,result->ai_family);
            ret = -1;
            break;
    }

    freeaddrinfo(result);
    return ret;
}

#define PREPARE_NETLINK_REQUEST(netlink_message_type,message_type,attrbuf_size)\
struct {\
    struct nlmsghdr header;\
    message_type message;\
    char attrbuf[attrbuf_size];\
} request;\
memset(&request, 0, sizeof(request));\
request.header.nlmsg_type = netlink_message_type;\
request.header.nlmsg_flags = NLM_F_REQUEST;\
request.header.nlmsg_len = NLMSG_LENGTH(sizeof(request.message));\
request.header.nlmsg_seq = time(0);

static int if_set_addrgenmode(const char *ifname, enum in6_addr_gen_mode mode) {
    int fd = netlink_connect(NETLINK_ROUTE,0);
    if(fd < 0) {
        return -1;
    }

    PREPARE_NETLINK_REQUEST(RTM_NEWLINK,struct ifinfomsg,512);

    request.message.ifi_index = if_nametoindex(ifname);
    if(!request.message.ifi_index) {
        fprintf(stderr,"%s: if_nametoindex[%s] -> 0\n",__func__,ifname);
        close(fd);
        return -1;
    }

    struct rtattr *af_spec = netlink_add_attribute(&request.header,sizeof(request),
                                                   IFLA_AF_SPEC,0,0);
    struct rtattr *af_inet6 = netlink_add_attribute(&request.header,sizeof(request),
                                                    AF_INET6,0,0);
    struct rtattr *addr_gen_mode = netlink_add_attribute(&request.header,sizeof(request),
                                                         IFLA_INET6_ADDR_GEN_MODE,&mode,sizeof(char));
    af_inet6->rta_len += addr_gen_mode->rta_len;
    af_spec->rta_len += af_inet6->rta_len;

    if(netlink_transceive(fd,&request.header,netlink_check_response,0) < 0) {
        close(fd);
        return -1;
    }

    close(fd);

    return 0;
}

static int if_set_hwaddr(const char *ifname, void* hwaddr, size_t len) {
    int fd = netlink_connect(NETLINK_ROUTE,0);
    if(fd < 0) {
        return -1;
    }

    PREPARE_NETLINK_REQUEST(RTM_NEWLINK,struct ifinfomsg,512);

    request.message.ifi_index = if_nametoindex(ifname);
    if(!request.message.ifi_index) {
        fprintf(stderr,"%s: if_nametoindex[%s] -> 0\n",__func__,ifname);
        close(fd);
        return -1;
    }

    netlink_add_attribute(&request.header,sizeof(request),
                          IFLA_ADDRESS,hwaddr,len);

    if(netlink_transceive(fd,&request.header,netlink_check_response,0) < 0) {
        close(fd);
        return -1;
    }

    close(fd);

    return 0;
}

int main() {
    char tun_name[IFNAMSIZ] = "tun1";

    int tun_fd = tun_alloc(tun_name);
    printf("tun[%d]\n",tun_fd);
    if(tun_fd < 0) {
        return 1;
    }

    //int htype;
    //get_address("enp0s3",&htype);

    /*const unsigned char hwaddr[] = {1,2,3,4,5,6};
    if(if_set_hwaddr(tun_name,hwaddr,sizeof(hwaddr)) < 0) {
        return 3;
    }*/

    //system("ip link set dev tun1 addrgenmode none");

    //if_set_hwaddr("lo","\x00\x02\x03\x04\x05\x06",6);

    if(if_set_addrgenmode(tun_name,IN6_ADDR_GEN_MODE_NONE) < 0) {
        return 3;
    }

    if(if_mod_flags(tun_name,IFF_UP | IFF_RUNNING | IFF_PROMISC,0) < 0) {
        return 2;
    }

    if(if_mod_address_str(tun_name,ADDR_ADD,"fe80::1",64) < 0) {
        return 1;
    }

    if(if_mod_address_str(tun_name,ADDR_ADD,"10.0.30.1",32) < 0) {
        return 1;
    }

    sleep(60);

    if(if_mod_address_str(tun_name,ADDR_DEL,"11::22",64) < 0) {
        return 1;
    }

    sleep(100);

    return 0;
}