From d89af3ef1c409fb6bb4a1ad6bcd625ae9285913b Mon Sep 17 00:00:00 2001
From: LeoHung <leo.hung@saviah.com>
Date: Tue, 28 May 2024 07:48:42 +0000
Subject: [PATCH] Add ToS/TC in outer IP header.

---
 include/far.h       |  1 +
 include/genl_far.h  |  1 +
 src/genl/genl_far.c | 10 ++++++++++
 src/gtpu/encap.c    |  3 +++
 src/gtpu/pktinfo.c  |  8 +++++++-
 5 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/include/far.h b/include/far.h
index 8cb9208..cb58b7e 100644
--- a/include/far.h
+++ b/include/far.h
@@ -28,6 +28,7 @@ struct outer_header_creation {
     u32 teid;
     struct in_addr peer_addr_ipv4;
     u16 port;
+    u8 tosTc;
 };
 
 struct forwarding_policy {
diff --git a/include/genl_far.h b/include/genl_far.h
index c6f5438..7c0fd3b 100644
--- a/include/genl_far.h
+++ b/include/genl_far.h
@@ -24,6 +24,7 @@ enum gtp5g_forwarding_parameter_attrs {
     GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION = 1,
     GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY,
     GTP5G_FORWARDING_PARAMETER_PFCPSM_REQ_FLAGS,
+    GTP5G_FORWARDING_PARAMETER_TOS_TC,
 
     __GTP5G_FORWARDING_PARAMETER_ATTR_MAX,
 };
diff --git a/src/genl/genl_far.c b/src/genl/genl_far.c
index d090b2a..293b6c2 100644
--- a/src/genl/genl_far.c
+++ b/src/genl/genl_far.c
@@ -454,6 +454,12 @@ static int forwarding_parameter_fill(struct forwarding_parameter *param,
         }
     }
 
+    if (attrs[GTP5G_FORWARDING_PARAMETER_TOS_TC]) {
+        if (param->hdr_creation) {
+            param->hdr_creation->tosTc = nla_get_u8(attrs[GTP5G_FORWARDING_PARAMETER_TOS_TC]);
+        }
+    }
+
     return 0;
 }
 
@@ -578,6 +584,10 @@ static int gtp5g_genl_fill_far(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,
             if (nla_put(skb, GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY, fwd_policy->len, fwd_policy->identifier))
                 goto genlmsg_fail;
 
+        if (fwd_param->hdr_creation) {
+            if (nla_put_u8(skb, GTP5G_FORWARDING_PARAMETER_TOS_TC, hdr_creation->tosTc))
+                goto genlmsg_fail;
+        }
         nla_nest_end(skb, nest_fwd_param);
     }
 
diff --git a/src/gtpu/encap.c b/src/gtpu/encap.c
index 65e471f..d3081be 100644
--- a/src/gtpu/encap.c
+++ b/src/gtpu/encap.c
@@ -795,6 +795,9 @@ static int gtp5g_fwd_skb_encap(struct sk_buff *skb, struct net_device *dev,
 
             iph->saddr = pdr->pdi->f_teid->gtpu_addr_ipv4.s_addr;
             iph->daddr = hdr_creation->peer_addr_ipv4.s_addr;
+            if (hdr_creation->tosTc) {
+                iph->tos = hdr_creation->tosTc;
+            }
             iph->check = 0;
 
             uh = udp_hdr(skb);
diff --git a/src/gtpu/pktinfo.c b/src/gtpu/pktinfo.c
index 48503d1..c627a22 100644
--- a/src/gtpu/pktinfo.c
+++ b/src/gtpu/pktinfo.c
@@ -239,12 +239,18 @@ void gtp5g_fwd_emark_skb_ipv4(struct sk_buff *skb,
 
 void gtp5g_xmit_skb_ipv4(struct sk_buff *skb, struct gtp5g_pktinfo *pktinfo)
 {
+    u8 tos = 0;
+    if (pktinfo->hdr_creation == NULL) {
+        tos = pktinfo->iph->tos;
+    } else {
+        tos = pktinfo->hdr_creation->tosTc;
+    }
     udp_tunnel_xmit_skb(pktinfo->rt, 
         pktinfo->sk,
         skb,
         pktinfo->fl4.saddr,
         pktinfo->fl4.daddr,
-        pktinfo->iph->tos,
+        tos,
         ip4_dst_hoplimit(&pktinfo->rt->dst),
         0,
         pktinfo->gtph_port,