[PATCH 4/4] Add vlan-tag support

[Lubomir Rintel]

From: Paulo Flabiano Smorigo <address@hidden>

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:

The VLAN tag is set from vlan-tag variable. Its setting from OpenFirmware will
be submitted in a separate patch.

Link: http://en.wikipedia.org/wiki/IEEE_802.1Q
Link: https://bugzilla.redhat.com/show_bug.cgi?id=871563

address@hidden: Clarified the commit message]
address@hidden: Split platform-dependent and independent parts]
address@hidden: Add Changelog]
---
 ChangeLog                |  6 ++++++
 grub-core/net/ethernet.c | 42 +++++++++++++++++++++++++++++++++++++++---
 include/grub/net.h       |  2 ++
 3 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/ChangeLog b/ChangeLog
index f877a2a..411fc2e 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,5 +1,11 @@
 2014-01-20  Paulo Flabiano Smorigo  <address@hidden>
 
+       * grub-core/net/ethernet.c: Include grub/env.h.
+       (send_ethernet_packet): Set VLAN tag if "vlan-tag" variable is set.
+       * include/grub/net.h: Add VLANTAG_IDENTIFIER define.
+
+2014-01-20  Paulo Flabiano Smorigo  <address@hidden>
+
        * grub-core/net/net.c (grub_net_search_configfile): Add.
        * grub-core/normal/main.c: Include grub/net.h.
        * (grub_cmd_normal): Call grub_net_search_configfile() to look up
diff --git a/grub-core/net/ethernet.c b/grub-core/net/ethernet.c
index c397b1b..faaca67 100644
--- a/grub-core/net/ethernet.c
+++ b/grub-core/net/ethernet.c
@@ -23,6 +23,7 @@
 #include <grub/net/arp.h>
 #include <grub/net/netbuff.h>
 #include <grub/net.h>
+#include <grub/env.h>
 #include <grub/time.h>
 #include <grub/net/arp.h>
 
@@ -56,10 +57,19 @@ send_ethernet_packet (struct 
grub_net_network_level_interface *inf,
 {
   struct etherhdr *eth;
   grub_err_t err;
+  grub_uint32_t vlantag = 0;
+  grub_uint8_t etherhdr_size;
 
-  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));
+  const char *vlantag_text = grub_env_get ("vlan-tag");
+  if (vlantag_text != 0) {
+      etherhdr_size += 4;
+      vlantag = grub_strtoul (vlantag_text, 0, 16);
+  }
+
+  err = grub_netbuff_push (nb, etherhdr_size);
   if (err)
     return err;
   eth = (struct etherhdr *) nb->data;
@@ -76,6 +86,19 @@ send_ethernet_packet (struct 
grub_net_network_level_interface *inf,
        return err;
       inf->card->opened = 1;
     }
+
+  /* Check if a vlan-tag is needed. */
+  if (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, 4);
+    }
+
   return inf->card->driver->send (inf->card, nb);
 }
 
@@ -90,10 +113,23 @@ 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_identifier = 0;
+  grub_memcpy (&vlantag_identifier, nb->data + etherhdr_size - 2, 2);
+
+  /* Check if a vlan-tag is present. */
+  if (vlantag_identifier == VLANTAG_IDENTIFIER)
+    {
+      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;
 
diff --git a/include/grub/net.h b/include/grub/net.h
index 6a1156b..314a610 100644
--- a/include/grub/net.h
+++ b/include/grub/net.h
@@ -535,6 +535,8 @@ extern char *grub_net_default_server;
 #define GRUB_NET_TRIES 40
 #define GRUB_NET_INTERVAL 400
 
+#define VLANTAG_IDENTIFIER 0x8100
+
 grub_err_t
 grub_net_search_configfile (char *config);
 
-- 
1.8.3.1