ip link経由でのドライバ追加

家にあるLinuxデバイスドライバの本のネットワークドライバのサンプルが古すぎて動かないので 4.9.57で動くようにしてついでに今風にipコマンド経由で追加できるように書き換えてみた。

最低限動くようにするところだけ移行。

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/netdevice.h>
#include <net/rtnetlink.h>
#include <linux/if_ether.h>
#include <linux/ip.h>
#include <linux/etherdevice.h>

MODULE_LICENSE("GPL");

#define MYQUEUE_RX_INTR 1

struct mynet_packet {
    struct mynet_packet *next;
    struct net_device *dev;
    int datalen;
    u8 data[ETH_DATA_LEN];
};

int mynet_newlink(struct net *net, struct net_device *dev, struct nlattr *tb[], struct nlattr *data[]);

static int pool_size = 8;

struct mynet_private {
    struct net_device *peer;
    int is_peer;
    struct mynet_packet *ppool;
    struct mynet_packet *rx_queue;
    spinlock_t lock;
    int status;
};

static void mynet_setup_pool(struct net_device *dev) {
    struct mynet_private *priv = netdev_priv(dev);
    struct mynet_packet *pkt;
    int i;
    priv->ppool = NULL;
    for ( i = 0; i < pool_size; ++i ) {
        pkt = kmalloc(sizeof(struct mynet_packet), GFP_KERNEL);
        if ( !pkt ) {
            printk(KERN_ERR "mynet setup pool error");
            return;
        }
        pkt->next = priv->ppool;
        pkt->dev = dev;
        pkt->datalen = 0;
        priv->ppool = pkt;
    }
}

static struct mynet_packet* mynet_get_tx_buffer(struct net_device *dev) {
    struct mynet_private *priv = netdev_priv(dev);
    struct mynet_packet *pkt;
    unsigned long flags;
    spin_lock_irqsave(&priv->lock, flags);
    pkt = priv->ppool;
    priv->ppool = pkt->next;
    pkt->next = NULL;

    if ( priv->ppool == NULL ) {
        netif_stop_queue(dev);
    }
    spin_unlock_irqrestore(&priv->lock, flags);
    return pkt;
}

void mynet_enqueue_buf(struct net_device *dev, struct mynet_packet *pkt) {
    unsigned long flag;
    struct mynet_private *priv = netdev_priv(dev);
    spin_lock_irqsave(&priv->lock, flag);
    pkt->next = priv->rx_queue;
    priv->rx_queue = pkt;
    spin_unlock_irqrestore(&priv->lock, flag);
}

void mynet_release_buffer(struct mynet_packet *pkt) {
    struct mynet_private *priv = netdev_priv(pkt->dev);
    unsigned long flags;
    spin_lock_irqsave(&priv->lock, flags);
    pkt->next = priv->ppool;
    priv->ppool = pkt;
    spin_unlock_irqrestore(&priv->lock, flags);
    if ( netif_queue_stopped(pkt->dev) && pkt->next == NULL ) {
        netif_wake_queue(pkt->dev);
    }

}

void print_mac(char *mac) {
    unsigned char buff[1024];
    int i, k = 0;

    for ( i = 0; i < ETH_ALEN; ++i ) {
        buff[++k] = hex_asc_hi(mac[i]);
        buff[++k] = hex_asc_hi(mac[i]);
        buff[++k] = ':';
    }

    buff[k-1] = '\0';
    printk("%s", buff);
}

static void mynet_rx(struct net_device *dev, struct mynet_packet *pkt) {
    struct sk_buff *skb;

    struct iphdr *iph;

    printk(KERN_DEBUG "mynet_rx");
    skb = dev_alloc_skb(pkt->datalen + 2);

    skb_reserve(skb, 2);
    iph = (struct iphdr*)(skb->data + sizeof(struct ethhdr));
    memcpy(skb_put(skb, pkt->datalen), pkt->data, pkt->datalen);
    skb->dev = dev;
    skb->protocol = eth_type_trans(skb, dev);
    skb->ip_summed = CHECKSUM_UNNECESSARY;

    printk(KERN_DEBUG "||||protocol %d\n", skb->protocol);
    printk(KERN_DEBUG "||||len %d", pkt->datalen);
    printk(KERN_DEBUG "||||saddr : %pI4", &iph->saddr);
    printk(KERN_DEBUG "||||daddr : %pI4", &iph->daddr);

    netif_rx(skb);
}

static void mynet_interrupt(struct net_device *dev) {
    struct mynet_private *priv = netdev_priv(dev);
    struct mynet_packet *pkt = NULL;
    int status = priv->status;
    priv->status = 0;

    spin_lock(&priv->lock);
    if ( status & MYQUEUE_RX_INTR ) {
        pkt = priv->rx_queue;
        if ( pkt ) {
            priv->rx_queue = pkt->next;
            mynet_rx(dev, pkt);
        }
    }

    spin_unlock(&priv->lock);
}

netdev_tx_t mynet_start_xmit(struct sk_buff *skb, struct net_device *dev) {
    struct ethhdr *eh;
    struct iphdr *iph;
    struct mynet_private *priv, *peer_priv;
    u32 *saddr, *daddr;
    struct mynet_packet *tx_buffer;

    priv = netdev_priv(dev);
    peer_priv = netdev_priv(priv->peer);

    printk(KERN_WARNING "mynet xmit %s", dev->name);

    if ( !pskb_may_pull(skb, sizeof(struct iphdr) + sizeof(struct ethhdr)) ) {
        printk(KERN_DEBUG "can no read ether header\n");
        return NETDEV_TX_OK;
    }

    eh = (struct ethhdr*)skb->data;
    memcpy(eh->h_dest, priv->peer->dev_addr, ETH_ALEN);
    memcpy(eh->h_source, dev->dev_addr, ETH_ALEN);

    printk(KERN_DEBUG "addr len %d", dev->addr_len);
    printk(KERN_DEBUG "hard header len %d", dev->hard_header_len);
    iph = (struct iphdr*)((skb->data) + sizeof(struct ethhdr));
    printk(KERN_DEBUG "version: %d", iph->version);
    printk(KERN_DEBUG "saddr : %pI4", &iph->saddr);
    printk(KERN_DEBUG "daddr : %pI4", &iph->daddr);

    saddr = &iph->saddr;
    daddr = &iph->daddr;
    ((u8 *)saddr)[2] ^= 1;
    ((u8 *)daddr)[2] ^= 1;

    printk(KERN_DEBUG "=> saddr : %pI4", &iph->saddr);
    printk(KERN_DEBUG "=> daddr : %pI4", &iph->daddr);

    iph->check = 0;
    iph->check = ip_fast_csum((unsigned char*)iph, iph->ihl);

    tx_buffer = mynet_get_tx_buffer(dev);
    tx_buffer->datalen = skb->len;
    printk(KERN_DEBUG "||||len %d", skb->len);

    memcpy(tx_buffer->data, skb->data, skb->len);
    mynet_enqueue_buf(priv->peer, tx_buffer);

    peer_priv->status |= MYQUEUE_RX_INTR;
    mynet_interrupt(priv->peer);

    dev_kfree_skb(skb);

    return NETDEV_TX_OK;
}

int mynet_open(struct net_device *dev) {
    memcpy(dev->dev_addr, "\0TEST0", ETH_ALEN);
    if ( ((struct mynet_private*)netdev_priv(dev))->is_peer ) {
        dev->dev_addr[ETH_ALEN -1]++;
    }
    netif_start_queue(dev);
    return 0;
}

static struct net_device_ops mynet_ops = {
    .ndo_start_xmit = mynet_start_xmit,
    .ndo_open =  mynet_open,
};

void mynet_setup(struct net_device *dev) {
    struct mynet_private *priv;
    ether_setup(dev);
    dev->netdev_ops = &mynet_ops;
    dev->destructor = free_netdev;
    dev->flags = IFF_NOARP;
    mynet_setup_pool(dev);

    priv = netdev_priv(dev);
    spin_lock_init(&priv->lock);

}

void mynet_dellink(struct net_device *dev, struct list_head *head) {
    unregister_netdevice_queue(((struct mynet_private*)netdev_priv(dev))->peer, head);
    unregister_netdevice_queue(dev, head);
}

static struct rtnl_link_ops mynet_link_ops = {
    .kind = "mynet",
    .setup = mynet_setup,
    .newlink = mynet_newlink,
    .dellink = mynet_dellink,
    .priv_size  = sizeof(struct mynet_private),
};

int mynet_newlink(struct net *net, struct net_device *dev, struct nlattr *tb[], struct nlattr *data[]){
    struct net_device *peer = rtnl_create_link(net, "mynetpeer_%d", NET_NAME_ENUM, &mynet_link_ops, tb);
    ((struct mynet_private*)netdev_priv(peer))->is_peer = 1;
    printk("mynet newlink %p", peer);

    ((struct mynet_private*)netdev_priv(dev))->peer = peer;
    ((struct mynet_private*)netdev_priv(peer))->peer = dev;

    register_netdevice(dev);
    register_netdevice(peer);

    return 0;
}

static int init_mynet(void) {
    printk(KERN_DEBUG "===========-mynet init==========");
    return rtnl_link_register(&mynet_link_ops);
}

static void exit_mynet(void) {
    rtnl_link_unregister(&mynet_link_ops);
}

module_init(init_mynet);
module_exit(exit_mynet);

追加するスクリプトはこんな感じ

#!/bin/sh
FLAG_D=0

while getopts d OPT
do
    case $OPT in
        d) FLAG_D=1
        ;;
    esac
done

if [ $FLAG_D -eq 0 ];
then
    echo "install"
    insmod mynet.ko
    ip link add mynet0 type mynet
    ip link set mynet0 up
    ip link set mynetpeer_0 up

    ip addr add dev mynet0 1.1.0.1/24
    ip addr add dev mynetpeer_0 1.1.1.2/24
else
    echo "uninstall"
    ip link del mynet0
    rmmod mynet
fi    

ping 1.1.0.2でpingが通ることが確認できる