ipoe: futher kernel module implementing

2 files changed, 187 insertions, 51 deletions

diff --git a/drivers/ipoe/ipoe.c b/drivers/ipoe/ipoe.c
index 58e4bbdd..6749fa8b 100644
--- a/drivers/ipoe/ipoe.c
+++ b/drivers/ipoe/ipoe.c
@@ -3,16 +3,15 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
-#include <asm/uaccess.h>
 #include <linux/skbuff.h>
 #include <linux/netdevice.h>
+#include <linux/etherdevice.h>
 #include <linux/in.h>
 #include <linux/tcp.h>
 #include <linux/udp.h>
 #include <linux/if_arp.h>
 #include <linux/mroute.h>
 #include <linux/init.h>
-#include <linux/netfilter_ipv4.h>
 #include <linux/if_ether.h>
 #include <linux/semaphore.h>
 #include <linux/rbtree.h>
@@ -22,8 +21,6 @@
 #include <net/sock.h>
 #include <net/ip.h>
 #include <net/icmp.h>
-#include <net/inet_ecn.h>
-#include <net/xfrm.h>
 #include <net/net_namespace.h>
 #include <net/netns/generic.h>
 
@@ -35,13 +32,17 @@
 #define NEED_UPDATE 1
 #define UPDATE 2
 
+#ifndef DEFINE_SEMAPHORE
+#define DEFINE_SEMAPHORE(name) struct semaphore name = __SEMAPHORE_INITIALIZER(name, 1)
+#endif
+
 struct ipoe_session
 {
 	struct rb_node node;
 
 	__be32 addr;
 	__be32 peer_addr;
-	//__u8 hwaddr[ETH_ALEN];
+	__u8 hwaddr[ETH_ALEN];
 
 	struct net_device *dev;
 	struct net_device *link_dev;
@@ -246,24 +247,57 @@ static netdev_tx_t ipoe_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct ipoe_session *ses = netdev_priv(dev);
 	struct net_device_stats *stats = &dev->stats;
 	struct iphdr  *iph;
+	struct ethhdr *eth;
+	/*struct arphdr *arp;
+	unsigned char *arp_ptr;
+	__be32 tip;*/
 	int noff;
 
 	noff = skb_network_offset(skb);
 
-	if (!pskb_may_pull(skb, sizeof(*iph) + noff))
-		goto drop;
-	
-	iph = ip_hdr(skb);
+	if (skb->protocol == htons(ETH_P_IP)) {
+		if (!pskb_may_pull(skb, sizeof(*iph) + noff))
+			goto drop;
+		
+		iph = ip_hdr(skb);
 
-	pr_info("ipoe: xmit %08x %08x\n", iph->saddr, iph->daddr);
-	
-	/*u64_stats_update_begin(&ses->tsync);
-	ses->tx_packets++;
-	ses->tx_bytes += skb->len;
-	u64_stats_update_end(&ses->tsync);*/
+		//pr_info("ipoe: xmit %08x %08x\n", iph->saddr, iph->daddr);
+		
+		/*u64_stats_update_begin(&ses->tsync);
+		ses->tx_packets++;
+		ses->tx_bytes += skb->len;
+		u64_stats_update_end(&ses->tsync);*/
 
-	if (iph->daddr == ses->addr && ipoe_do_nat(skb, ses->peer_addr, 1))
-		goto drop;
+		if (iph->daddr == ses->addr) {
+			if (ipoe_do_nat(skb, ses->peer_addr, 1))
+				goto drop;
+
+			eth = (struct ethhdr *)skb->data;
+
+			memcpy(eth->h_dest, ses->hwaddr, ETH_ALEN);
+			memcpy(eth->h_source, ses->link_dev->dev_addr, ETH_ALEN);
+		}
+	} /*else if (skb->protocol == htons(ETH_P_ARP)) {
+		if (!pskb_may_pull(skb, arp_hdr_len(dev) + noff))
+			goto drop;
+		
+		arp = arp_hdr(skb);
+		arp_ptr = (unsigned char *)(arp + 1);
+		
+		if (arp->ar_op == htons(ARPOP_REQUEST)) {
+			memcpy(&tip, arp_ptr + ETH_ALEN + 4 + ETH_ALEN, 4);
+			if (tip == ses->addr) {
+				if (skb_cloned(skb) &&
+						!skb_clone_writable(skb, arp_hdr_len(dev) + noff) &&
+						pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
+					goto drop;
+				
+				arp = arp_hdr(skb);
+				arp_ptr = (unsigned char *)(arp + 1);
+				memcpy(arp_ptr + ETH_ALEN + 4 + ETH_ALEN, &ses->peer_addr, 4);
+			}
+		}
+	}*/
 		
 	skb->dev = ses->link_dev;
 	skb->skb_iif = dev->ifindex;
@@ -276,17 +310,92 @@ drop:
 	return NETDEV_TX_OK;
 }
 
+static int ipoe_rcv_arp(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt, struct net_device *orig_dev)
+{
+	struct ipoe_session *ses = NULL;
+	struct arphdr *arp;
+	unsigned char *arp_ptr;
+	int noff;
+	__be32 sip;
+	int upd;
+	struct sk_buff *skb1;
+
+	//pr_info("ipoe: recv arp\n");
+	
+	if (skb->pkt_type == PACKET_OTHERHOST)
+		goto drop;
+	
+	noff = skb_network_offset(skb);
+
+	if (!pskb_may_pull(skb, arp_hdr_len(dev) + noff))
+		goto drop;
+	
+	arp = arp_hdr(skb);
+	arp_ptr = (unsigned char *)(arp + 1);
+
+	if (arp->ar_pro != htons(ETH_P_IP))
+		goto drop;
+	
+	memcpy(&sip, arp_ptr + ETH_ALEN, 4);
+
+	//pr_info("ipoe: recv arp %08x\n", sip);
+
+	atomic_inc(&ipoe_rlock);
+	upd = atomic_read(&ipoe_update);
+	if (upd)
+		goto drop_unlock;
+
+	ses = ipoe_lookup(sip, NULL, NULL);
+	if (!ses)
+		goto drop_unlock;
+	
+	if (ses->drop || ses->addr)
+		goto drop_unlock;
+	
+	if (skb->dev == ses->dev)
+		goto drop_unlock;
+	
+	/*if (ses->addr && arp->ar_op == htons(ARPOP_REPLY)) {
+		if (skb_cloned(skb) &&
+				!skb_clone_writable(skb, arp_hdr_len(dev) + noff) &&
+				pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
+			goto drop_unlock;
+
+		arp = arp_hdr(skb);
+		arp_ptr = (unsigned char *)(arp + 1);
+		memcpy(arp_ptr + ETH_ALEN, &ses->addr, 4);
+	}*/
+
+	skb1 = skb_clone(skb, GFP_ATOMIC);
+	if (!skb1)
+		goto drop_unlock;
+
+	skb1->dev = ses->dev;
+	skb1->skb_iif = ses->dev->ifindex;
+
+	netif_rx(skb1);
+	
+drop_unlock:
+	upd = atomic_read(&ipoe_update);
+	if (atomic_dec_and_test(&ipoe_rlock) && upd == NEED_UPDATE)
+		atomic_set(&ipoe_update, UPDATE);
+
+drop:
+	kfree_skb(skb);
+	return NET_RX_DROP;
+}
+
 static int ipoe_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt, struct net_device *orig_dev)
 {
 	struct ipoe_session *ses = NULL;
 	struct iphdr *iph;
 	int upd;
 	int noff;
-	int r;
+	struct sk_buff *skb1;
 
 	if (skb->pkt_type == PACKET_OTHERHOST)
 		goto drop;
-
+	
 	noff = skb_network_offset(skb);
 
 	if (!pskb_may_pull(skb, sizeof(*iph) + noff))
@@ -318,21 +427,26 @@ static int ipoe_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_t
 	if (ses->drop)
 		goto drop_unlock;
 	
-	if (ses->addr && ipoe_do_nat(skb, ses->addr, 0))
+	if (skb->dev == ses->dev)
 		goto drop_unlock;
 	
-	skb->dev = ses->dev;
-	//skb->skb_iif = ses->link_dev->ifindex;
-
-	r = netif_rx(skb);
-	
-	atomic_dec(&ipoe_rlock);
+	if (ses->addr && ipoe_do_nat(skb, ses->addr, 0))
+		goto drop_unlock;
 	
-	return r;
+	skb1 = skb_clone(skb, GFP_ATOMIC);
+	if (!skb1)
+		goto drop_unlock;
 
+	skb1->dev = ses->dev;
+	skb1->skb_iif = ses->dev->ifindex;
 
+	netif_rx(skb1);
+	
 drop_unlock:
-	atomic_dec(&ipoe_rlock);
+	upd = atomic_read(&ipoe_update);
+	if (atomic_dec_and_test(&ipoe_rlock) && upd == NEED_UPDATE)
+		atomic_set(&ipoe_update, UPDATE);
+	
 drop:
 	kfree_skb(skb);
 	return NET_RX_DROP;
@@ -344,10 +458,18 @@ static int ipoe_hard_header(struct sk_buff *skb, struct net_device *dev,
 {
 	const struct ipoe_session *ses = netdev_priv(dev);
 
-	return dev_hard_header(skb, ses->link_dev, type, ses->hwaddr,
-			       dev->dev_addr, len);
+	return dev_hard_header(skb, ses->link_dev, type, daddr,
+			       saddr, len);
 }
 
+static const struct header_ops ipoe_hard_header_ops = {
+	.create  	= ipoe_hard_header,
+	.rebuild	= eth_rebuild_header,
+	.parse		= eth_header_parse,
+	.cache		= eth_header_cache,
+	.cache_update	= eth_header_cache_update,
+};
+
 static void ipoe_netdev_setup(struct net_device *dev)
 {
 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
@@ -358,11 +480,11 @@ static void ipoe_netdev_setup(struct net_device *dev)
 	dev->destructor = free_netdev;
 
 	dev->type = ARPHRD_ETHER;
-	dev->hard_header_len = ETH_ALEN;
+	dev->hard_header_len = 0;
 	dev->mtu = ETH_DATA_LEN;
-	dev->flags = IFF_NOARP;
+	dev->flags = 0;//IFF_NOARP | IFF_BROADCAST;
 	dev->iflink = 0;
-	dev->addr_len = 4;
+	dev->addr_len = ETH_ALEN;
 	dev->features  = 0;//|= NETIF_F_NETNS_LOCAL;
 	dev->header_ops		= &ipoe_hard_header_ops,
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35)
@@ -383,7 +505,7 @@ static int ipoe_create(__be32 peer_addr, __be32 addr, const char *link_ifname, c
 	if (!link_dev)
 		return -EINVAL;
 
-	sprintf(name, "ipoe%%d");
+	sprintf(name, "%s.ipoe%%d", link_ifname);
 
 	dev = alloc_netdev(sizeof(*ses), name, ipoe_netdev_setup);
 	if (dev == NULL)
@@ -402,6 +524,12 @@ static int ipoe_create(__be32 peer_addr, __be32 addr, const char *link_ifname, c
 	ses->link_dev = link_dev;
 	memcpy(ses->hwaddr, hwaddr, ETH_ALEN);
 	dev->features = link_dev->features;
+	memcpy(dev->dev_addr, link_dev->dev_addr, ETH_ALEN);
+	memcpy(dev->broadcast, link_dev->broadcast, ETH_ALEN);
+	if (addr)
+		dev->flags = IFF_NOARP;
+	else
+		dev->flags = IFF_BROADCAST;
 
 	rtnl_lock();
 	r = register_netdevice(dev);
@@ -538,7 +666,6 @@ out:
 
 static int ipoe_nl_cmd_create(struct sk_buff *skb, struct genl_info *info)
 {
-	struct ipoe_session *ses;
 	__be32 peer_addr, addr  = 0;
 	int ret = 0;
 	char ifname[IFNAMSIZ];
@@ -555,7 +682,7 @@ static int ipoe_nl_cmd_create(struct sk_buff *skb, struct genl_info *info)
 		addr = nla_get_be32(info->attrs[IPOE_ATTR_ADDR]);
 	nla_strlcpy(ifname, info->attrs[IPOE_ATTR_IFNAME], IFNAMSIZ - 1);
 	if (info->attrs[IPOE_ATTR_HWADDR])
-		nla_strlcpy(hwaddr, info->attrs[IPOE_ATTR_HWADDR], ETH_ALEN);
+		nla_memcpy(hwaddr, info->attrs[IPOE_ATTR_HWADDR], ETH_ALEN);
 	else
 		memset(hwaddr, 0, sizeof(hwaddr));
 
@@ -588,7 +715,7 @@ static struct nla_policy ipoe_nl_policy[IPOE_ATTR_MAX + 1] = {
 	[IPOE_ATTR_ADDR]	      = { .type = NLA_U32,                        },
 	[IPOE_ATTR_PEER_ADDR]	= { .type = NLA_U32,                          },
 	[IPOE_ATTR_IFNAME]	    = { .type = NLA_STRING, .len = IFNAMSIZ - 1 },
-	[IPOE_ATTR_HWADDR]	    = { .type = NLA_BINARY, .len = ETH_ALEN     },
+	[IPOE_ATTR_HWADDR]	    = { .type = NLA_U64                         },
 };
 
 static struct genl_ops ipoe_nl_ops[] = {
@@ -620,14 +747,6 @@ static struct genl_family ipoe_nl_family = {
 	.maxattr	= IPOE_ATTR_MAX,
 };
 
-static const struct header_ops ipoe_hard_header_ops = {
-	.create  	= ipoe_hard_header,
-	.rebuild	= eth_rebuild_header,
-	.parse		= eth_header_parse,
-	.cache		= eth_header_cache,
-	.cache_update	= eth_header_cache_update,
-};
-
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35)
 static const struct net_device_ops ipoe_netdev_ops = {
 	.ndo_start_xmit	= ipoe_xmit,
@@ -639,6 +758,11 @@ static struct packet_type ip_packet_type = {
 	.func = ipoe_rcv,
 };
 
+static struct packet_type arp_packet_type = {
+	.type = __constant_htons(ETH_P_ARP),
+	.func = ipoe_rcv_arp,
+};
+
 /*static struct pernet_operations ipoe_net_ops = {
 	.init = ipoe_init_net,
 	.exit = ipoe_exit_net,
@@ -648,7 +772,7 @@ static struct packet_type ip_packet_type = {
 
 static int __init ipoe_init(void)
 {
-	int i, err;
+	int err;
 
 	printk("IPoE session driver v0.1\n");
 
@@ -687,6 +811,7 @@ static int __init ipoe_init(void)
 	skb_queue_head_init(&ipoe_rq);
 
 	dev_add_pack(&ip_packet_type);
+	dev_add_pack(&arp_packet_type);
 
 	return 0;
 
@@ -701,6 +826,7 @@ out:
 static void __exit ipoe_fini(void)
 {
 	dev_remove_pack(&ip_packet_type);
+	dev_remove_pack(&arp_packet_type);
 	genl_unregister_family(&ipoe_nl_family);
 }
 
diff --git a/ipoe-util/ipses-create.c b/ipoe-util/ipses-create.c
index 5aaf1ab7..4c02cc67 100644
--- a/ipoe-util/ipses-create.c
+++ b/ipoe-util/ipses-create.c
@@ -1,4 +1,6 @@
+#include <net/ethernet.h>
 #include <netinet/in.h>
+#include <netinet/ether.h>
 #include <arpa/inet.h>
 #include <string.h>
 #include <errno.h>
@@ -22,14 +24,20 @@ int main(int argc, char **argv)
 	int family;
 	in_addr_t local, remote;
 	int err;
+	union {
+		struct ether_addr a;
+		uint64_t u64;
+	} hwaddr;
 
-	if (argc != 4) {
-		printf("usage: ipses-create <ifname> <peer_addr> <addr>\n");
+	if (argc != 4 && argc != 5) {
+		printf("usage: ipses-create <ifname> <hwaddr> <peer_addr> <addr>\n");
 		return 1;
 	}
 
-	local = inet_addr(argv[2]);
-	remote = inet_addr(argv[3]);
+	ether_aton_r(argv[2], &hwaddr.a);
+	local = inet_addr(argv[3]);
+	if (argc == 5)
+		remote = inet_addr(argv[4]);
 
 #if LIBNL2
 	h = nl_socket_alloc();
@@ -42,8 +50,10 @@ int main(int argc, char **argv)
 	msg = nlmsg_alloc();
 	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, NLM_F_REQUEST, IPOE_CMD_CREATE, IPOE_GENL_VERSION);
 	nla_put_u32(msg, IPOE_ATTR_PEER_ADDR, local);
-	nla_put_u32(msg, IPOE_ATTR_ADDR, remote);
 	nla_put_string(msg, IPOE_ATTR_IFNAME, argv[1]);
+	nla_put_u64(msg, IPOE_ATTR_HWADDR, hwaddr.u64);
+	if (argc == 5)
+		nla_put_u32(msg, IPOE_ATTR_ADDR, remote);
 	
 	nl_send_auto_complete(h, msg);
 	err = nl_recvmsgs_default(h);