253 lines
10 KiB
C++
253 lines
10 KiB
C++
|
#include <time.h>
|
||
|
|
|
||
|
|
#include "packet_build.h"
|
||
|
|
#include "checksum.h"
|
||
|
|
#include "tcp_utils.h"
|
||
|
|
#include "udp_utils.h"
|
||
|
|
#include "ipv4_utils.h"
|
||
|
|
#include "ipv6_utils.h"
|
||
|
|
#include "packet_priv.h"
|
||
|
|
#include "packet_utils.h"
|
||
|
|
|
||
|
|
#define PACKET_BUILD_LOG_DEBUG(format, ...) LOG_DEBUG("packet build", format, ##__VA_ARGS__)
|
||
|
|
#define PACKET_BUILD_LOG_ERROR(format, ...) LOG_ERROR("packet build", format, ##__VA_ARGS__)
|
||
|
|
|
||
|
|
struct packet_fingerprint
|
||
|
|
{
|
||
|
|
// TODO
|
||
|
|
uint16_t ip_id;
|
||
|
|
uint8_t ip_ttl;
|
||
|
|
uint16_t tcp_win;
|
||
|
|
};
|
||
|
|
|
||
|
|
static inline void calc_packet_fingerprint(struct packet_fingerprint *finger)
|
||
|
|
{
|
||
|
|
#define RANGE(rand, start, end) (start + rand % (end - start + 1)) // [start, end]
|
||
|
|
struct timespec time;
|
||
|
|
clock_gettime(CLOCK_MONOTONIC, &time);
|
||
|
|
uint64_t random = 0x013579ABCDEF ^ time.tv_nsec;
|
||
|
|
finger->ip_id = (uint16_t)(RANGE(random, 32767, 65535));
|
||
|
|
finger->ip_ttl = (uint8_t)(RANGE(random, 48, 120));
|
||
|
|
finger->tcp_win = (uint16_t)(RANGE(random, 1000, 1460));
|
||
|
|
}
|
||
|
|
|
||
|
|
static void update_tcp_hdr(struct tcphdr *tcphdr, uint32_t seq, uint32_t ack, uint16_t win, uint8_t flags)
|
||
|
|
{
|
||
|
|
tcp_hdr_set_seq(tcphdr, seq);
|
||
|
|
tcp_hdr_set_ack(tcphdr, ack);
|
||
|
|
tcp_hdr_set_hdr_len(tcphdr, sizeof(struct tcphdr));
|
||
|
|
tcp_hdr_set_flags(tcphdr, flags);
|
||
|
|
tcp_hdr_set_window(tcphdr, win);
|
||
|
|
tcp_hdr_set_urg_ptr(tcphdr, 0);
|
||
|
|
tcp_hdr_set_checksum(tcphdr, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void update_udp_hdr(struct udphdr *udphdr, int trim)
|
||
|
|
{
|
||
|
|
uint16_t total = udp_hdr_get_total_len(udphdr);
|
||
|
|
udp_hdr_set_total_len(udphdr, total - trim);
|
||
|
|
udp_hdr_set_checksum(udphdr, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void update_ip4_hdr(struct ip *iphdr, uint16_t ipid, uint8_t ttl, int trim)
|
||
|
|
{
|
||
|
|
int hdr_len = ipv4_hdr_get_hdr_len(iphdr);
|
||
|
|
uint16_t total = ipv4_hdr_get_total_len(iphdr);
|
||
|
|
ipv4_hdr_set_total_len(iphdr, total - trim);
|
||
|
|
ipv4_hdr_set_ipid(iphdr, ipid);
|
||
|
|
ipv4_hdr_set_ttl(iphdr, ttl);
|
||
|
|
iphdr->ip_sum = 0;
|
||
|
|
iphdr->ip_sum = checksum((const char *)iphdr, hdr_len);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void update_ip6_hdr(struct ip6_hdr *ip6hdr, int trim)
|
||
|
|
{
|
||
|
|
uint16_t len = ipv6_hdr_get_payload_len(ip6hdr);
|
||
|
|
ipv6_hdr_set_payload_len(ip6hdr, len - trim);
|
||
|
|
}
|
||
|
|
|
||
|
|
struct packet *imitate_tcp_packet(const struct packet *origin_pkt, uint32_t tcp_seq, uint32_t tcp_ack, uint8_t tcp_flags, const char *tcp_payload, uint16_t tcp_payload_len)
|
||
|
|
{
|
||
|
|
// check arguments
|
||
|
|
if (origin_pkt == NULL || (tcp_payload == NULL && tcp_payload_len != 0) || (tcp_payload != NULL && tcp_payload_len == 0))
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate TCP packet failed, invalid arguments");
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
// check the innermost layer of the original packet
|
||
|
|
int layers = packet_get_layer_count(origin_pkt);
|
||
|
|
const struct raw_layer *tcp_layer = packet_get_raw_layer(origin_pkt, layers - 1);
|
||
|
|
if (tcp_layer == NULL || tcp_layer->proto != LAYER_PROTO_TCP)
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate TCP packet failed, the innermost layer of the original packet is not TCP");
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
// calculate the new packet length
|
||
|
|
int trim = tcp_layer->hdr_len + tcp_layer->pld_len - tcp_payload_len - sizeof(struct tcphdr);
|
||
|
|
uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim;
|
||
|
|
struct packet *new_pkt = packet_new(new_pkt_len);
|
||
|
|
if (new_pkt == NULL)
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate TCP packet 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);
|
||
|
|
memcpy(new_pkt_data, packet_get_raw_data(origin_pkt), tcp_layer->hdr_offset + sizeof(struct tcphdr));
|
||
|
|
memcpy(new_pkt_data + tcp_layer->hdr_offset + sizeof(struct tcphdr), tcp_payload, tcp_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;
|
||
|
|
struct raw_layer *curr_layer = NULL;
|
||
|
|
struct raw_layer *last_layer = NULL;
|
||
|
|
struct packet_fingerprint finger = {0};
|
||
|
|
calc_packet_fingerprint(&finger);
|
||
|
|
for (int i = layers - 1; i >= 0; i--)
|
||
|
|
{
|
||
|
|
curr_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i);
|
||
|
|
switch (curr_layer->proto)
|
||
|
|
{
|
||
|
|
case LAYER_PROTO_TCP:
|
||
|
|
tcp_hdr = (struct tcphdr *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
update_tcp_hdr(tcp_hdr, tcp_seq, tcp_ack, finger.tcp_win, tcp_flags);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_UDP:
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
update_udp_hdr(udp_hdr, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_IPV4:
|
||
|
|
ip4_hdr = (struct ip *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
|
||
|
|
if (last_layer->proto == LAYER_PROTO_TCP)
|
||
|
|
{
|
||
|
|
tcp_hdr = (struct tcphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
tcp_hdr->th_sum = checksum_v4(tcp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip4_hdr->ip_src, &ip4_hdr->ip_dst);
|
||
|
|
}
|
||
|
|
if (last_layer->proto == LAYER_PROTO_UDP)
|
||
|
|
{
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
udp_hdr->uh_sum = checksum_v4(udp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip4_hdr->ip_src, &ip4_hdr->ip_dst);
|
||
|
|
}
|
||
|
|
update_ip4_hdr(ip4_hdr, finger.ip_id, finger.ip_ttl, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_IPV6:
|
||
|
|
ip6_hdr = (struct ip6_hdr *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
|
||
|
|
if (last_layer->proto == LAYER_PROTO_TCP)
|
||
|
|
{
|
||
|
|
tcp_hdr = (struct tcphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
tcp_hdr->th_sum = checksum_v6(tcp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip6_hdr->ip6_src, &ip6_hdr->ip6_dst);
|
||
|
|
}
|
||
|
|
if (last_layer->proto == LAYER_PROTO_UDP)
|
||
|
|
{
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
udp_hdr->uh_sum = checksum_v6(udp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip6_hdr->ip6_src, &ip6_hdr->ip6_dst);
|
||
|
|
}
|
||
|
|
update_ip6_hdr(ip6_hdr, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_GRE:
|
||
|
|
// TODO
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
packet_parse(new_pkt, new_pkt_data, new_pkt_len);
|
||
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
||
|
|
new_pkt->meta.origin_ctx = NULL;
|
||
|
|
|
||
|
|
return new_pkt;
|
||
|
|
}
|
||
|
|
|
||
|
|
struct packet *imitate_udp_packet(const struct packet *origin_pkt, const char *udp_payload, uint16_t udp_payload_len)
|
||
|
|
{
|
||
|
|
// check arguments
|
||
|
|
if (origin_pkt == NULL || (udp_payload == NULL && udp_payload_len != 0) || (udp_payload != NULL && udp_payload_len == 0))
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate UDP packet failed, invalid arguments");
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
// check the innermost layer of the original packet
|
||
|
|
int layers = packet_get_layer_count(origin_pkt);
|
||
|
|
const struct raw_layer *udp_layer = packet_get_raw_layer(origin_pkt, layers - 1);
|
||
|
|
if (udp_layer == NULL || udp_layer->proto != LAYER_PROTO_UDP)
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate UDP packet failed, the innermost layer of the original packet is not UDP");
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
// calculate the new packet length
|
||
|
|
int trim = udp_layer->hdr_len + udp_layer->pld_len - udp_payload_len - sizeof(struct udphdr);
|
||
|
|
uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim;
|
||
|
|
struct packet *new_pkt = packet_new(new_pkt_len);
|
||
|
|
if (new_pkt == NULL)
|
||
|
|
{
|
||
|
|
PACKET_BUILD_LOG_ERROR("imitate UDP packet 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);
|
||
|
|
memcpy(new_pkt_data, packet_get_raw_data(origin_pkt), udp_layer->hdr_offset + sizeof(struct udphdr));
|
||
|
|
memcpy(new_pkt_data + udp_layer->hdr_offset + sizeof(struct udphdr), udp_payload, udp_payload_len);
|
||
|
|
|
||
|
|
// update the headers of the new packet
|
||
|
|
struct udphdr *udp_hdr = NULL;
|
||
|
|
struct ip *ip4_hdr = NULL;
|
||
|
|
struct ip6_hdr *ip6_hdr = NULL;
|
||
|
|
struct raw_layer *curr_layer = NULL;
|
||
|
|
struct raw_layer *last_layer = NULL;
|
||
|
|
struct packet_fingerprint finger = {0};
|
||
|
|
calc_packet_fingerprint(&finger);
|
||
|
|
for (int i = layers - 1; i >= 0; i--)
|
||
|
|
{
|
||
|
|
curr_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i);
|
||
|
|
switch (curr_layer->proto)
|
||
|
|
{
|
||
|
|
case LAYER_PROTO_UDP:
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
update_udp_hdr(udp_hdr, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_IPV4:
|
||
|
|
ip4_hdr = (struct ip *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
|
||
|
|
if (last_layer->proto == LAYER_PROTO_UDP)
|
||
|
|
{
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
udp_hdr->uh_sum = checksum_v4(udp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip4_hdr->ip_src, &ip4_hdr->ip_dst);
|
||
|
|
}
|
||
|
|
update_ip4_hdr(ip4_hdr, finger.ip_id, finger.ip_ttl, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_IPV6:
|
||
|
|
ip6_hdr = (struct ip6_hdr *)(new_pkt_data + curr_layer->hdr_offset);
|
||
|
|
last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
|
||
|
|
if (last_layer->proto == LAYER_PROTO_UDP)
|
||
|
|
{
|
||
|
|
udp_hdr = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
|
||
|
|
udp_hdr->uh_sum = checksum_v6(udp_hdr, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip6_hdr->ip6_src, &ip6_hdr->ip6_dst);
|
||
|
|
}
|
||
|
|
update_ip6_hdr(ip6_hdr, trim);
|
||
|
|
break;
|
||
|
|
case LAYER_PROTO_GRE:
|
||
|
|
// TODO
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
packet_parse(new_pkt, new_pkt_data, new_pkt_len);
|
||
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
||
|
|
new_pkt->meta.origin_ctx = NULL;
|
||
|
|
|
||
|
|
return new_pkt;
|
||
|
|
}
|