autobuild_mac80211_release/target/linux/mediatek/patches-5.4/999-3016-flow-offload-add-mtkhnat-dscp.patch

From e3e7531c6a986b0f033a1a346dd6ac8ed86bad11 Mon Sep 17 00:00:00 2001
From: Bo-Cun Chen <bc-bocun.chen@mediatek.com>
Date: Tue, 13 Jun 2023 17:12:50 +0800
Subject: [PATCH] 999-3016-flow-offload-add-mtkhnat-dscp

---
 drivers/net/ethernet/mediatek/mtk_ppe.c       | 11 +++++++
 drivers/net/ethernet/mediatek/mtk_ppe.h       |  2 ++
 .../net/ethernet/mediatek/mtk_ppe_offload.c   | 12 +++++++
 include/net/netfilter/nf_flow_table.h         |  2 ++
 net/netfilter/nf_flow_table_offload.c         |  7 +++-
 net/netfilter/xt_FLOWOFFLOAD.c                | 32 +++++++++++++++++++
 6 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.c b/drivers/net/ethernet/mediatek/mtk_ppe.c
index 0d7b83a..71b59ed 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.c
@@ -508,6 +508,17 @@ int mtk_foe_entry_set_queue(struct mtk_eth *eth, struct mtk_foe_entry *entry,
 	return 0;
 }
 
+int mtk_foe_entry_set_dscp(struct mtk_eth *eth, struct mtk_foe_entry *entry,
+			   unsigned int dscp)
+{
+	u32 *ib2 = mtk_foe_entry_ib2(eth, entry);
+
+	*ib2 &= ~MTK_FOE_IB2_DSCP;
+	*ib2 |= FIELD_PREP(MTK_FOE_IB2_DSCP, dscp);
+
+	return 0;
+}
+
 static int
 mtk_flow_entry_match_len(struct mtk_eth *eth, struct mtk_foe_entry *entry)
 {
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.h b/drivers/net/ethernet/mediatek/mtk_ppe.h
index ba71884..d5f0aa7 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe.h
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.h
@@ -418,6 +418,8 @@ int mtk_foe_entry_set_wdma(struct mtk_eth *eth, struct mtk_foe_entry *entry,
 			   int wdma_idx, int txq, int bss, int wcid);
 int mtk_foe_entry_set_queue(struct mtk_eth *eth, struct mtk_foe_entry *entry,
 			    unsigned int queue);
+int mtk_foe_entry_set_dscp(struct mtk_eth *eth, struct mtk_foe_entry *entry,
+			   unsigned int dscp);
 int mtk_foe_entry_commit(struct mtk_ppe *ppe, struct mtk_flow_entry *entry);
 void mtk_foe_entry_clear(struct mtk_ppe *ppe, struct mtk_flow_entry *entry);
 void mtk_foe_entry_get_stats(struct mtk_ppe *ppe, struct mtk_flow_entry *entry,
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
index 43371f3..cb2bfac 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
@@ -254,6 +254,7 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f,
 	int wed_index = -1;
 	u16 addr_type = 0;
 	u8 l4proto = 0;
+	u8 dscp = 0;
 	int err = 0;
 	int i;
 
@@ -290,6 +291,15 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f,
 		return -EOPNOTSUPP;
 	}
 
+	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_IP)) {
+		struct flow_match_ip match;
+
+		flow_rule_match_ip(rule, &match);
+		dscp = match.key->tos;
+	} else {
+		return -EOPNOTSUPP;
+	}
+
 	switch (addr_type) {
 	case 0:
 		offload_type = MTK_PPE_PKT_TYPE_BRIDGE;
@@ -448,6 +458,8 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f,
 	if (data.pppoe.num == 1)
 		mtk_foe_entry_set_pppoe(eth, &foe, data.pppoe.sid);
 
+	mtk_foe_entry_set_dscp(eth, &foe, dscp);
+
 	err = mtk_flow_set_output_device(eth, &foe, odev, f->flow->ct, data.eth.h_dest,
 					 &wed_index);
 	if (err)
diff --git a/include/net/netfilter/nf_flow_table.h b/include/net/netfilter/nf_flow_table.h
index 7cf8976..8a84de3 100644
--- a/include/net/netfilter/nf_flow_table.h
+++ b/include/net/netfilter/nf_flow_table.h
@@ -36,6 +36,7 @@ struct nf_flow_key {
 	};
 	struct flow_dissector_key_tcp			tcp;
 	struct flow_dissector_key_ports			tp;
+	struct flow_dissector_key_ip			ip;
 } __aligned(BITS_PER_LONG / 8); /* Ensure that we can do comparisons as longs. */
 
 struct nf_flow_match {
@@ -145,6 +146,7 @@ struct flow_offload_tuple {
 			u8		h_dest[ETH_ALEN];
 		} out;
 	};
+	u8				tos;
 };
 
 struct flow_offload_tuple_rhash {
diff --git a/net/netfilter/nf_flow_table_offload.c b/net/netfilter/nf_flow_table_offload.c
index 2927dbc..5593e28 100644
--- a/net/netfilter/nf_flow_table_offload.c
+++ b/net/netfilter/nf_flow_table_offload.c
@@ -104,6 +104,7 @@ static int nf_flow_rule_match(struct nf_flow_match *match,
 	NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6);
 	NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_TCP, tcp);
 	NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_PORTS, tp);
+	NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_IP, ip);
 
 	if (other_dst && other_dst->lwtstate) {
 		tun_info = lwt_tun_info(other_dst->lwtstate);
@@ -183,10 +184,14 @@ static int nf_flow_rule_match(struct nf_flow_match *match,
 	key->tp.dst = tuple->dst_port;
 	mask->tp.dst = 0xffff;
 
+	key->ip.tos = tuple->tos;
+	mask->ip.tos = 0xff;
+
 	match->dissector.used_keys |= BIT(FLOW_DISSECTOR_KEY_META) |
 				      BIT(FLOW_DISSECTOR_KEY_CONTROL) |
 				      BIT(FLOW_DISSECTOR_KEY_BASIC) |
-				      BIT(FLOW_DISSECTOR_KEY_PORTS);
+				      BIT(FLOW_DISSECTOR_KEY_PORTS) |
+				      BIT(FLOW_DISSECTOR_KEY_IP);
 	return 0;
 }
 
diff --git a/net/netfilter/xt_FLOWOFFLOAD.c b/net/netfilter/xt_FLOWOFFLOAD.c
index 3437d6a..c459b7a 100644
--- a/net/netfilter/xt_FLOWOFFLOAD.c
+++ b/net/netfilter/xt_FLOWOFFLOAD.c
@@ -49,6 +49,35 @@ static DEFINE_SPINLOCK(hooks_lock);
 
 struct xt_flowoffload_table flowtable[2];
 
+static int
+xt_flowoffload_dscp_init(struct sk_buff *skb, struct flow_offload *flow,
+			 enum ip_conntrack_dir dir)
+{
+	const struct flow_offload_tuple *flow_tuple = &flow->tuplehash[dir].tuple;
+	struct iphdr *iph;
+	struct ipv6hdr *ip6h;
+	u32 offset = 0;
+	u8 tos = 0;
+
+	switch (flow_tuple->l3proto) {
+	case NFPROTO_IPV4:
+		iph = (struct iphdr *)(skb_network_header(skb) + offset);
+		tos = iph->tos;
+		break;
+	case NFPROTO_IPV6:
+		ip6h = (struct ipv6hdr *)(skb_network_header(skb) + offset);
+		tos = ipv6_get_dsfield(ip6h);
+		break;
+	default:
+		return -1;
+	};
+
+	flow->tuplehash[dir].tuple.tos = tos;
+	flow->tuplehash[!dir].tuple.tos = tos;
+
+	return 0;
+}
+
 static unsigned int
 xt_flowoffload_net_hook(void *priv, struct sk_buff *skb,
 			const struct nf_hook_state *state)
@@ -617,6 +646,9 @@ flowoffload_tg(struct sk_buff *skb, const struct xt_action_param *par)
 	if (flow_offload_route_init(flow, &route) < 0)
 		goto err_flow_add;
 
+	if (xt_flowoffload_dscp_init(skb, flow, dir) < 0)
+		goto err_flow_add;
+
 	if (tcph) {
 		ct->proto.tcp.seen[0].flags |= IP_CT_TCP_FLAG_BE_LIBERAL;
 		ct->proto.tcp.seen[1].flags |= IP_CT_TCP_FLAG_BE_LIBERAL;
-- 
2.18.0