From abc124be2606ffabe675684408cbf916850c030f Mon Sep 17 00:00:00 2001
From: goldsimon <goldsimon@gmx.de>
Date: Fri, 28 Apr 2017 15:59:41 +0200
Subject: [PATCH] task #11620: Add outgoing VLAN PCP support for Ethernet level
 QoS

---
 src/core/tcp.c           |  3 +++
 src/core/tcp_in.c        |  3 +++
 src/include/lwip/netif.h | 12 +++++++++---
 src/include/lwip/opt.h   | 18 +++++++++++++++---
 src/netif/ethernet.c     | 12 ++++++++++--
 5 files changed, 40 insertions(+), 8 deletions(-)

diff --git a/src/core/tcp.c b/src/core/tcp.c
index d3e0350..98d7575 100644
--- a/src/core/tcp.c
+++ b/src/core/tcp.c
@@ -735,6 +735,9 @@ tcp_listen_with_backlog_and_err(struct tcp_pcb *pcb, u8_t backlog, err_t *err)
   lpcb->so_options = pcb->so_options;
   lpcb->ttl = pcb->ttl;
   lpcb->tos = pcb->tos;
+#if LWIP_VLAN_PCP
+  lpcb->netif_hints.tci = pcb->netif_hints.tci;
+#endif /* LWIP_VLAN_PCP */
 #if LWIP_IPV4 && LWIP_IPV6
   IP_SET_TYPE_VAL(lpcb->remote_ip, pcb->local_ip.type);
 #endif /* LWIP_IPV4 && LWIP_IPV6 */
diff --git a/src/core/tcp_in.c b/src/core/tcp_in.c
index d154f98..fc0aea8 100644
--- a/src/core/tcp_in.c
+++ b/src/core/tcp_in.c
@@ -603,6 +603,9 @@ tcp_listen_input(struct tcp_pcb_listen *pcb)
 #if LWIP_CALLBACK_API || TCP_LISTEN_BACKLOG
     npcb->listener = pcb;
 #endif /* LWIP_CALLBACK_API || TCP_LISTEN_BACKLOG */
+#if LWIP_VLAN_PCP
+    npcb->netif_hints.tci = pcb->netif_hints.tci;
+#endif /* LWIP_VLAN_PCP */
     /* inherit socket options */
     npcb->so_options = pcb->so_options & SOF_INHERITED;
     /* Register the new PCB so that we can begin receiving segments
diff --git a/src/include/lwip/netif.h b/src/include/lwip/netif.h
index fc78729..4f8af53 100644
--- a/src/include/lwip/netif.h
+++ b/src/include/lwip/netif.h
@@ -225,14 +225,20 @@ u8_t netif_alloc_client_data_id(void);
 #define netif_get_client_data(netif, id)       (netif)->client_data[(id)]
 #endif
 
-#if LWIP_NETIF_HWADDRHINT
+#if LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP
 #define LWIP_NETIF_USE_HINTS              1
 struct netif_hint {
+#if LWIP_NETIF_HWADDRHINT
   u8_t addr_hint;
+#endif
+#if LWIP_VLAN_PCP
+  /** VLAN hader is set if this is >= 0 (but must be <= 0xFFFF) */
+  s32_t tci;
+#endif
 };
-#else /* LWIP_NETIF_HWADDRHINT */
+#else /* LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP */
 #define LWIP_NETIF_USE_HINTS              0
-#endif /* LWIP_NETIF_HWADDRHINT */
+#endif /* LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP*/
 
 /** Generic data structure used for all lwIP network interfaces.
  *  The following fields should be filled in by the initialization
diff --git a/src/include/lwip/opt.h b/src/include/lwip/opt.h
index 8ee4f3b..b11f2d7 100644
--- a/src/include/lwip/opt.h
+++ b/src/include/lwip/opt.h
@@ -615,6 +615,18 @@
 #define ETHARP_SUPPORT_VLAN             0
 #endif
 
+/**
+ * LWIP_VLAN_PCP==1: Enable outgoing VLAN taggning of frames on a per-PCB basis
+ * for QoS purposes. With this feature enabled, each PCB has a new variable: "tci".
+ * (Tag Control Identifier). The TCI contains three fields: VID, CFI and PCP.
+ * VID is the VLAN ID, which should be set to zero.
+ * The "CFI" bit is used to enable or disable VLAN tags for the PCB.
+ * PCP (Priority Code Point) is a 3 bit field used for Ethernet level QoS.
+ */
+#ifndef LWIP_VLAN_PCP
+#define LWIP_VLAN_PCP                   0
+#endif
+
 /** LWIP_ETHERNET==1: enable ethernet support even though ARP might be disabled
  */
 #if !defined LWIP_ETHERNET || defined __DOXYGEN__
@@ -1401,11 +1413,11 @@
  * Ethernet.
  */
 #if !defined PBUF_LINK_HLEN || defined __DOXYGEN__
-#if defined LWIP_HOOK_VLAN_SET && !defined __DOXYGEN__
+#if (defined LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP) && !defined __DOXYGEN__
 #define PBUF_LINK_HLEN                  (18 + ETH_PAD_SIZE)
-#else /* LWIP_HOOK_VLAN_SET */
+#else /* LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP */
 #define PBUF_LINK_HLEN                  (14 + ETH_PAD_SIZE)
-#endif /* LWIP_HOOK_VLAN_SET */
+#endif /* LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP */
 #endif
 
 /**
diff --git a/src/netif/ethernet.c b/src/netif/ethernet.c
index 7270cfd..60f40e9 100644
--- a/src/netif/ethernet.c
+++ b/src/netif/ethernet.c
@@ -272,8 +272,16 @@ ethernet_output(struct netif* netif, struct pbuf* p,
   struct eth_hdr* ethhdr;
   u16_t eth_type_be = lwip_htons(eth_type);
 
-#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET)
-  s32_t vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type);
+#if ETHARP_SUPPORT_VLAN
+  s32_t vlan_prio_vid;
+#ifdef LWIP_HOOK_VLAN_SET
+  vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type);
+#elif LWIP_VLAN_PCP
+  vlan_prio_vid = -1;
+  if (netif->hints && (netif->hints->tci >= 0)) {
+    vlan_prio_vid = (u16_t)netif->hints->tci;
+  }
+#endif
   if (vlan_prio_vid >= 0) {
     struct eth_vlan_hdr* vlanhdr;
 
-- 
1.9.5.msysgit.0

