add feature dscp

1 files changed, 48 insertions, 0 deletions

diff --git a/common/src/raw_packet.cpp b/common/src/raw_packet.cpp
index 03be2b1..83dd0f0 100644
--- a/common/src/raw_packet.cpp
+++ b/common/src/raw_packet.cpp
@@ -1,5 +1,6 @@
 #include <assert.h>
 #include <errno.h>
+#include <linux/if_ether.h>
 #include <string.h>
 #include <stdlib.h>
 
@@ -1056,4 +1057,51 @@ static const void *parse_mpls(struct raw_pkt_parser *handler, const void *data,
         // TESTED
         return parse_mpls(handler, data_next_layer, data_next_length, LAYER_TYPE_MPLS);
     }
+}
+
+static unsigned short ip_checksum(struct iphdr *iph) {
+    // 初始化检验和为0
+    unsigned short checksum = 0;
+    // 将IP头的指针转换为无符号短整型的指针
+    unsigned short *p = (unsigned short *)iph;
+    // 计算IP头的长度，单位为字节
+    int len = iph->ihl * 4;
+    // 逐个累加IP头的每个16位字段
+    while (len > 1) {
+        checksum += *p++;
+        len -= 2;
+    }
+    // 如果IP头的长度为奇数，将最后一个字节视为高8位，低8位补0，再累加
+    if (len == 1) {
+        checksum += *(unsigned char *)p << 8;
+    }
+    // 将检验和的高16位和低16位相加，直到没有进位
+    while (checksum >> 16) {
+        checksum = (checksum & 0xFFFF) + (checksum >> 16);
+    }
+    // 将检验和取反
+    checksum = ~checksum;
+    // 返回检验和
+    return checksum;
+}
+
+void raw_packet_set_dscp(struct ethhdr *eth_hdr, uint8_t dscp)
+{
+    if (eth_hdr->h_proto == htons(ETH_P_IP))
+    {
+        struct iphdr *ip_hdr = (struct iphdr *)(eth_hdr + 1);
+        ip_hdr->tos = (ip_hdr->tos & 0x03) | (dscp << 2);
+        //recalculate checksum
+        ip_hdr->check = ip_checksum(ip_hdr);
+    } 
+    else if (eth_hdr->h_proto == htons(ETH_P_IPV6))
+    {
+        struct ip6_hdr *ip6_hdr = (struct ip6_hdr *)(eth_hdr + 1);
+        unsigned int flow = ntohl(ip6_hdr->ip6_flow);
+        flow &= 0xF03FFFFF;
+        flow |= (dscp << 22);
+        ip6_hdr->ip6_flow = htonl(flow);
+    }
+
+    return;
 }
\ No newline at end of file