[PATCH] Implementation of tap device for Ethernet level 2 in pfinet

So, as a last kick on the horse, this is a re-send as seemeingly expected.

Ralph.

---
 pfinet/linux-src/include/linux/if_ether.h | 44 +++++++++++++
 pfinet/linux-src/net/core/dev.c           |  4 +-
 pfinet/main.c                             |  2 +
 pfinet/tunnel.c                           | 77 ++++++++++++++++++-----
 4 files changed, 110 insertions(+), 17 deletions(-)

diff --git a/pfinet/linux-src/include/linux/if_ether.h b/pfinet/linux-src/include/linux/if_ether.h
index 99bb97fa2..efb566e05 100644
--- a/pfinet/linux-src/include/linux/if_ether.h
+++ b/pfinet/linux-src/include/linux/if_ether.h
@@ -87,6 +87,50 @@ struct ethhdr
 	unsigned short	h_proto;		/* packet type ID field	*/
 };
 
+/* Alternative Ethernet header with VLAN tagging. The header without
+   VLAN tagging (the original header) is found in <net/ethernet.h>.
+   This header modelling accounts for packets optionally having VLAN
+   tagging inserted before the ether_type field.
+*/
+
+struct ethhdr_vlan
+{
+    uint8_t  h_dest[ETH_ALEN];        /* destination eth addr */
+    uint8_t  h_source[ETH_ALEN];      /* source ether addr    */
+    uint16_t h_vlan_tag;              /* htons( 0x8100 ) */
+    uint16_t h_vlan_id;               /* VLAN ID */
+    uint16_t h_proto;                 /* packet type ID field */
+};
+
+/* Generic header that incorporates both without and with VLAN alternatives */
+union ethhdr_either {
+    struct ethhdr without_vlan;
+    struct ethhdr_vlan with_vlan;
+};
+
+/* Repeating from net/ethernet.h */
+#define ETHERTYPE_VLAN 0x8100
+
+/* Detecting presence of VLAN tag */
+static inline int has_vlan_tag(unsigned char *frame) {
+    uint16_t v = ntohs( ((struct ethhdr_vlan*)frame)->h_vlan_tag );
+    return ( v == ETHERTYPE_VLAN );
+}
+
+/* Obtain ether_type handling optional VLAN tag. */
+static inline uint16_t get_protocol(unsigned char *frame)
+{
+    return ntohs( has_vlan_tag(frame)?
+		  (((struct ethhdr_vlan*)frame)->h_proto) :
+		  (((struct ethhdr*)frame)->h_proto) );
+}
+
+/* Obtain payload base address handling optional VLAN tag */
+static inline unsigned char *get_ether_payload_p(unsigned char *frame) {
+    return frame + ( has_vlan_tag(frame)?
+		     sizeof(struct ethhdr_vlan) : sizeof(struct ethhdr) );
+}
+
 /*
  *	We Have changed the ethernet statistics collection data. This
  *	is just for partial compatibility for now.
diff --git a/pfinet/linux-src/net/core/dev.c b/pfinet/linux-src/net/core/dev.c
index b47c50270..93e5aab1f 100644
--- a/pfinet/linux-src/net/core/dev.c
+++ b/pfinet/linux-src/net/core/dev.c
@@ -923,7 +923,9 @@ void net_bh(void)
 		 */
 
 		/* XXX until we figure out every place to modify.. */
-		skb->h.raw = skb->nh.raw = skb->data;
+		if ( skb->nh.raw < skb->data ||
+		     skb->nh.raw > skb->data + sizeof(struct ethhdr_vlan)) 
+		    skb->h.raw = skb->nh.raw = skb->data;
 
 		if (skb->mac.raw < skb->head || skb->mac.raw > skb->data) {
 			printk(KERN_CRIT "%s: wrong mac.raw ptr, proto=%04x\n", skb->dev->name, skb->protocol);
diff --git a/pfinet/main.c b/pfinet/main.c
index cd50950b7..233735a3b 100644
--- a/pfinet/main.c
+++ b/pfinet/main.c
@@ -244,6 +244,8 @@ find_device (char *name, struct device **device)
 
   if (strncmp(base_name, "tun", 3) == 0)
     setup_tunnel_device (name, device);
+  else if (strncmp(base_name, "tap", 3) == 0)
+    setup_tunnel_device (name, device);
   else if (strncmp(base_name, "dummy", 5) == 0)
     setup_dummy_device (name, device);
   else
diff --git a/pfinet/tunnel.c b/pfinet/tunnel.c
index 4301d054f..6532cac5c 100644
--- a/pfinet/tunnel.c
+++ b/pfinet/tunnel.c
@@ -58,7 +58,6 @@ struct tunnel_device
   struct net_device_stats stats;
 };
 
-
 /* Linked list of all tunnel devices.  */
 struct tunnel_device *tunnel_dev;
 
@@ -146,6 +145,14 @@ tunnel_xmit (struct sk_buff *skb, struct device *dev)
   return 0;
 }
 
+/* Obtained from ethernet.c and edited */
+static int
+tunnel_change_flags (struct device *dev, short flags)
+{
+    /* unsupported, but ignore */
+  return 0;
+}
+
 void
 setup_tunnel_device (char *name, struct device **device)
 {
@@ -168,16 +175,17 @@ setup_tunnel_device (char *name, struct device **device)
   tunnel_dev = tdev;
 
   *device = dev = &tdev->dev;
+  dev->priv = tdev;
+  skb_queue_head_init(&tdev->xq); // Initialise tdev->xq
 
   base_name = strrchr (name, '/');
   if (base_name)
     base_name++;
   else
     base_name = name;
-
+  
   dev->name = strdup (base_name);
 
-  dev->priv = tdev;
   dev->get_stats = tunnel_get_stats;
 
   /* Functions.  These ones are the true "hardware layer" in Linux.  */
@@ -186,21 +194,44 @@ setup_tunnel_device (char *name, struct device **device)
   dev->hard_start_xmit = tunnel_xmit;
   dev->set_multicast_list = tunnel_set_multi;
 
-  /* These are the ones set by drivers/net/ppp_generic.c::ppp_net_init.  */
-  dev->hard_header = 0;
-  dev->hard_header_len = 0;
-  dev->mtu = PPP_MTU;
-  dev->addr_len = 0;
-  dev->tx_queue_len = 3;
-  dev->type = ARPHRD_PPP;
-  dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
+  if ( strncmp( base_name, "tap", 3 ) == 0 ) {
+      /* The setup for a tap is most similar the ethernet.c */
+      dev->hard_header = eth_header;
+      dev->rebuild_header = eth_rebuild_header;
+      dev->hard_header_cache = eth_header_cache;
+      dev->header_cache_update = eth_header_cache_update;
+      dev->hard_header_parse = eth_header_parse;
+      dev->hard_header_len = ETH_HLEN;
+      dev->mtu = PPP_MTU;
+      dev->addr_len = ETH_ALEN;
+      dev->tx_queue_len = 3;
+      dev->type = ARPHRD_ETHER;
+      dev->flags = IFF_BROADCAST | IFF_MULTICAST | IFF_ALLMULTI;
+      dev->change_flags = tunnel_change_flags;
+
+      // We *know* that ETH_ALEN == 6
+      unsigned char num[ 4 ]; // 
+      sscanf( name, "tap%u", (unsigned int*)&num );
+      memcpy( dev->dev_addr, (char[6]) {
+	      0x06, 'T', num[3], num[2], num[1], num[0] }, 6 );
+  } else {
+      /* These are the ones set by drivers/net/ppp_generic.c::ppp_net_init.  */
+      dev->hard_header = 0;
+      dev->hard_header_len = 0;
+      dev->mtu = PPP_MTU;
+      dev->addr_len = 0;
+      dev->tx_queue_len = 3;
+      dev->type = ARPHRD_ETHER;
+      dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
+      dev->change_flags = tunnel_change_flags;
+  }
 
   dev_init_buffers (dev);
 
   if (base_name != name)
     tdev->devname = strdup (name);
   else
-    /* Setting up the translator at /dev/tunX.  */
+    /* Setting up the translator at /dev/tunX or /dev/tapX  */
     asprintf (&tdev->devname, "/dev/%s", tdev->dev.name);
   tdev->underlying = file_name_lookup (tdev->devname, O_CREAT|O_NOTRANS, 0664);
 
@@ -299,7 +330,8 @@ trivfs_S_io_read (struct trivfs_protid *cred,
   /* Deny access if they have bad credentials. */
   if (! cred)
     return EOPNOTSUPP;
-  else if (! (cred->po->openmodes & O_READ))
+
+  if (! (cred->po->openmodes & O_READ))
     return EBADF;
 
   if (cred->pi.class != tunnel_class)
@@ -380,7 +412,8 @@ trivfs_S_io_write (struct trivfs_protid *cred,
   /* Deny access if they have bad credentials. */
   if (! cred)
     return EOPNOTSUPP;
-  else if (! (cred->po->openmodes & O_WRITE))
+
+  if (! (cred->po->openmodes & O_WRITE))
     return EBADF;
 
   if (cred->pi.class != tunnel_class)
@@ -388,6 +421,10 @@ trivfs_S_io_write (struct trivfs_protid *cred,
 
   tdev = (struct tunnel_device *) cred->po->cntl->hook;
 
+  if ((tdev->dev.hard_header != 0) && (datalen < 18)) // small packet
+    return EBADF;
+
+
   pthread_mutex_lock (&tdev->lock);
 
   pthread_mutex_lock (&net_bh_lock);
@@ -400,7 +437,14 @@ trivfs_S_io_write (struct trivfs_protid *cred,
 
   /* Drop it on the queue. */
   skb->mac.raw = skb->data;
-  skb->protocol = htons (ETH_P_IP);
+  if ( tdev->dev.hard_header != 0 ) {
+      skb->protocol = get_protocol(skb->data);
+      skb->h.raw = skb->nh.raw = get_ether_payload_p(skb->data);
+  } else {
+      skb->protocol = htons(ETH_P_IP);
+      skb->h.raw = skb->nh.raw = skb->data;
+  }
+
   netif_rx (skb);
   pthread_mutex_unlock (&net_bh_lock);
 
@@ -424,7 +468,8 @@ trivfs_S_io_readable (struct trivfs_protid *cred,
   /* Deny access if they have bad credentials. */
   if (! cred)
     return EOPNOTSUPP;
-  else if (! (cred->po->openmodes & O_READ))
+
+  if (! (cred->po->openmodes & O_READ))
     return EBADF;
 
   if (cred->pi.class != tunnel_class)
-- 
2.47.3