From 28903f396af4b151e16ea606cda66a9244fb179f Mon Sep 17 00:00:00 2001
From: Eugene Grosbein <eugen@FreeBSD.org>
Date: Mon, 2 May 2022 21:55:24 +0700
Subject: [PATCH] ng_pppoe: introduce new sysctl net.graph.pppoe.lcp_pcp

This fixes incomplete commit 2e547442ab3822d3d7c46a68f152032ef5fe337c

New sysctl allows to mark transmitted PPPoE LCP Control
ethernet frames with needed 3-bit Priority Code Point (PCP) value.
Confirming driver like if_vlan(4) uses the value to fill
IEEE 802.1p class of service field.

This is similar to Cisco IOS "control-packets vlan cos priority"
command.

It helps to avoid premature disconnection of user sessions
due to control frame drops (LCP Echo etc.)
if network infrastructure has a botteleck at a switch
or the xdsl DSLAM.

See also:
https://sourceforge.net/p/mpd/discussion/44692/thread/c7abe70e3a/

Tested by:      Klaus Fokuhl at SourceForge
MFC after:      2 weeks
---
 share/man/man4/ng_pppoe.4 |  2 +-
 sys/netgraph/ng_pppoe.c   | 22 ++++++++++++++++++++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/share/man/man4/ng_pppoe.4 b/share/man/man4/ng_pppoe.4
index d9853a74651..ff53d4ef3a9 100644
--- a/share/man/man4/ng_pppoe.4
+++ b/share/man/man4/ng_pppoe.4
@@ -322,7 +322,7 @@ control message, when all session have been disconnected or when the
 hook is disconnected.
 .Sh SYSCTL VARIABLES
 The node can mark transmitted LCP Ethernet packets (protocol 0xc021)
-with 3-bit Priority code point (PCP) referring to IEEE 802.1p
+with 3-bit Priority Code Point (PCP) referring to IEEE 802.1p
 class of service with following
 .Xr sysctl 8
 variable.
diff --git a/sys/netgraph/ng_pppoe.c b/sys/netgraph/ng_pppoe.c
index 8bc44e16004..5a327f95b93 100644
--- a/sys/netgraph/ng_pppoe.c
+++ b/sys/netgraph/ng_pppoe.c
@@ -49,8 +49,13 @@
 #include <sys/malloc.h>
 #include <sys/errno.h>
 #include <sys/epoch.h>
+#include <sys/socket.h>
+#include <sys/sysctl.h>
 #include <sys/syslog.h>
 #include <net/ethernet.h>
+#include <net/if.h>
+#include <net/if_vlan_var.h>
+#include <net/vnet.h>
 
 #include <netgraph/ng_message.h>
 #include <netgraph/netgraph.h>
@@ -64,8 +69,19 @@ static MALLOC_DEFINE(M_NETGRAPH_PPPOE, "netgraph_pppoe", "netgraph pppoe node");
 #define M_NETGRAPH_PPPOE M_NETGRAPH
 #endif
 
+/* Some PPP protocol numbers we're interested in */
+#define PROT_LCP		0xc021
+
 #define SIGNOFF "session closed"
 
+VNET_DEFINE_STATIC(u_int32_t, ng_pppoe_lcp_pcp) = 0;
+#define V_ng_pppoe_lcp_pcp	VNET(ng_pppoe_lcp_pcp)
+
+SYSCTL_NODE(_net_graph, OID_AUTO, pppoe, CTLFLAG_RW, 0, "PPPoE");
+SYSCTL_UINT(_net_graph_pppoe, OID_AUTO, lcp_pcp,
+	CTLFLAG_VNET | CTLFLAG_RW, &VNET_NAME(ng_pppoe_lcp_pcp), 0,
+	"Set PCP for LCP");
+
 /*
  * This section contains the netgraph method declarations for the
  * pppoe node. These methods define the netgraph pppoe 'type'.
@@ -1438,6 +1454,12 @@ ng_pppoe_rcvdata(hook_p hook, item_p item)
 			    mtod(m, u_char *)[1] == 0x03)
 				m_adj(m, 2);
 		}
+
+		if (V_ng_pppoe_lcp_pcp && m->m_pkthdr.len >= 2 &&
+		    m->m_len >= 2 && (m = m_pullup(m, 2)) &&
+		    mtod(m, uint16_t *)[0] == htons(PROT_LCP))
+			EVL_APPLY_PRI(m, (uint8_t)(V_ng_pppoe_lcp_pcp & 0x7));
+
 		/*
 		 * Bang in a pre-made header, and set the length up
 		 * to be correct. Then send it to the ethernet driver.