From 3af8ac7172e99a9c11af6f112b431ecdc64138b1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Fatih=20A=C5=9F=C4=B1c=C4=B1?= <fatih.asici@tamara.com.tr>
Date: Wed, 3 Apr 2013 11:27:41 +0300
Subject: [PATCH] Add support for transmitting packets with VLAN headers

If LWIP_HOOK_VLAN_SET is defined, it could be used to set VLAN priority and ID.
Structures for network interface and ethernet header can be used to determine
priority and ID. Similarly, LWIP_HOOK_VLAN_CHECK can be defined to determine
whether the incoming packets will be accepted or not. It is similar to
ETHARP_VLAN_CHECK_FN; but it also gets a pointer to netif structure as the
first argument.
---
 CHANGELOG                  |    9 +++++++++
 src/include/lwip/opt.h     |   30 +++++++++++++++++++++++++++++-
 src/include/netif/etharp.h |    5 +++++
 src/netif/etharp.c         |   43 ++++++++++++++++++++++++++++++++++++++++---
 4 files changed, 83 insertions(+), 4 deletions(-)

diff --git a/CHANGELOG b/CHANGELOG
index 685b568..a910305 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -6,6 +6,15 @@ HISTORY
 
  ++ New features:
 
+  2013-04-03: Fatih Asici
+  * opt.h, etharp.h/.c: Added support for transmitting packets with VLAN
+    headers. If LWIP_HOOK_VLAN_SET is defined, it could be used to set VLAN
+    priority and ID. Structures for network interface and ethernet header can
+    be used to determine priority and ID. Similarly, LWIP_HOOK_VLAN_CHECK
+    can be defined to determine whether the incoming packets will be accepted
+    or not. It is similar to ETHARP_VLAN_CHECK_FN; but it also gets a pointer
+    to netif structure as the first argument.
+
   2012-03-25: Simon Goldschmidt (idea by Mason)
   * posix/*: added posix-compatibility include files posix/netdb.h and posix/sys/socket.h
     which are a simple wrapper to the correct lwIP include files.
diff --git a/src/include/lwip/opt.h b/src/include/lwip/opt.h
index e51f8e5..af60893 100644
--- a/src/include/lwip/opt.h
+++ b/src/include/lwip/opt.h
@@ -459,7 +459,9 @@
 #endif
 
 /**
- * ETHARP_SUPPORT_VLAN==1: support receiving ethernet packets with VLAN header.
+ * ETHARP_SUPPORT_VLAN==1: support receiving and sending ethernet packets with
+ * VLAN header. See the description of LWIP_HOOK_VLAN_CHECK and
+ * LWIP_HOOK_VLAN_SET hooks to check/set VLAN headers.
  * Additionally, you can define ETHARP_VLAN_CHECK to an u16_t VLAN ID to check.
  * If ETHARP_VLAN_CHECK is defined, only VLAN-traffic for this VLAN is accepted.
  * If ETHARP_VLAN_CHECK is not defined, all traffic is accepted.
@@ -1092,7 +1094,11 @@
  * Ethernet.
  */
 #ifndef PBUF_LINK_HLEN
+#ifdef LWIP_HOOK_VLAN_SET
+#define PBUF_LINK_HLEN                  (18 + ETH_PAD_SIZE)
+#else /* LWIP_HOOK_VLAN_SET */
 #define PBUF_LINK_HLEN                  (14 + ETH_PAD_SIZE)
+#endif /* LWIP_HOOK_VLAN_SET */
 #endif
 
 /**
@@ -2127,6 +2133,28 @@
 /* Hooks are undefined by default, define them to a function if you need them. */
 
 /**
+ * LWIP_HOOK_VLAN_CHECK(netif, eth_hdr, vlan_hdr):
+ * - called from ethernet_input() if VLAN support is enabled
+ * - netif: struct netif on which the packet has been received
+ * - eth_hdr: struct eth_hdr of the packet
+ * - vlan_hdr: struct eth_vlan_hdr of the packet
+ * Return values:
+ * - 0: Packet must be dropped.
+ * - != 0: Packet must be accepted.
+ */
+
+/**
+ * LWIP_HOOK_VLAN_SET(netif, eth_hdr, vlan_hdr):
+ * - called from etharp_raw() and etharp_send_ip() if VLAN support is enabled
+ * - netif: struct netif that the packet will be sent through
+ * - eth_hdr: struct eth_hdr of the packet
+ * - vlan_hdr: struct eth_vlan_hdr of the packet
+ * Return values:
+ * - Hook does not return a value.
+ * Hook can be used to set prio_vid field of vlan_hdr.
+ */
+
+/**
  * LWIP_HOOK_IP4_INPUT(pbuf, input_netif):
  * - called from ip_input() (IPv4)
  * - pbuf: received struct pbuf passed to ip_input()
diff --git a/src/include/netif/etharp.h b/src/include/netif/etharp.h
index 92d57ae..1271e0e 100644
--- a/src/include/netif/etharp.h
+++ b/src/include/netif/etharp.h
@@ -129,7 +129,12 @@ PACK_STRUCT_END
 #endif
 
 #define SIZEOF_ETHARP_HDR 28
+
+#ifdef LWIP_HOOK_VLAN_SET
+#define SIZEOF_ETHARP_PACKET (SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR + SIZEOF_ETHARP_HDR)
+#else /* LWIP_HOOK_VLAN_SET */
 #define SIZEOF_ETHARP_PACKET (SIZEOF_ETH_HDR + SIZEOF_ETHARP_HDR)
+#endif /* LWIP_HOOK_VLAN_SET */
 
 /** 5 seconds period */
 #define ARP_TMR_INTERVAL 5000
diff --git a/src/netif/etharp.c b/src/netif/etharp.c
index 1b7eb98..2d5ad70 100644
--- a/src/netif/etharp.c
+++ b/src/netif/etharp.c
@@ -416,12 +416,23 @@ static err_t
 etharp_send_ip(struct netif *netif, struct pbuf *p, struct eth_addr *src, struct eth_addr *dst)
 {
   struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload;
+#ifdef LWIP_HOOK_VLAN_SET
+  struct eth_vlan_hdr *vlanhdr;
+#endif /* LWIP_HOOK_VLAN_SET */
 
   LWIP_ASSERT("netif->hwaddr_len must be the same as ETHARP_HWADDR_LEN for etharp!",
               (netif->hwaddr_len == ETHARP_HWADDR_LEN));
   ETHADDR32_COPY(&ethhdr->dest, dst);
   ETHADDR16_COPY(&ethhdr->src, src);
+#ifdef LWIP_HOOK_VLAN_SET
+  ethhdr->type = PP_HTONS(ETHTYPE_VLAN);
+  vlanhdr = (struct eth_vlan_hdr*)(((u8_t*)ethhdr) + SIZEOF_ETH_HDR);
+  vlanhdr->prio_vid = 0;
+  vlanhdr->tpid = PP_HTONS(ETHTYPE_IP);
+  LWIP_HOOK_VLAN_SET(netif, ethhdr, vlanhdr);
+#else /* LWIP_HOOK_VLAN_SET */
   ethhdr->type = PP_HTONS(ETHTYPE_IP);
+#endif /* LWIP_HOOK_VLAN_SET */
   LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("etharp_send_ip: sending packet %p\n", (void *)p));
   /* send the packet */
   return netif->linkoutput(netif, p);
@@ -888,7 +899,11 @@ etharp_output(struct netif *netif, struct pbuf *q, ip_addr_t *ipaddr)
   LWIP_ASSERT("ipaddr != NULL", ipaddr != NULL);
 
   /* make room for Ethernet header - should not fail */
+#ifdef LWIP_HOOK_VLAN_SET
+  if (pbuf_header(q, sizeof(struct eth_hdr) + SIZEOF_VLAN_HDR) != 0) {
+#else /* LWIP_HOOK_VLAN_SET */
   if (pbuf_header(q, sizeof(struct eth_hdr)) != 0) {
+#endif /* LWIP_HOOK_VLAN_SET */
     /* bail out */
     LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS,
       ("etharp_output: could not allocate room for header.\n"));
@@ -923,7 +938,11 @@ etharp_output(struct netif *netif, struct pbuf *q, ip_addr_t *ipaddr)
         !ip_addr_islinklocal(ipaddr)) {
 #if LWIP_AUTOIP
       struct ip_hdr *iphdr = (struct ip_hdr*)((u8_t*)q->payload +
+#ifdef LWIP_HOOK_VLAN_SET
+        sizeof(struct eth_hdr) + SIZEOF_VLAN_HDR);
+#else /* LWIP_HOOK_VLAN_SET */
         sizeof(struct eth_hdr));
+#endif /* LWIP_HOOK_VLAN_SET */
       /* According to RFC 3297, chapter 2.6.2 (Forwarding Rules), a packet with
          a link-local source address must always be "directly to its destination
          on the same physical link. The host MUST NOT send the packet to any
@@ -1182,6 +1201,9 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr,
   struct pbuf *p;
   err_t result = ERR_OK;
   struct eth_hdr *ethhdr;
+#ifdef LWIP_HOOK_VLAN_SET
+  struct eth_vlan_hdr *vlanhdr;
+#endif /* LWIP_HOOK_VLAN_SET */
   struct etharp_hdr *hdr;
 #if LWIP_AUTOIP
   const u8_t * ethdst_hwaddr;
@@ -1202,7 +1224,12 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr,
               (p->len >= SIZEOF_ETHARP_PACKET));
 
   ethhdr = (struct eth_hdr *)p->payload;
+#ifdef LWIP_HOOK_VLAN_SET
+  vlanhdr = (struct eth_vlan_hdr*)(((u8_t*)ethhdr) + SIZEOF_ETH_HDR);
+  hdr = (struct etharp_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR);
+#else /* LWIP_HOOK_VLAN_SET */
   hdr = (struct etharp_hdr *)((u8_t*)ethhdr + SIZEOF_ETH_HDR);
+#endif /* LWIP_HOOK_VLAN_SET */
   LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("etharp_raw: sending raw ARP packet.\n"));
   hdr->opcode = htons(opcode);
 
@@ -1235,7 +1262,15 @@ etharp_raw(struct netif *netif, const struct eth_addr *ethsrc_addr,
   hdr->hwlen = ETHARP_HWADDR_LEN;
   hdr->protolen = sizeof(ip_addr_t);
 
+#ifdef LWIP_HOOK_VLAN_SET
+  ethhdr->type = PP_HTONS(ETHTYPE_VLAN);
+  vlanhdr->tpid = PP_HTONS(ETHTYPE_ARP);
+  vlanhdr->prio_vid = 0;
+  LWIP_HOOK_VLAN_SET(netif, ethhdr, vlanhdr);
+#else /* LWIP_HOOK_VLAN_SET */
   ethhdr->type = PP_HTONS(ETHTYPE_ARP);
+#endif /* LWIP_HOOK_VLAN_SET */
+
   /* send ARP query */
   result = netif->linkoutput(netif, p);
   ETHARP_STATS_INC(etharp.xmit);
@@ -1310,8 +1345,10 @@ ethernet_input(struct pbuf *p, struct netif *netif)
       ETHARP_STATS_INC(etharp.drop);
       goto free_and_return;
     }
-#if defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) /* if not, allow all VLANs */
-#ifdef ETHARP_VLAN_CHECK_FN
+#if defined(LWIP_HOOK_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) /* if not, allow all VLANs */
+#ifdef LWIP_HOOK_VLAN_CHECK
+    if (!LWIP_HOOK_VLAN_CHECK(netif, ethhdr, vlan)) {
+#elif defined(ETHARP_VLAN_CHECK_FN)
     if (!ETHARP_VLAN_CHECK_FN(ethhdr, vlan)) {
 #elif defined(ETHARP_VLAN_CHECK)
     if (VLAN_ID(vlan) != ETHARP_VLAN_CHECK) {
@@ -1320,7 +1357,7 @@ ethernet_input(struct pbuf *p, struct netif *netif)
       pbuf_free(p);
       return ERR_OK;
     }
-#endif /* defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) */
+#endif /* defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) */
     type = vlan->tpid;
     ip_hdr_offset = SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR;
   }
-- 
1.7.10.4

