From a46fc77772ec0a9373985162afb36c3299bb1218 Mon Sep 17 00:00:00 2001 From: LorD ClockaN Date: Wed, 24 Aug 2011 03:37:35 +0800 Subject: [PATCH] ADD: Kineto Gand module --- drivers/net/kineto_gan.c | 540 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 540 insertions(+) create mode 100644 drivers/net/kineto_gan.c diff --git a/drivers/net/kineto_gan.c b/drivers/net/kineto_gan.c new file mode 100644 index 00000000..f3931d60 --- /dev/null +++ b/drivers/net/kineto_gan.c @@ -0,0 +1,540 @@ +/* GAN Virtual Ethernet Device + * + * Copyright c 2010, Kineto Wireless, Inc. sourcecode@kineto.com + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /* struct iphdr */ +#include /* struct tcphdr */ + +#include +#include +#include +#include + + +#include +#include +#include +/* For socket etc */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define MODULE_NAME "gannet" + +#define GAN_VIF_LINK_PORT 13010 +#define GAN_PS_PORT_2 13001 + +struct gannet_private { + struct net_device_stats stats; + struct sockaddr_in tx_addr; + struct sockaddr_in rx_addr; + struct socket *tx_sock; + struct socket *rx_sock; +}; + +struct gannet_thread { + struct task_struct *thread; + struct net_device *dev; + struct gannet_private *priv; +}; + +static struct gannet_thread *gthread; +static struct net_device *gdev; +static int gthreadquit; +static int firstsetup; + +/* Work queue modifications */ +static struct workqueue_struct *gannet_wq; + +struct gannet_work_struct { + struct work_struct gannet_work; + struct sk_buff *skb; + struct gannet_private *p; +}; + + +static int count_this_packet(void *_hdr, int len) +{ + struct ethhdr *hdr = _hdr; + + if (len >= ETH_HLEN && hdr->h_proto == htons(ETH_P_ARP)) + return 0; + + return 1; +} + + +static void rx(unsigned char *buf, int len) +{ + struct sk_buff *skb; + void *ptr = 0; + int sz; + int r; + + sz = len; + + if (sz > 1514) { + printk(KERN_ERR MODULE_NAME "gannet discarding %d len\n", sz); + ptr = 0; + } else { + skb = dev_alloc_skb(sz + 14 + NET_IP_ALIGN); + if (skb == NULL) { + printk(KERN_ERR MODULE_NAME + "gannet cannot allocate skb\n"); + } else { + skb_reserve(skb, NET_IP_ALIGN); + + ptr = skb_put(skb, 14); /* ethernet hdr */ + memcpy(&((unsigned char *) ptr)[6], + gthread->dev->dev_addr, 6); + + ptr = skb_put(skb, sz); + memcpy(ptr, buf, sz); + + skb->dev = gthread->dev; + skb->protocol = eth_type_trans(skb, gthread->dev); + skb->protocol = htons(ETH_P_IP); + skb->ip_summed = CHECKSUM_NONE; /* check it */ + + skb->pkt_type = PACKET_HOST; + + if (count_this_packet(ptr, skb->len)) { + gthread->priv->stats.rx_packets++; + gthread->priv->stats.rx_bytes += skb->len; + } + r = netif_rx(skb); + } + } +} + + +static int ksocket_receive(struct socket *sock, + struct sockaddr_in *addr, + unsigned char *buf, int len) +{ + struct msghdr msg; + struct iovec iov; + mm_segment_t oldfs; + int size = 0; + + if (sock->sk == NULL) + return 0; + + iov.iov_base = buf; + iov.iov_len = len; + + msg.msg_flags = 0; + msg.msg_name = addr; + msg.msg_namelen = sizeof(struct sockaddr_in); + msg.msg_control = NULL; + msg.msg_controllen = 0; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = NULL; + + oldfs = get_fs(); + set_fs(KERNEL_DS); + size = sock_recvmsg(sock, &msg, len, msg.msg_flags); + set_fs(oldfs); + + return size; +} + + +static int ksocket_sendto(struct socket *sock, + struct sockaddr_in *addr, + unsigned char *buf, int len) +{ + struct msghdr msg; + struct iovec iov; + mm_segment_t oldfs; + int size = 0; + + if (sock->sk == NULL) + return 0; + + iov.iov_base = buf; + iov.iov_len = len; + + msg.msg_flags = 0; + msg.msg_name = addr; + msg.msg_namelen = sizeof(struct sockaddr_in); + msg.msg_control = NULL; + msg.msg_controllen = 0; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = NULL; + + oldfs = get_fs(); + set_fs(KERNEL_DS); + size = sock_sendmsg(sock, &msg, len); + set_fs(oldfs); + + return size; +} + + +static void gannet_wq_func(struct work_struct *work) +{ + struct gannet_work_struct *gannet_work = + (struct gannet_work_struct *)work; + + /* write skb->data of skb->len to udp socket */ + struct gannet_private *p = gannet_work->p; + struct sk_buff *skb = gannet_work->skb; + char *data; + int len; + + data = skb->data; + len = skb->len; + + /* Remove ethernet header */ + data += 14; + len -= 14; + + if (len != ksocket_sendto(p->tx_sock, &p->tx_addr, data, len)) { + printk(KERN_ERR + "gannet sendto() failed, dropping packet\n"); + } else { + if (count_this_packet(data, len)) { + p->stats.tx_packets++; + p->stats.tx_bytes += len; + } + } + + dev_kfree_skb(skb); + + kfree((void *)work); +} + + +static void gannet_recvloop(void) +{ + int size; + int bufsize = 1600; + unsigned char buf[bufsize + 1]; + + /* kernel thread initialization */ + lock_kernel(); + + current->flags |= PF_NOFREEZE; + + /* daemonize (take care with signals, + after daemonize they are disabled) */ + daemonize(MODULE_NAME); + allow_signal(SIGKILL); + unlock_kernel(); + + + /* main loop */ + while (!gthreadquit) { + memset(&buf, 0, bufsize + 1); + size = ksocket_receive(gthread->priv->rx_sock, + >hread->priv->rx_addr, + buf, bufsize); + + if (signal_pending(current)) + break; + + if (size < 0) { + printk(KERN_ERR MODULE_NAME + "gannet: error getting datagram, " + "sock_recvmsg error = %d\n", size); + } else { + /* send to kernel */ + rx(buf, size); + } + } + + printk(KERN_INFO "gannet thread exit\n"); +} + + +static int gannet_open(struct net_device *dev) +{ + int ret; + struct gannet_private *p; + + printk(KERN_DEBUG "gannet_open()\n"); + netif_start_queue(dev); + + if (firstsetup == 1) { + printk(KERN_DEBUG "gannet firstsetup executed\n"); + firstsetup = 0; + + gthreadquit = 0; + p = netdev_priv(dev); + + /* Create tx socket */ + ret = sock_create(PF_INET, SOCK_DGRAM, + IPPROTO_UDP, &p->tx_sock); + if (ret < 0) { + printk(KERN_ERR + "gannet tx socket() failed, failing init.\n"); + unregister_netdev(dev); + free_netdev(dev); + return -EIO; /* I/O error */ + } + memset(&p->tx_addr, 0, sizeof(p->tx_addr)); + p->tx_addr.sin_family = AF_INET; + p->tx_addr.sin_port = htons(GAN_PS_PORT_2); + p->tx_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + + /* Create rx socket */ + ret = sock_create(PF_INET, SOCK_DGRAM, + IPPROTO_UDP, &p->rx_sock); + if (ret < 0) { + printk(KERN_ERR + "gannet rx socket() failed, failing init.\n"); + sock_release(p->tx_sock); + unregister_netdev(dev); + free_netdev(dev); + return -EIO; /* I/O error? */ + } + memset(&p->rx_addr, 0, sizeof(p->rx_addr)); + p->rx_addr.sin_family = AF_INET; + p->rx_addr.sin_port = htons(GAN_VIF_LINK_PORT); + p->rx_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + + /* Bind rx socket */ + ret = p->rx_sock->ops->bind(p->rx_sock, + (struct sockaddr *) &p->rx_addr, + sizeof(struct sockaddr)); + if (ret < 0) { + printk(KERN_ERR "gannet rx socket() bind failed.\n"); + sock_release(p->tx_sock); + sock_release(p->rx_sock); + unregister_netdev(dev); + free_netdev(dev); + return -EIO; /* I/O error */ + } else { + printk(KERN_ERR "gannet rx socket() bind success.\n"); + } + + /* Create kernel thread for rx loop */ + gthread = kzalloc(sizeof(struct gannet_thread), GFP_KERNEL); + if (gthread == NULL) { + printk(KERN_ERR MODULE_NAME + "gannet: unable to allocate mem for kernel thread\n"); + sock_release(p->tx_sock); + sock_release(p->rx_sock); + unregister_netdev(dev); + free_netdev(dev); + return -ENOMEM; + } + + /* Create work queue */ + gannet_wq = create_workqueue("gannet_queue"); + if (gannet_wq == NULL) { + printk(KERN_ERR MODULE_NAME + "gannet: unable to initialize work queue\n"); + kfree(gthread); + sock_release(p->tx_sock); + sock_release(p->rx_sock); + unregister_netdev(dev); + free_netdev(dev); + return -ENOMEM; + } + + /* Store ref to private info */ + gthread->dev = dev; + gthread->priv = p; + + printk(KERN_INFO "gannet starting kernel thread\n"); + gthread->thread = kthread_run((void *) gannet_recvloop, + NULL, MODULE_NAME); + if (IS_ERR(gthread->thread)) { + printk(KERN_ERR MODULE_NAME + "gannet: unable to start kernel thread\n"); + sock_release(p->tx_sock); + sock_release(p->rx_sock); + unregister_netdev(dev); + free_netdev(dev); + kfree(gthread); + destroy_workqueue(gannet_wq); + gthread = NULL; + return -ENOMEM; + } + } + + return 0; +} + +static int gannet_stop(struct net_device *dev) +{ + printk(KERN_DEBUG "gannet_stop()\n"); + netif_stop_queue(dev); + return 0; +} + + + +static int gannet_xmit(struct sk_buff *skb, struct net_device *dev) +{ + struct gannet_private *p = netdev_priv(dev); + int ret; + struct gannet_work_struct *work; + + if (NULL == ipip_hdr(skb)) { + printk(KERN_WARNING "gannet dropping packet, no IP header\n"); + dev_kfree_skb(skb); + return NET_XMIT_DROP; + } + + if (skb->len < (sizeof(struct ethhdr) + sizeof(struct iphdr))) { + printk(KERN_WARNING + "gannet: Packet too short (%i bytes)\n", skb->len); + return 0; + } + + if (gannet_wq) { + work = kmalloc(sizeof(struct gannet_work_struct), GFP_ATOMIC); + if (work) { + INIT_WORK((struct work_struct *)work, gannet_wq_func); + work->skb = skb; + work->p = p; + + ret = queue_work(gannet_wq, (struct work_struct *)work); + + } else { + printk(KERN_WARNING "gannet dropping packet, \ + cannot allocate work queue item\n"); + } + } + + return 0; +} + +static struct net_device_stats *gannet_get_stats(struct net_device *dev) +{ + struct gannet_private *p = netdev_priv(dev); + return &p->stats; +} + +static void gannet_set_multicast_list(struct net_device *dev) +{ +} + +static void gannet_tx_timeout(struct net_device *dev) +{ + printk(KERN_DEBUG "gannet_tx_timeout()\n"); +} + + +static const struct net_device_ops gannet_netdev_ops = { + .ndo_open = gannet_open, + .ndo_stop = gannet_stop, + .ndo_start_xmit = gannet_xmit, + .ndo_get_stats = gannet_get_stats, + .ndo_set_multicast_list = gannet_set_multicast_list, + .ndo_tx_timeout = gannet_tx_timeout, + .ndo_change_mtu = NULL, +}; + +static void __init gannet_setup(struct net_device *dev) +{ + printk(KERN_INFO "gannet_setup\n"); + + ether_setup(dev); + + dev->mtu = 1000; + dev->netdev_ops = &gannet_netdev_ops; + /* The minimum time (in jiffies) that should pass before the networking + * layer decides that a transmission timeout has occurred and calls the + * driver's tx_time-out function. + */ + dev->watchdog_timeo = 200; + + /* keep the default flags, just add NOARP */ + dev->flags |= IFF_NOARP; + + random_ether_addr(dev->dev_addr); + + netif_start_queue(dev); +} + +static int __init gannet_init(void) +{ + int ret; + struct net_device *dev; + + dev = alloc_netdev(sizeof(struct gannet_private), + "gannet%d", gannet_setup); + if (NULL == dev) + return -ENOMEM; + + ret = register_netdev(dev); + if (ret) { + printk(KERN_ERR "gannet failed to register netdev\n"); + free_netdev(dev); + return ret; + } + + gdev = dev; + + firstsetup = 1; + + printk(KERN_INFO "gannet initialized OK\n"); + + return 0; +} + +static void __exit gannet_exit(void) +{ + gthreadquit = 1; + if (NULL != gdev) { + struct gannet_private *p = netdev_priv(gdev); + + flush_scheduled_work(); + destroy_workqueue(gannet_wq); + sock_release(p->rx_sock); + sock_release(p->tx_sock); + unregister_netdev(gdev); + free_netdev(gdev); + kfree(gthread); + gthread = NULL; + gdev = NULL; + } +} + +module_init(gannet_init); +module_exit(gannet_exit); + +MODULE_DESCRIPTION("Kineto GAN Virtual Ethernet Device"); +MODULE_ALIAS("gannet"); +MODULE_VERSION("1.0"); +MODULE_LICENSE("GPL");