1From 95aba99f41333ad430496eab2596bc8b489ae731 Mon Sep 17 00:00:00 2001
2From: Dirk Ziegelmeier <dirk@ziegelmeier.net>
3Date: Fri, 19 Oct 2018 22:30:17 +0200
4Subject: [PATCH] Implement task #11620: Add outgoing VLAN PCP support for
5 Ethernet level QoS
6
7Apply rebased patch from Timmy Brolin
8---
9 src/core/tcp.c           | 29 ++++++++++++++++-------------
10 src/core/tcp_in.c        |  3 +++
11 src/include/lwip/netif.h | 22 ++++++++++++++--------
12 src/include/lwip/opt.h   | 34 +++++++++++++++++++++++-----------
13 src/netif/ethernet.c     | 12 ++++++++++--
14 5 files changed, 66 insertions(+), 34 deletions(-)
15
16diff --git a/src/core/tcp.c b/src/core/tcp.c
17index ce03c8161..1f91d24ba 100644
18--- a/src/core/tcp.c
19+++ b/src/core/tcp.c
20@@ -892,6 +892,9 @@ tcp_listen_with_backlog_and_err(struct tcp_pcb *pcb, u8_t backlog, err_t *err)
21   lpcb->ttl = pcb->ttl;
22   lpcb->tos = pcb->tos;
23
24+#if LWIP_VLAN_PCP
25+  lpcb->netif_hints.tci = pcb->netif_hints.tci;
26+#endif /* LWIP_VLAN_PCP */
27 #if GAZELLE_TCP_REUSE_IPPORT
28   lpcb->connect_num = 0;
29   lpcb->next_same_port_pcb = NULL;
30index 428a6f48d..d1fe067a4 100644
31--- a/src/core/tcp_in.c
32+++ b/src/core/tcp_in.c
33@@ -690,6 +690,9 @@ tcp_listen_input(struct tcp_pcb_listen *pcb)
34 #if LWIP_CALLBACK_API || TCP_LISTEN_BACKLOG
35     npcb->listener = pcb;
36 #endif /* LWIP_CALLBACK_API || TCP_LISTEN_BACKLOG */
37+#if LWIP_VLAN_PCP
38+    npcb->netif_hints.tci = pcb->netif_hints.tci;
39+#endif /* LWIP_VLAN_PCP */
40     /* inherit socket options */
41     npcb->so_options = pcb->so_options & SOF_INHERITED;
42     npcb->netif_idx = pcb->netif_idx;
43diff --git a/src/include/lwip/netif.h b/src/include/lwip/netif.h
44index 9e2007a64..013a69b5a 100644
45--- a/src/include/lwip/netif.h
46+++ b/src/include/lwip/netif.h
47@@ -248,14 +248,20 @@ typedef u8_t netif_addr_idx_t;
48 #define NETIF_ADDR_IDX_MAX 0x7F
49 #endif
50 
51+#if LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP
52+ #define LWIP_NETIF_USE_HINTS              1
53+ struct netif_hint {
54 #if LWIP_NETIF_HWADDRHINT
55-#define LWIP_NETIF_USE_HINTS              1
56-struct netif_hint {
57-  netif_addr_idx_t addr_hint;
58-};
59-#else /* LWIP_NETIF_HWADDRHINT */
60-#define LWIP_NETIF_USE_HINTS              0
61-#endif /* LWIP_NETIF_HWADDRHINT */
62+   u8_t addr_hint;
63+#endif
64+#if LWIP_VLAN_PCP
65+  /** VLAN hader is set if this is >= 0 (but must be <= 0xFFFF) */
66+  s32_t tci;
67+#endif
68+ };
69+#else /* LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP */
70+ #define LWIP_NETIF_USE_HINTS              0
71+#endif /* LWIP_NETIF_HWADDRHINT || LWIP_VLAN_PCP*/
72 
73 /** Generic data structure used for all lwIP network interfaces.
74  *  The following fields should be filled in by the initialization
75 #if LWIP_IPV6_AUTOCONFIG
76diff --git a/src/include/lwip/opt.h b/src/include/lwip/opt.h
77index 90fce4f05..fb4b10c8b 100644
78--- a/src/include/lwip/opt.h
79+++ b/src/include/lwip/opt.h
80@@ -677,6 +677,18 @@
81 #define ETHARP_SUPPORT_VLAN             0
82 #endif
83 
84+/**
85+ * LWIP_VLAN_PCP==1: Enable outgoing VLAN taggning of frames on a per-PCB basis
86+ * for QoS purposes. With this feature enabled, each PCB has a new variable: "tci".
87+ * (Tag Control Identifier). The TCI contains three fields: VID, CFI and PCP.
88+ * VID is the VLAN ID, which should be set to zero.
89+ * The "CFI" bit is used to enable or disable VLAN tags for the PCB.
90+ * PCP (Priority Code Point) is a 3 bit field used for Ethernet level QoS.
91+ */
92+#ifndef LWIP_VLAN_PCP
93+#define LWIP_VLAN_PCP                   0
94+#endif
95+
96 /** LWIP_ETHERNET==1: enable ethernet support even though ARP might be disabled
97  */
98 #if !defined LWIP_ETHERNET || defined __DOXYGEN__
99@@ -1548,13 +1560,13 @@
100  * link level header. The default is 14, the standard value for
101  * Ethernet.
102  */
103-#if !defined PBUF_LINK_HLEN || defined __DOXYGEN__
104-#if defined LWIP_HOOK_VLAN_SET && !defined __DOXYGEN__
105-#define PBUF_LINK_HLEN                  (18 + ETH_PAD_SIZE)
106-#else /* LWIP_HOOK_VLAN_SET */
107-#define PBUF_LINK_HLEN                  (14 + ETH_PAD_SIZE)
108-#endif /* LWIP_HOOK_VLAN_SET */
109-#endif
110+ #if !defined PBUF_LINK_HLEN || defined __DOXYGEN__
111+#if (defined LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP) && !defined __DOXYGEN__
112+ #define PBUF_LINK_HLEN                  (18 + ETH_PAD_SIZE)
113+#else /* LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP */
114+ #define PBUF_LINK_HLEN                  (14 + ETH_PAD_SIZE)
115+#endif /* LWIP_HOOK_VLAN_SET || LWIP_VLAN_PCP */
116+ #endif
117 
118 /**
119  * PBUF_LINK_ENCAPSULATION_HLEN: the number of bytes that should be allocated
120diff --git a/src/netif/ethernet.c b/src/netif/ethernet.c
121index dd171e280..9e367f8cc 100644
122--- a/src/netif/ethernet.c
123+++ b/src/netif/ethernet.c
124@@ -273,8 +273,16 @@ ethernet_output(struct netif * netif, struct pbuf * p,
125   struct eth_hdr *ethhdr;
126   u16_t eth_type_be = lwip_htons(eth_type);
127 
128-#if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET)
129-  s32_t vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type);
130+#if ETHARP_SUPPORT_VLAN
131+  s32_t vlan_prio_vid;
132+#ifdef LWIP_HOOK_VLAN_SET
133+  vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type);
134+#elif LWIP_VLAN_PCP
135+  vlan_prio_vid = -1;
136+  if (netif->hints && (netif->hints->tci >= 0)) {
137+    vlan_prio_vid = (u16_t)netif->hints->tci;
138+  }
139+#endif
140   if (vlan_prio_vid >= 0) {
141     struct eth_vlan_hdr *vlanhdr;
142 
143
144