From 72dd3b798e3a08122e9a769d6f0197b0f357478e Mon Sep 17 00:00:00 2001
From: Bo-Cun Chen <bc-bocun.chen@mediatek.com>
Date: Wed, 23 Aug 2023 14:03:52 +0800
Subject: [PATCH] 999-3019-flow-offload-add-mtkhnat-dscp

---
 drivers/net/ethernet/mediatek/mtk_ppe.c       | 11 +++++++
 drivers/net/ethernet/mediatek/mtk_ppe.h       |  1 +
 .../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, 64 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.c b/drivers/net/ethernet/mediatek/mtk_ppe.c
index 4c17a4e..2860e99 100755
--- a/drivers/net/ethernet/mediatek/mtk_ppe.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.c
@@ -435,6 +435,17 @@ int mtk_foe_entry_set_qid(struct mtk_foe_entry *entry, int qid)
 
 	return 0;
 }
+
+int mtk_foe_entry_set_dscp(struct mtk_foe_entry *entry, int dscp)
+{
+	u32 *ib2 = mtk_foe_entry_ib2(entry);
+
+	*ib2 &= ~MTK_FOE_IB2_DSCP;
+	*ib2 |= FIELD_PREP(MTK_FOE_IB2_DSCP, dscp);
+
+	return 0;
+}
+
 static inline bool mtk_foe_entry_usable(struct mtk_foe_entry *entry)
 {
 	return !(entry->ib1 & MTK_FOE_IB1_STATIC) &&
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.h b/drivers/net/ethernet/mediatek/mtk_ppe.h
index 4abed85..57251d3 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe.h
+++ b/drivers/net/ethernet/mediatek/mtk_ppe.h
@@ -430,6 +430,7 @@ int mtk_foe_entry_set_pppoe(struct mtk_foe_entry *entry, int sid);
 int mtk_foe_entry_set_wdma(struct mtk_foe_entry *entry, int wdma_idx, int txq,
 			   int bss, int wcid);
 int mtk_foe_entry_set_qid(struct mtk_foe_entry *entry, int qid);
+int mtk_foe_entry_set_dscp(struct mtk_foe_entry *entry, int dscp);
 bool mtk_foe_entry_match(struct mtk_foe_entry *entry, struct mtk_foe_entry *data);
 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);
diff --git a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
index 989b412..f89875d 100644
--- a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
+++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c
@@ -262,6 +262,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;
 
@@ -298,6 +299,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;
@@ -471,6 +481,8 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f)
 	if (data.pppoe.num == 1)
 		mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid);
 
+	mtk_foe_entry_set_dscp(&foe, dscp);
+
 	mtk_foe_entry_set_sp(eth->ppe[ppe_index], &foe);
 
 	err = mtk_flow_set_output_device(eth, &foe, odev, f->flow->ct, data.eth.h_dest,
diff --git a/include/net/netfilter/nf_flow_table.h b/include/net/netfilter/nf_flow_table.h
index feac793..97a277d 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 ba34572..b8b2fa6 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 1defb15..d4aecab 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