Allow to specify PCP on packets not belonging to any VLAN.

According to 802.1Q-2014, VLAN tagged packets with VLAN id 0 should be
considered as untagged, and only PCP and DEI values from the VLAN tag
are meaningful.  See for instance
https://www.cisco.com/c/en/us/td/docs/switches/connectedgrid/cg-switch-sw-master/software/configuration/guide/vlan0/b_vlan_0.html.

Make it possible to specify PCP value for outgoing packets on an
ethernet interface.  When PCP is supplied, the tag is appended, VLAN
id set to 0, and PCP is filled by the supplied value.  The code to do
VLAN tag encapsulation is refactored from the if_vlan.c and moved into
if_ethersubr.c.

Drivers might have issues with filtering VID 0 packets on
receive.  This bug should be fixed for each driver.

Reviewed by:	ae (previous version), hselasky, melifaro
Sponsored by:	Mellanox Technologies
MFC after:	2 weeks
Differential revision:	https://reviews.freebsd.org/D14702

2 files changed, 97 insertions, 69 deletions

diff --git a/sbin/ifconfig/af_link.c b/sbin/ifconfig/af_link.c
index f83359da0b2b..fe2527ae9680 100644
--- a/sbin/ifconfig/af_link.c
+++ b/sbin/ifconfig/af_link.c
@@ -60,78 +60,78 @@ static void
 link_status(int s __unused, const struct ifaddrs *ifa)
 {
 	/* XXX no const 'cuz LLADDR is defined wrong */
-	struct sockaddr_dl *sdl = (struct sockaddr_dl *) ifa->ifa_addr;
+	struct sockaddr_dl *sdl;
 	char *ether_format, *format_char;
-
-	if (sdl != NULL && sdl->sdl_alen > 0) {
-		if ((sdl->sdl_type == IFT_ETHER ||
-		    sdl->sdl_type == IFT_L2VLAN ||
-		    sdl->sdl_type == IFT_BRIDGE) &&
-		    sdl->sdl_alen == ETHER_ADDR_LEN) {
-			ether_format = ether_ntoa((struct ether_addr *)LLADDR(sdl));
-			if (f_ether != NULL && strcmp(f_ether, "dash") == 0) {
-				for (format_char = strchr(ether_format, ':');
-				    format_char != NULL; 
-				    format_char = strchr(ether_format, ':'))
-					*format_char = '-';
-			}
-			printf("\tether %s\n", ether_format);
-		} else {
-			int n = sdl->sdl_nlen > 0 ? sdl->sdl_nlen + 1 : 0;
-
-			printf("\tlladdr %s\n", link_ntoa(sdl) + n);
-		}
-		/* Best-effort (i.e. failures are silent) to get original
-		 * hardware address, as read by NIC driver at attach time. Only
-		 * applies to Ethernet NICs (IFT_ETHER). However, laggX
-		 * interfaces claim to be IFT_ETHER, and re-type their component
-		 * Ethernet NICs as IFT_IEEE8023ADLAG. So, check for both. If
-		 * the MAC is zeroed, then it's actually a lagg.
-		 */
-		if ((sdl->sdl_type == IFT_ETHER ||
-		    sdl->sdl_type == IFT_IEEE8023ADLAG) &&
-		    sdl->sdl_alen == ETHER_ADDR_LEN) {
-			struct ifreq ifr;
-			int sock_hw;
-			int rc;
-			static const u_char laggaddr[6] = {0};
-
-			strncpy(ifr.ifr_name, ifa->ifa_name,
-			    sizeof(ifr.ifr_name));
-			memcpy(&ifr.ifr_addr, ifa->ifa_addr,
-			    sizeof(ifa->ifa_addr->sa_len));
-			ifr.ifr_addr.sa_family = AF_LOCAL;
-			if ((sock_hw = socket(AF_LOCAL, SOCK_DGRAM, 0)) < 0) {
-				warn("socket(AF_LOCAL,SOCK_DGRAM)");
-				return;
-			}
-			rc = ioctl(sock_hw, SIOCGHWADDR, &ifr);
-			close(sock_hw);
-			if (rc != 0) {
-				return;
-			}
-
-			/*
-			 * If this is definitely a lagg device or the hwaddr
-			 * matches the link addr, don't bother.
-			 */
-			if (memcmp(ifr.ifr_addr.sa_data, laggaddr,
-			    sdl->sdl_alen) == 0 ||
-			    memcmp(ifr.ifr_addr.sa_data, LLADDR(sdl),
-			    sdl->sdl_alen) == 0) {
-				return;
-			}
-			ether_format = ether_ntoa((const struct ether_addr *)
-			    &ifr.ifr_addr.sa_data);
-			if (f_ether != NULL && strcmp(f_ether, "dash") == 0) {
-				for (format_char = strchr(ether_format, ':');
-				    format_char != NULL; 
-				    format_char = strchr(ether_format, ':'))
-					*format_char = '-';
-			}
-			printf("\thwaddr %s\n", ether_format);
+	struct ifreq ifr;
+	int n, rc, sock_hw;
+	static const u_char laggaddr[6] = {0};
+
+	sdl = (struct sockaddr_dl *) ifa->ifa_addr;
+	if (sdl == NULL || sdl->sdl_alen == 0)
+		return;
+
+	if ((sdl->sdl_type == IFT_ETHER || sdl->sdl_type == IFT_L2VLAN ||
+	    sdl->sdl_type == IFT_BRIDGE) && sdl->sdl_alen == ETHER_ADDR_LEN) {
+		ether_format = ether_ntoa((struct ether_addr *)LLADDR(sdl));
+		if (f_ether != NULL && strcmp(f_ether, "dash") == 0) {
+			for (format_char = strchr(ether_format, ':');
+			    format_char != NULL;
+			    format_char = strchr(ether_format, ':'))
+				*format_char = '-';
 		}
+		printf("\tether %s\n", ether_format);
+	} else {
+		n = sdl->sdl_nlen > 0 ? sdl->sdl_nlen + 1 : 0;
+		printf("\tlladdr %s\n", link_ntoa(sdl) + n);
 	}
+
+	/*
+	 * Best-effort (i.e. failures are silent) to get original
+	 * hardware address, as read by NIC driver at attach time. Only
+	 * applies to Ethernet NICs (IFT_ETHER). However, laggX
+	 * interfaces claim to be IFT_ETHER, and re-type their component
+	 * Ethernet NICs as IFT_IEEE8023ADLAG. So, check for both. If
+	 * the MAC is zeroed, then it's actually a lagg.
+	 */
+	if ((sdl->sdl_type != IFT_ETHER &&
+	    sdl->sdl_type != IFT_IEEE8023ADLAG) ||
+	    sdl->sdl_alen != ETHER_ADDR_LEN)
+		return;
+
+	strncpy(ifr.ifr_name, ifa->ifa_name, sizeof(ifr.ifr_name));
+	memcpy(&ifr.ifr_addr, ifa->ifa_addr, sizeof(ifa->ifa_addr->sa_len));
+	ifr.ifr_addr.sa_family = AF_LOCAL;
+	if ((sock_hw = socket(AF_LOCAL, SOCK_DGRAM, 0)) < 0) {
+		warn("socket(AF_LOCAL,SOCK_DGRAM)");
+		return;
+	}
+	rc = ioctl(sock_hw, SIOCGHWADDR, &ifr);
+	close(sock_hw);
+	if (rc != 0)
+		return;
+
+	/*
+	 * If this is definitely a lagg device or the hwaddr
+	 * matches the link addr, don't bother.
+	 */
+	if (memcmp(ifr.ifr_addr.sa_data, laggaddr, sdl->sdl_alen) == 0 ||
+	    memcmp(ifr.ifr_addr.sa_data, LLADDR(sdl), sdl->sdl_alen) == 0)
+		goto pcp;
+
+	ether_format = ether_ntoa((const struct ether_addr *)
+	    &ifr.ifr_addr.sa_data);
+	if (f_ether != NULL && strcmp(f_ether, "dash") == 0) {
+		for (format_char = strchr(ether_format, ':');
+		     format_char != NULL;
+		     format_char = strchr(ether_format, ':'))
+			*format_char = '-';
+	}
+	printf("\thwaddr %s\n", ether_format);
+
+pcp:
+	if (ioctl(s, SIOCGLANPCP, (caddr_t)&ifr) == 0 &&
+	    ifr.ifr_lan_pcp != IFNET_PCP_NONE)
+		printf("\tpcp %d\n", ifr.ifr_lan_pcp);
 }
 
 static void
diff --git a/sbin/ifconfig/ifconfig.c b/sbin/ifconfig/ifconfig.c
index ca401adc6add..b0a2f17586b5 100644
--- a/sbin/ifconfig/ifconfig.c
+++ b/sbin/ifconfig/ifconfig.c
@@ -1080,6 +1080,32 @@ setifmtu(const char *val, int dummy __unused, int s,
 }
 
 static void
+setifpcp(const char *val, int arg __unused, int s, const struct afswtch *afp)
+{
+	u_long ul;
+	char *endp;
+
+	ul = strtoul(val, &endp, 0);
+	if (*endp != '\0')
+		errx(1, "invalid value for pcp");
+	if (ul > 7)
+		errx(1, "value for pcp out of range");
+	ifr.ifr_lan_pcp = ul;
+	if (ioctl(s, SIOCSLANPCP, (caddr_t)&ifr) == -1)
+		err(1, "SIOCSLANPCP");
+}
+
+static void
+disableifpcp(const char *val, int arg __unused, int s,
+    const struct afswtch *afp)
+{
+
+	ifr.ifr_lan_pcp = IFNET_PCP_NONE;
+	if (ioctl(s, SIOCSLANPCP, (caddr_t)&ifr) == -1)
+		err(1, "SIOCSLANPCP");
+}
+
+static void
 setifname(const char *val, int dummy __unused, int s, 
     const struct afswtch *afp)
 {
@@ -1436,6 +1462,8 @@ static struct cmd basic_cmds[] = {
 	DEF_CMD("-txcsum",	-IFCAP_TXCSUM,	setifcap),
 	DEF_CMD("netcons",	IFCAP_NETCONS,	setifcap),
 	DEF_CMD("-netcons",	-IFCAP_NETCONS,	setifcap),
+	DEF_CMD_ARG("pcp",			setifpcp),
+	DEF_CMD("-pcp", 0,			disableifpcp),
 	DEF_CMD("polling",	IFCAP_POLLING,	setifcap),
 	DEF_CMD("-polling",	-IFCAP_POLLING,	setifcap),
 	DEF_CMD("tso6",		IFCAP_TSO6,	setifcap),