support craft packet from scratch

src/packet/packet_build.cpp

@@ -253,3 +253,120 @@ struct packet *imitate_udp_packet(const struct packet *origin_pkt, const char *u
 
     return new_pkt;
 }
+
+struct packet *craft_packet_from_scratch(const struct layer layers[], uint16_t layer_count, const char *payload, uint16_t payload_len)
+{
+    // check arguments
+    if (layers == NULL || layer_count == 0 || (payload == NULL && payload_len != 0) || (payload != NULL && payload_len == 0))
+    {
+        PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, invalid arguments");
+        return NULL;
+    }
+
+    // calculate the new packet length
+    uint16_t new_pkt_len = 0;
+    for (int i = 0; i < layer_count; i++)
+    {
+        if (layers[i].hdr.raw == NULL || layers[i].hdr_len == 0)
+        {
+            PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, the header of layer %d is invalid", i);
+            return NULL;
+        }
+        new_pkt_len += layers[i].hdr_len;
+    }
+    new_pkt_len += payload_len;
+    struct packet *new_pkt = packet_new(new_pkt_len);
+    if (new_pkt == NULL)
+    {
+        PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, no space to allocate new packet");
+        return NULL;
+    }
+
+    // copy the data to the new packet
+    char *new_pkt_data = (char *)packet_get_raw_data(new_pkt);
+    int offset = 0;
+    for (int i = 0; i < layer_count; i++)
+    {
+        memcpy(new_pkt_data + offset, layers[i].hdr.raw, layers[i].hdr_len);
+        offset += layers[i].hdr_len;
+    }
+    memcpy(new_pkt_data + offset, payload, payload_len);
+
+    // update the headers of the new packet
+    struct tcphdr *tcp_hdr = NULL;
+    struct udphdr *udp_hdr = NULL;
+    struct ip *ip4_hdr = NULL;
+    struct ip6_hdr *ip6_hdr = NULL;
+    // update checksums and lengths
+    uint16_t curr_layer_payload_len = payload_len;
+    for (int i = layer_count - 1; i >= 0; i--)
+    {
+        switch (layers[i].proto)
+        {
+        case LAYER_PROTO_TCP:
+            tcp_hdr = (struct tcphdr *)(new_pkt_data + new_pkt_len - layers[i].hdr_len - curr_layer_payload_len);
+            // update the TCP header
+            tcp_hdr_set_hdr_len(tcp_hdr, layers[i].hdr_len);
+            tcp_hdr_set_checksum(tcp_hdr, 0);
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        case LAYER_PROTO_UDP:
+            udp_hdr = (struct udphdr *)(new_pkt_data + new_pkt_len - layers[i].hdr_len - curr_layer_payload_len);
+            // update the UDP header
+            udp_hdr_set_total_len(udp_hdr, layers[i].hdr_len + curr_layer_payload_len);
+            udp_hdr_set_checksum(udp_hdr, 0);
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        case LAYER_PROTO_IPV4:
+            ip4_hdr = (struct ip *)(new_pkt_data + new_pkt_len - layers[i].hdr_len - curr_layer_payload_len);
+            // update the checksums of the upper layer
+            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_TCP)
+            {
+                tcp_hdr = (struct tcphdr *)(new_pkt_data + new_pkt_len - curr_layer_payload_len);
+                tcp_hdr->th_sum = checksum_v4(tcp_hdr, curr_layer_payload_len, IPPROTO_TCP, &ip4_hdr->ip_src, &ip4_hdr->ip_dst);
+            }
+            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_UDP)
+            {
+                udp_hdr = (struct udphdr *)(new_pkt_data + new_pkt_len - curr_layer_payload_len);
+                udp_hdr->uh_sum = checksum_v4(udp_hdr, curr_layer_payload_len, IPPROTO_UDP, &ip4_hdr->ip_src, &ip4_hdr->ip_dst);
+            }
+            // update the IPv4 header
+            ipv4_hdr_set_hdr_len(ip4_hdr, layers[i].hdr_len);
+            ipv4_hdr_set_total_len(ip4_hdr, layers[i].hdr_len + curr_layer_payload_len);
+            ip4_hdr->ip_sum = 0;
+            ip4_hdr->ip_sum = checksum((const char *)ip4_hdr, layers[i].hdr_len);
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        case LAYER_PROTO_IPV6:
+            ip6_hdr = (struct ip6_hdr *)(new_pkt_data + new_pkt_len - layers[i].hdr_len - curr_layer_payload_len);
+            // update the checksums of the upper layer
+            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_TCP)
+            {
+                tcp_hdr = (struct tcphdr *)(new_pkt_data + new_pkt_len - curr_layer_payload_len);
+                tcp_hdr->th_sum = checksum_v6(tcp_hdr, curr_layer_payload_len, IPPROTO_TCP, &ip6_hdr->ip6_src, &ip6_hdr->ip6_dst);
+            }
+            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_UDP)
+            {
+                udp_hdr = (struct udphdr *)(new_pkt_data + new_pkt_len - curr_layer_payload_len);
+                udp_hdr->uh_sum = checksum_v6(udp_hdr, curr_layer_payload_len, IPPROTO_UDP, &ip6_hdr->ip6_src, &ip6_hdr->ip6_dst);
+            }
+            // update the IPv6 header
+            ipv6_hdr_set_payload_len(ip6_hdr, layers[i].hdr_len + curr_layer_payload_len - sizeof(struct ip6_hdr));
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        case LAYER_PROTO_GRE:
+            // TODO
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        default:
+            curr_layer_payload_len += layers[i].hdr_len;
+            break;
+        }
+    }
+
+    packet_parse(new_pkt, new_pkt_data, new_pkt_len);
+    // no metadata for the new packet from scratch
+    new_pkt->meta.origin_ctx = NULL;
+
+    return new_pkt;
+}