From d9f7de0ae3c0c09cff7257c55418450261f3e082 Mon Sep 17 00:00:00 2001

From: Paulo Flabiano Smorigo <pfsmorigo@br.ibm.com>

Date: Mon, 23 Dec 2013 12:32:02 -0200

Subject: [PATCH 013/198] Add Virtual LAN support.

This patch adds support for virtual LAN (VLAN) tagging. VLAN tagging allows

multiple VLANs in a bridged network to share the same physical network link

but maintain isolation:

http://en.wikipedia.org/wiki/IEEE_802.1Q

* grub-core/net/ethernet.c: Add check, get, and set vlan tag id.

* grub-core/net/drivers/ieee1275/ofnet.c: Get vlan tag id from bootargs.

* grub-core/net/arp.c: Add check.

* grub-core/net/ip.c: Likewise.

* include/grub/net/arp.h: Add vlantag attribute.

* include/grub/net/ip.h: Likewise.

---

 grub-core/net/arp.c                    | 12 +++++++--

 grub-core/net/drivers/ieee1275/ofnet.c |  9 ++++++-

 grub-core/net/ethernet.c               | 47 ++++++++++++++++++++++++++++++----

 grub-core/net/ip.c                     | 33 +++++++++++++++++-------

 include/grub/net.h                     |  3 +++

 include/grub/net/arp.h                 |  5 ++--

 include/grub/net/ip.h                  |  3 ++-

 7 files changed, 92 insertions(+), 20 deletions(-)

diff --git a/grub-core/net/arp.c b/grub-core/net/arp.c

index 4b68c4151..54306e3b1 100644

--- a/grub-core/net/arp.c

+++ b/grub-core/net/arp.c

@@ -111,8 +111,8 @@ grub_net_arp_send_request (struct grub_net_network_level_interface *inf,

 }

 grub_err_t

-grub_net_arp_receive (struct grub_net_buff *nb,

-		      struct grub_net_card *card)

+grub_net_arp_receive (struct grub_net_buff *nb, struct grub_net_card *card,

+                      grub_uint16_t *vlantag)

 {

   struct arppkt *arp_packet = (struct arppkt *) nb->data;

   grub_net_network_level_address_t sender_addr, target_addr;

@@ -138,6 +138,14 @@ grub_net_arp_receive (struct grub_net_buff *nb,

   FOR_NET_NETWORK_LEVEL_INTERFACES (inf)

   {

+    /* Verify vlantag id */

+    if (inf->card == card && inf->vlantag != *vlantag)

+      {

+        grub_dprintf ("net", "invalid vlantag! %x != %x\n",

+                      inf->vlantag, *vlantag);

+        break;

+      }

+

     /* Am I the protocol address target? */

     if (grub_net_addr_cmp (&inf->address, &target_addr) == 0

 	&& arp_packet->op == grub_cpu_to_be16_compile_time (ARP_REQUEST))

diff --git a/grub-core/net/drivers/ieee1275/ofnet.c b/grub-core/net/drivers/ieee1275/ofnet.c

index a78d164db..002446be1 100644

--- a/grub-core/net/drivers/ieee1275/ofnet.c

+++ b/grub-core/net/drivers/ieee1275/ofnet.c

@@ -153,11 +153,11 @@ grub_ieee1275_parse_bootpath (const char *devpath, char *bootpath,

   char *comma_char = 0;

   char *equal_char = 0;

   grub_size_t field_counter = 0;

-

   grub_net_network_level_address_t client_addr, gateway_addr, subnet_mask;

   grub_net_link_level_address_t hw_addr;

   grub_net_interface_flags_t flags = 0;

   struct grub_net_network_level_interface *inter = NULL;

+  grub_uint16_t vlantag = 0;

   hw_addr.type = GRUB_NET_LINK_LEVEL_PROTOCOL_ETHERNET;

@@ -175,6 +175,11 @@ grub_ieee1275_parse_bootpath (const char *devpath, char *bootpath,

           *equal_char = 0;

           grub_env_set_net_property ((*card)->name, args, equal_char + 1,

                                      grub_strlen(equal_char + 1));

+

+          if ((grub_strcmp (args, "vtag") == 0) &&

+              (grub_strlen (equal_char + 1) == 8))

+            vlantag = grub_strtoul (equal_char + 1 + 4, 0, 16);

+

           *equal_char = '=';

         }

       else

@@ -213,8 +218,10 @@ grub_ieee1275_parse_bootpath (const char *devpath, char *bootpath,

                                   hw_addr.mac, sizeof(hw_addr.mac), 0);

       inter = grub_net_add_addr ((*card)->name, *card, &client_addr, &hw_addr,

                                  flags);

+      inter->vlantag = vlantag;

       grub_net_add_ipv4_local (inter,

                           __builtin_ctz (~grub_le_to_cpu32 (subnet_mask.ipv4)));

+

     }

   if (gateway_addr.ipv4 != 0)

diff --git a/grub-core/net/ethernet.c b/grub-core/net/ethernet.c

index c397b1b34..4d7ceed6f 100644

--- a/grub-core/net/ethernet.c

+++ b/grub-core/net/ethernet.c

@@ -18,6 +18,7 @@

 #include <grub/misc.h>

 #include <grub/mm.h>

+#include <grub/env.h>

 #include <grub/net/ethernet.h>

 #include <grub/net/ip.h>

 #include <grub/net/arp.h>

@@ -56,10 +57,17 @@ send_ethernet_packet (struct grub_net_network_level_interface *inf,

 {

   struct etherhdr *eth;

   grub_err_t err;

+  grub_uint8_t etherhdr_size;

+  grub_uint16_t vlantag_id = VLANTAG_IDENTIFIER;

-  COMPILE_TIME_ASSERT (sizeof (*eth) < GRUB_NET_MAX_LINK_HEADER_SIZE);

+  etherhdr_size = sizeof (*eth);

+  COMPILE_TIME_ASSERT (sizeof (*eth) + 4 < GRUB_NET_MAX_LINK_HEADER_SIZE);

-  err = grub_netbuff_push (nb, sizeof (*eth));

+  /* Increase ethernet header in case of vlantag */

+  if (inf->vlantag != 0)

+    etherhdr_size += 4;

+

+  err = grub_netbuff_push (nb, etherhdr_size);

   if (err)

     return err;

   eth = (struct etherhdr *) nb->data;

@@ -76,6 +84,19 @@ send_ethernet_packet (struct grub_net_network_level_interface *inf,

 	return err;

       inf->card->opened = 1;

     }

+

+  /* Check and add a vlan-tag if needed. */

+  if (inf->vlantag != 0)

+    {

+      /* Move eth type to the right */

+      grub_memcpy ((char *) nb->data + etherhdr_size - 2,

+                   (char *) nb->data + etherhdr_size - 6, 2);

+

+      /* Add the tag in the middle */

+      grub_memcpy ((char *) nb->data + etherhdr_size - 6, &vlantag_id, 2);

+      grub_memcpy ((char *) nb->data + etherhdr_size - 4, (char *) &(inf->vlantag), 2);

+    }

+

   return inf->card->driver->send (inf->card, nb);

 }

@@ -90,10 +111,25 @@ grub_net_recv_ethernet_packet (struct grub_net_buff *nb,

   grub_net_link_level_address_t hwaddress;

   grub_net_link_level_address_t src_hwaddress;

   grub_err_t err;

+  grub_uint8_t etherhdr_size = sizeof (*eth);

+  grub_uint16_t vlantag = 0;

+

+

+  /* Check if a vlan-tag is present. If so, the ethernet header is 4 bytes */

+  /* longer than the original one. The vlantag id is extracted and the header */

+  /* is reseted to the original size. */

+  if (grub_get_unaligned16 (nb->data + etherhdr_size - 2) == VLANTAG_IDENTIFIER)

+    {

+      vlantag = grub_get_unaligned16 (nb->data + etherhdr_size);

+      etherhdr_size += 4;

+      /* Move eth type to the original position */

+      grub_memcpy((char *) nb->data + etherhdr_size - 6,

+                  (char *) nb->data + etherhdr_size - 2, 2);

+    }

   eth = (struct etherhdr *) nb->data;

   type = grub_be_to_cpu16 (eth->type);

-  err = grub_netbuff_pull (nb, sizeof (*eth));

+  err = grub_netbuff_pull (nb, etherhdr_size);

   if (err)

     return err;

@@ -121,13 +157,14 @@ grub_net_recv_ethernet_packet (struct grub_net_buff *nb,

     {

       /* ARP packet. */

     case GRUB_NET_ETHERTYPE_ARP:

-      grub_net_arp_receive (nb, card);

+      grub_net_arp_receive (nb, card, &vlantag);

       grub_netbuff_free (nb);

       return GRUB_ERR_NONE;

       /* IP packet.  */

     case GRUB_NET_ETHERTYPE_IP:

     case GRUB_NET_ETHERTYPE_IP6:

-      return grub_net_recv_ip_packets (nb, card, &hwaddress, &src_hwaddress);

+      return grub_net_recv_ip_packets (nb, card, &hwaddress, &src_hwaddress,

+                                       &vlantag);

     }

   grub_netbuff_free (nb);

   return GRUB_ERR_NONE;

diff --git a/grub-core/net/ip.c b/grub-core/net/ip.c

index aba4f8908..7c95cc746 100644

--- a/grub-core/net/ip.c

+++ b/grub-core/net/ip.c

@@ -228,12 +228,13 @@ handle_dgram (struct grub_net_buff *nb,

 	      grub_net_ip_protocol_t proto,

 	      const grub_net_network_level_address_t *source,

 	      const grub_net_network_level_address_t *dest,

+              grub_uint16_t *vlantag,

 	      grub_uint8_t ttl)

 {

   struct grub_net_network_level_interface *inf = NULL;

   grub_err_t err;

   int multicast = 0;

-  

+

   /* DHCP needs special treatment since we don't know IP yet.  */

   {

     struct udphdr *udph;

@@ -293,6 +294,15 @@ handle_dgram (struct grub_net_buff *nb,

 	&& grub_net_addr_cmp (&inf->address, dest) == 0

 	&& grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0)

       break;

+

+    /* Verify vlantag id */

+    if (inf->card == card && inf->vlantag != *vlantag)

+      {

+        grub_dprintf ("net", "invalid vlantag! %x != %x\n",

+                      inf->vlantag, *vlantag);

+        break;

+      }

+

     /* Solicited node multicast.  */

     if (inf->card == card

 	&& inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV6

@@ -383,7 +393,8 @@ static grub_err_t

 grub_net_recv_ip4_packets (struct grub_net_buff *nb,

 			   struct grub_net_card *card,

 			   const grub_net_link_level_address_t *hwaddress,

-			   const grub_net_link_level_address_t *src_hwaddress)

+			   const grub_net_link_level_address_t *src_hwaddress,

+                           grub_uint16_t *vlantag)

 {

   struct iphdr *iph = (struct iphdr *) nb->data;

   grub_err_t err;

@@ -458,7 +469,7 @@ grub_net_recv_ip4_packets (struct grub_net_buff *nb,

       dest.ipv4 = iph->dest;

       return handle_dgram (nb, card, src_hwaddress, hwaddress, iph->protocol,

-			   &source, &dest, iph->ttl);

+			   &source, &dest, vlantag, iph->ttl);

     }

   for (prev = &reassembles, rsm = *prev; rsm; prev = &rsm->next, rsm = *prev)

@@ -594,7 +605,7 @@ grub_net_recv_ip4_packets (struct grub_net_buff *nb,

       dest.ipv4 = dst;

       return handle_dgram (ret, card, src_hwaddress,

-			   hwaddress, proto, &source, &dest,

+			   hwaddress, proto, &source, &dest, vlantag,

 			   ttl);

     }

 }

@@ -652,7 +663,8 @@ static grub_err_t

 grub_net_recv_ip6_packets (struct grub_net_buff *nb,

 			   struct grub_net_card *card,

 			   const grub_net_link_level_address_t *hwaddress,

-			   const grub_net_link_level_address_t *src_hwaddress)

+			   const grub_net_link_level_address_t *src_hwaddress,

+                           grub_uint16_t *vlantag)

 {

   struct ip6hdr *iph = (struct ip6hdr *) nb->data;

   grub_err_t err;

@@ -703,21 +715,24 @@ grub_net_recv_ip6_packets (struct grub_net_buff *nb,

   grub_memcpy (dest.ipv6, &iph->dest, sizeof (dest.ipv6));

   return handle_dgram (nb, card, src_hwaddress, hwaddress, iph->protocol,

-		       &source, &dest, iph->ttl);

+		       &source, &dest, vlantag, iph->ttl);

 }

 grub_err_t

 grub_net_recv_ip_packets (struct grub_net_buff *nb,

 			  struct grub_net_card *card,

 			  const grub_net_link_level_address_t *hwaddress,

-			  const grub_net_link_level_address_t *src_hwaddress)

+			  const grub_net_link_level_address_t *src_hwaddress,

+                          grub_uint16_t *vlantag)

 {

   struct iphdr *iph = (struct iphdr *) nb->data;

   if ((iph->verhdrlen >> 4) == 4)

-    return grub_net_recv_ip4_packets (nb, card, hwaddress, src_hwaddress);

+    return grub_net_recv_ip4_packets (nb, card, hwaddress, src_hwaddress,

+                                      vlantag);

   if ((iph->verhdrlen >> 4) == 6)

-    return grub_net_recv_ip6_packets (nb, card, hwaddress, src_hwaddress);

+    return grub_net_recv_ip6_packets (nb, card, hwaddress, src_hwaddress,

+                                      vlantag);

   grub_dprintf ("net", "Bad IP version: %d\n", (iph->verhdrlen >> 4));

   grub_netbuff_free (nb);

   return GRUB_ERR_NONE;

diff --git a/include/grub/net.h b/include/grub/net.h

index 2192fa186..1096b2432 100644

--- a/include/grub/net.h

+++ b/include/grub/net.h

@@ -291,6 +291,7 @@ struct grub_net_network_level_interface

   grub_net_interface_flags_t flags;

   struct grub_net_bootp_packet *dhcp_ack;

   grub_size_t dhcp_acklen;

+  grub_uint16_t vlantag;

   void *data;

 };

@@ -561,4 +562,6 @@ extern char *grub_net_default_server;

 #define GRUB_NET_INTERVAL 400

 #define GRUB_NET_INTERVAL_ADDITION 20

+#define VLANTAG_IDENTIFIER 0x8100

+

 #endif /* ! GRUB_NET_HEADER */

diff --git a/include/grub/net/arp.h b/include/grub/net/arp.h

index bb1703622..8d9d08113 100644

--- a/include/grub/net/arp.h

+++ b/include/grub/net/arp.h

@@ -22,10 +22,11 @@

 #include <grub/net.h>

 extern grub_err_t grub_net_arp_receive (struct grub_net_buff *nb,

-					struct grub_net_card *card);

+                                        struct grub_net_card *card,

+                                        grub_uint16_t *vlantag);

 grub_err_t

 grub_net_arp_send_request (struct grub_net_network_level_interface *inf,

-			   const grub_net_network_level_address_t *proto_addr);

+                           const grub_net_network_level_address_t *proto_addr);

 #endif 

diff --git a/include/grub/net/ip.h b/include/grub/net/ip.h

index dcceaa568..ab9d68f98 100644

--- a/include/grub/net/ip.h

+++ b/include/grub/net/ip.h

@@ -48,7 +48,8 @@ grub_err_t

 grub_net_recv_ip_packets (struct grub_net_buff *nb,

 			  struct grub_net_card *card,

 			  const grub_net_link_level_address_t *hwaddress,

-			  const grub_net_link_level_address_t *src_hwaddress);

+			  const grub_net_link_level_address_t *src_hwaddress,

+                          grub_uint16_t *vlantag);

 grub_err_t

 grub_net_send_ip_packet (struct grub_net_network_level_interface *inf,

-- 

2.14.3