Allow disabling VLAN hardware tag stripping with software work

around. Management firmware(ASF/IPMI/UMP) requires the VLAN
hardware tag stripping so don't actually disable VLAN hardware tag
stripping. If VLAN hardware tag stripping was disabled, bce(4)
manually reconstruct VLAN frame by appending stripped VLAN tag.
Also remove unnecessary IFCAP_VLAN_MTU message.

Reviewed by:	davidch

1 files changed, 40 insertions, 20 deletions

diff --git a/sys/dev/bce/if_bce.c b/sys/dev/bce/if_bce.c
index 94198e8..268c632 100644
--- a/sys/dev/bce/if_bce.c
+++ b/sys/dev/bce/if_bce.c
@@ -5886,6 +5886,7 @@ bce_rx_intr(struct bce_softc *sc)
 {
 	struct ifnet *ifp = sc->bce_ifp;
 	struct l2_fhdr *l2fhdr;
+	struct ether_vlan_header *vh;
 	unsigned int pkt_len;
 	u16 sw_rx_cons, sw_rx_cons_idx, hw_rx_cons;
 	u32 status;
@@ -6141,12 +6142,37 @@ bce_rx_intr(struct bce_softc *sc)
 
 		/* Attach the VLAN tag.	*/
 		if (status & L2_FHDR_STATUS_L2_VLAN_TAG) {
+			if (ifp->if_capenable & IFCAP_VLAN_HWTAGGING) {
 #if __FreeBSD_version < 700000
-			VLAN_INPUT_TAG(ifp, m0, l2fhdr->l2_fhdr_vlan_tag, continue);
+				VLAN_INPUT_TAG(ifp, m0,
+				    l2fhdr->l2_fhdr_vlan_tag, continue);
 #else
-			m0->m_pkthdr.ether_vtag = l2fhdr->l2_fhdr_vlan_tag;
-			m0->m_flags |= M_VLANTAG;
+				m0->m_pkthdr.ether_vtag =
+				    l2fhdr->l2_fhdr_vlan_tag;
+				m0->m_flags |= M_VLANTAG;
 #endif
+			} else {
+				/*
+				 * bce(4) controllers can't disable VLAN
+				 * tag stripping if management firmware
+				 * (ASF/IPMI/UMP) is running. So we always
+				 * strip VLAN tag and manually reconstruct
+				 * the VLAN frame by appending stripped
+				 * VLAN tag in driver if VLAN tag stripping
+				 * was disabled.
+				 *
+				 * TODO: LLC SNAP handling.
+				 */
+				bcopy(mtod(m0, uint8_t *),
+				    mtod(m0, uint8_t *) - ETHER_VLAN_ENCAP_LEN,
+				    ETHER_ADDR_LEN * 2);
+				m0->m_data -= ETHER_VLAN_ENCAP_LEN;
+				vh = mtod(m0, struct ether_vlan_header *);
+				vh->evl_encap_proto = htons(ETHERTYPE_VLAN);
+				vh->evl_tag = htons(l2fhdr->l2_fhdr_vlan_tag);
+				m0->m_pkthdr.len += ETHER_VLAN_ENCAP_LEN;
+				m0->m_len += ETHER_VLAN_ENCAP_LEN;
+			}
 		}
 
 		/* Increment received packet statistics. */
@@ -7096,23 +7122,17 @@ bce_ioctl(struct ifnet *ifp, u_long command, caddr_t data)
 					ifp->if_hwassist = 0;
 			}
 
-			/* Toggle VLAN_MTU capabilities enable flag. */
-			if (mask & IFCAP_VLAN_MTU) {
-				BCE_PRINTF("%s(%d): Changing VLAN_MTU not supported.\n",
-					__FILE__, __LINE__);
-			}
-
-			/* Toggle VLANHWTAG capabilities enabled flag. */
-			if (mask & IFCAP_VLAN_HWTAGGING) {
-				if (sc->bce_flags & BCE_MFW_ENABLE_FLAG)
-					BCE_PRINTF("%s(%d): Cannot change VLAN_HWTAGGING while "
-						"management firmware (ASF/IPMI/UMP) is running!\n",
-						__FILE__, __LINE__);
-				else
-					BCE_PRINTF("%s(%d): Changing VLAN_HWTAGGING not supported!\n",
-						__FILE__, __LINE__);
-			}
-
+			/*
+			 * Don't actually disable VLAN tag stripping as
+			 * management firmware (ASF/IPMI/UMP) requires the
+			 * feature. If VLAN tag stripping is disabled driver
+			 * will manually reconstruct the VLAN frame by
+			 * appending stripped VLAN tag.
+			 */
+			if ((mask & IFCAP_VLAN_HWTAGGING) != 0 &&
+			    (ifp->if_capabilities & IFCAP_VLAN_HWTAGGING))
+				ifp->if_capenable ^= IFCAP_VLAN_HWTAGGING;
+			VLAN_CAPABILITIES(ifp);
 			break;
 		default:
 			/* We don't know how to handle the IOCTL, pass it on. */