437 lines
16 KiB
C
437 lines
16 KiB
C
#include <time.h>
|
|
|
|
#include "checksum.h"
|
|
#include "log_internal.h"
|
|
#include "packet_helper.h"
|
|
#include "packet_parser.h"
|
|
#include "packet_internal.h"
|
|
|
|
#define PACKET_BUILD_LOG_ERROR(format, ...) STELLAR_LOG_ERROR(__thread_local_logger, "packet build", format, ##__VA_ARGS__)
|
|
|
|
struct fingerprint
|
|
{
|
|
// TODO
|
|
uint16_t ip_id;
|
|
uint8_t ip_ttl;
|
|
uint16_t tcp_win;
|
|
};
|
|
|
|
static uint8_t append_fingerprint = 0;
|
|
|
|
/******************************************************************************
|
|
* Private API
|
|
******************************************************************************/
|
|
|
|
static inline void calc_packet_fingerprint(struct fingerprint *finger)
|
|
{
|
|
if (append_fingerprint)
|
|
{
|
|
#define RANGE(rand, start, end) (start + rand % (end - start + 1)) // [start, end]
|
|
struct timespec time;
|
|
clock_gettime(CLOCK_MONOTONIC_COARSE, &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));
|
|
}
|
|
else
|
|
{
|
|
finger->ip_id = 0;
|
|
finger->ip_ttl = 0;
|
|
finger->tcp_win = 0;
|
|
}
|
|
}
|
|
|
|
static void update_udp_hdr(struct udphdr *udp, int trim_len)
|
|
{
|
|
uint16_t total = udp_hdr_get_total_len(udp);
|
|
udp_hdr_set_total_len(udp, total - trim_len);
|
|
udp_hdr_set_checksum(udp, 0);
|
|
}
|
|
|
|
static void update_ip4_hdr(struct ip *ip, uint16_t ipid, uint8_t ttl, int trim_len)
|
|
{
|
|
int hdr_len = ip4_hdr_get_hdr_len(ip);
|
|
uint16_t total = ip4_hdr_get_total_len(ip);
|
|
ip4_hdr_set_total_len(ip, total - trim_len);
|
|
if (ipid)
|
|
{
|
|
ip4_hdr_set_ipid(ip, ipid);
|
|
}
|
|
if (ttl)
|
|
{
|
|
ip4_hdr_set_ttl(ip, ttl);
|
|
}
|
|
ip->ip_sum = 0;
|
|
ip->ip_sum = checksum((const void *)ip, hdr_len);
|
|
}
|
|
|
|
static void update_ip6_hdr(struct ip6_hdr *ip6, int trim_len)
|
|
{
|
|
uint16_t len = ip6_hdr_get_payload_len(ip6);
|
|
ip6_hdr_set_payload_len(ip6, len - trim_len);
|
|
}
|
|
|
|
static void update_gtp1_hdr(struct gtp1_hdr *gtp, int trim_len)
|
|
{
|
|
uint16_t msg_len = gtp1_hdr_get_msg_len(gtp);
|
|
gtp1_hdr_set_msg_len(gtp, msg_len - trim_len);
|
|
if (gtp1_hdr_get_seq_flag(gtp) && gtp1_hdr_get_seq(gtp))
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build packets may be dropped by intermediate devices, the GTPv1 layer requires a sequence number");
|
|
}
|
|
}
|
|
|
|
static void update_gtp2_hdr(struct gtp2_hdr *gtp, int trim_len)
|
|
{
|
|
uint16_t msg_len = gtp2_hdr_get_msg_len(gtp);
|
|
gtp2_hdr_set_msg_len(gtp, msg_len - trim_len);
|
|
if (gtp2_hdr_get_seq(gtp))
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build packets may be dropped by intermediate devices, the GTPv2 layer requires a sequence number");
|
|
}
|
|
}
|
|
|
|
static void update_gre1_hdr(struct gre1_hdr *gre, int trim_len)
|
|
{
|
|
uint16_t payload_len = gre1_hdr_get_payload_length(gre);
|
|
gre1_hdr_set_payload_length(gre, payload_len - trim_len);
|
|
}
|
|
|
|
// L2 -- data link layer
|
|
// LAYER_PROTO_ETHER: // SKIP
|
|
// LAYER_PROTO_PWETH: // SKIP
|
|
// LAYER_PROTO_PPP: // SKIP
|
|
// LAYER_PROTO_L2TP: // TODO ???
|
|
|
|
// L2 -- tunnel
|
|
// LAYER_PROTO_VLAN: // SKIP
|
|
// LAYER_PROTO_PPPOE: // TODO ????
|
|
// LAYER_PROTO_MPLS: // SKIP
|
|
|
|
// L3 -- network layer
|
|
// LAYER_PROTO_IPV4: // DONE
|
|
// LAYER_PROTO_IPV6: // DONE
|
|
// LAYER_PROTO_IPAH: // TODO ????
|
|
|
|
// L3 -- tunnel
|
|
// LAYER_PROTO_GRE: // DONE
|
|
|
|
// L4 -- transport layer
|
|
// LAYER_PROTO_UDP: // DONE
|
|
// LAYER_PROTO_TCP: // DONE
|
|
// LAYER_PROTO_ICMP:
|
|
// LAYER_PROTO_ICMP6:
|
|
|
|
// L4 -- tunnel
|
|
// LAYER_PROTO_VXLAN: // SKIP
|
|
// LAYER_PROTO_GTP_U: // DONE
|
|
// LAYER_PROTO_GTP_C:
|
|
static void calculate_length_and_checksum(const struct packet *origin_pkt, int layer_count, char *new_pkt_data, uint16_t new_pkt_len, int trim_len)
|
|
{
|
|
uint8_t version = 0;
|
|
uint16_t sum = 0;
|
|
char *curr_hdr_ptr = NULL;
|
|
char *last_hdr_ptr = NULL;
|
|
struct tcphdr *tcp = NULL;
|
|
struct udphdr *udp = NULL;
|
|
struct ip *ip4 = NULL;
|
|
struct ip6_hdr *ip6 = NULL;
|
|
struct gtp1_hdr *gtp1 = NULL;
|
|
struct gtp2_hdr *gtp2 = NULL;
|
|
struct gre0_hdr *gre0 = NULL;
|
|
struct gre1_hdr *gre1 = NULL;
|
|
struct layer_internal *curr_layer = NULL;
|
|
struct layer_internal *last_layer = NULL;
|
|
struct fingerprint finger = {};
|
|
calc_packet_fingerprint(&finger);
|
|
for (int i = layer_count - 1; i >= 0; i--)
|
|
{
|
|
curr_layer = (struct layer_internal *)packet_get_layer(origin_pkt, i);
|
|
last_layer = (struct layer_internal *)packet_get_layer(origin_pkt, i + 1);
|
|
curr_hdr_ptr = new_pkt_data + curr_layer->hdr_offset;
|
|
last_hdr_ptr = last_layer ? new_pkt_data + last_layer->hdr_offset : NULL;
|
|
switch (curr_layer->proto)
|
|
{
|
|
case LAYER_PROTO_TCP:
|
|
tcp = (struct tcphdr *)curr_hdr_ptr;
|
|
if (finger.tcp_win)
|
|
{
|
|
tcp_hdr_set_window(tcp, finger.tcp_win);
|
|
}
|
|
tcp_hdr_set_checksum(tcp, 0);
|
|
break;
|
|
case LAYER_PROTO_UDP:
|
|
udp = (struct udphdr *)curr_hdr_ptr;
|
|
update_udp_hdr(udp, trim_len);
|
|
break;
|
|
case LAYER_PROTO_IPV4:
|
|
ip4 = (struct ip *)curr_hdr_ptr;
|
|
if (last_layer && last_layer->proto == LAYER_PROTO_TCP)
|
|
{
|
|
tcp = (struct tcphdr *)last_hdr_ptr;
|
|
tcp->th_sum = checksum_v4(tcp, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip4->ip_src, &ip4->ip_dst);
|
|
}
|
|
if (last_layer && last_layer->proto == LAYER_PROTO_UDP)
|
|
{
|
|
udp = (struct udphdr *)last_hdr_ptr;
|
|
udp->uh_sum = checksum_v4(udp, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip4->ip_src, &ip4->ip_dst);
|
|
}
|
|
update_ip4_hdr(ip4, finger.ip_id, finger.ip_ttl, trim_len);
|
|
break;
|
|
case LAYER_PROTO_IPV6:
|
|
ip6 = (struct ip6_hdr *)curr_hdr_ptr;
|
|
if (last_layer && last_layer->proto == LAYER_PROTO_TCP)
|
|
{
|
|
tcp = (struct tcphdr *)last_hdr_ptr;
|
|
tcp->th_sum = checksum_v6(tcp, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip6->ip6_src, &ip6->ip6_dst);
|
|
}
|
|
if (last_layer && last_layer->proto == LAYER_PROTO_UDP)
|
|
{
|
|
udp = (struct udphdr *)last_hdr_ptr;
|
|
udp->uh_sum = checksum_v6(udp, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip6->ip6_src, &ip6->ip6_dst);
|
|
}
|
|
update_ip6_hdr(ip6, trim_len);
|
|
break;
|
|
case LAYER_PROTO_GTP_C: /* fall through */
|
|
case LAYER_PROTO_GTP_U:
|
|
version = peek_gtp_version(curr_hdr_ptr, curr_layer->hdr_len);
|
|
if (version == 1)
|
|
{
|
|
gtp1 = (struct gtp1_hdr *)curr_hdr_ptr;
|
|
update_gtp1_hdr(gtp1, trim_len);
|
|
}
|
|
if (version == 2)
|
|
{
|
|
gtp2 = (struct gtp2_hdr *)curr_hdr_ptr;
|
|
update_gtp2_hdr(gtp2, trim_len);
|
|
}
|
|
break;
|
|
case LAYER_PROTO_GRE:
|
|
version = peek_gre_version(curr_hdr_ptr, curr_layer->hdr_len);
|
|
if (version == 0)
|
|
{
|
|
gre0 = (struct gre0_hdr *)curr_hdr_ptr;
|
|
if (gre0_hdr_get_checksum_flag(gre0))
|
|
{
|
|
gre0_hdr_set_checksum(gre0, ntohs(0));
|
|
sum = checksum((const void *)curr_hdr_ptr, new_pkt_len - curr_layer->hdr_offset);
|
|
gre0_hdr_set_checksum(gre0, ntohs(sum));
|
|
}
|
|
}
|
|
if (version == 1)
|
|
{
|
|
gre1 = (struct gre1_hdr *)curr_hdr_ptr;
|
|
update_gre1_hdr(gre1, trim_len);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/******************************************************************************
|
|
* Public API
|
|
******************************************************************************/
|
|
|
|
void append_fingerprint_to_build_packet()
|
|
{
|
|
append_fingerprint = 1;
|
|
}
|
|
|
|
/*
|
|
* tcp_seq: the sequence number of the new TCP packet (in host byte order)
|
|
* tcp_ack: the acknowledgment number of the new TCP packet (in host byte order)
|
|
* tcp_options_len: the length of the options (must be a multiple of 4)
|
|
*/
|
|
struct packet *packet_build_tcp(const struct packet *origin_pkt, uint32_t tcp_seq, uint32_t tcp_ack, uint8_t tcp_flags,
|
|
const char *tcp_options, uint16_t tcp_options_len,
|
|
const char *tcp_payload, uint16_t tcp_payload_len)
|
|
{
|
|
// check arguments
|
|
if (origin_pkt == NULL ||
|
|
(tcp_options == NULL && tcp_options_len != 0) || (tcp_options != NULL && tcp_options_len == 0) ||
|
|
(tcp_payload == NULL && tcp_payload_len != 0) || (tcp_payload != NULL && tcp_payload_len == 0) ||
|
|
(tcp_options_len && tcp_options_len % 4 != 0))
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build TCP packet failed, invalid arguments");
|
|
return NULL;
|
|
}
|
|
|
|
// check the innermost layer of the original packet
|
|
int layer_count = packet_get_layer_count(origin_pkt);
|
|
const struct layer_internal *tcp_layer = packet_get_layer(origin_pkt, layer_count - 1);
|
|
if (tcp_layer == NULL || tcp_layer->proto != LAYER_PROTO_TCP)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build TCP packet failed, the innermost layer of the original packet is not TCP");
|
|
return NULL;
|
|
}
|
|
|
|
// calculate the new packet length
|
|
int trim_len = tcp_layer->hdr_len + tcp_layer->pld_len - tcp_options_len - tcp_payload_len - sizeof(struct tcphdr);
|
|
uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim_len;
|
|
struct packet *new_pkt = packet_new(new_pkt_len);
|
|
if (new_pkt == NULL)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build 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));
|
|
if (tcp_options_len)
|
|
{
|
|
memcpy(new_pkt_data + tcp_layer->hdr_offset + sizeof(struct tcphdr), tcp_options, tcp_options_len);
|
|
}
|
|
memcpy(new_pkt_data + tcp_layer->hdr_offset + sizeof(struct tcphdr) + tcp_options_len, tcp_payload, tcp_payload_len);
|
|
struct tcphdr *hdr = (struct tcphdr *)(new_pkt_data + tcp_layer->hdr_offset);
|
|
tcp_hdr_set_seq(hdr, tcp_seq);
|
|
tcp_hdr_set_ack(hdr, tcp_ack);
|
|
tcp_hdr_set_flags(hdr, tcp_flags);
|
|
tcp_hdr_set_hdr_len(hdr, sizeof(struct tcphdr) + tcp_options_len);
|
|
|
|
calculate_length_and_checksum(origin_pkt, layer_count, new_pkt_data, new_pkt_len, trim_len);
|
|
|
|
packet_parse(new_pkt, new_pkt_data, new_pkt_len);
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
struct packet_origin origin = {
|
|
.type = ORIGIN_TYPE_USER,
|
|
.ctx = NULL,
|
|
.cb = NULL,
|
|
.args = NULL,
|
|
.thr_idx = -1,
|
|
};
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
packet_set_origin(new_pkt, &origin);
|
|
|
|
return new_pkt;
|
|
}
|
|
|
|
struct packet *packet_build_udp(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("build UDP packet failed, invalid arguments");
|
|
return NULL;
|
|
}
|
|
|
|
// check the innermost layer of the original packet
|
|
int layer_count = packet_get_layer_count(origin_pkt);
|
|
const struct layer_internal *udp_layer = packet_get_layer(origin_pkt, layer_count - 1);
|
|
if (udp_layer == NULL || udp_layer->proto != LAYER_PROTO_UDP)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build UDP packet failed, the innermost layer of the original packet is not UDP");
|
|
return NULL;
|
|
}
|
|
|
|
// calculate the new packet length
|
|
int trim_len = 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_len;
|
|
struct packet *new_pkt = packet_new(new_pkt_len);
|
|
if (new_pkt == NULL)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build 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);
|
|
|
|
calculate_length_and_checksum(origin_pkt, layer_count, new_pkt_data, new_pkt_len, trim_len);
|
|
|
|
packet_parse(new_pkt, new_pkt_data, new_pkt_len);
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
struct packet_origin origin = {
|
|
.type = ORIGIN_TYPE_USER,
|
|
.ctx = NULL,
|
|
.cb = NULL,
|
|
.args = NULL,
|
|
.thr_idx = -1,
|
|
};
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
packet_set_origin(new_pkt, &origin);
|
|
|
|
return new_pkt;
|
|
}
|
|
|
|
struct packet *packet_build_l3(const struct packet *origin_pkt, uint8_t ip_proto, const char *l3_payload, uint16_t l3_payload_len)
|
|
{
|
|
if (origin_pkt == NULL || (l3_payload == NULL && l3_payload_len != 0) || (l3_payload != NULL && l3_payload_len == 0))
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build L3 packet failed, invalid arguments");
|
|
return NULL;
|
|
}
|
|
|
|
int i = 0;
|
|
int layers = packet_get_layer_count(origin_pkt);
|
|
const struct layer_internal *l3_layer = NULL;
|
|
for (i = layers - 1; i >= 0; i--)
|
|
{
|
|
l3_layer = packet_get_layer(origin_pkt, i);
|
|
if (l3_layer->proto == LAYER_PROTO_IPV4 || l3_layer->proto == LAYER_PROTO_IPV6)
|
|
{
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
l3_layer = NULL;
|
|
}
|
|
}
|
|
if (l3_layer == NULL)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build L3 packet failed, the original packet does not contain an IP layer");
|
|
return NULL;
|
|
}
|
|
|
|
// calculate the new packet length
|
|
// trim IPv4 options
|
|
// trim IPv6 extension headers
|
|
int l3_hdr_len = l3_layer->proto == LAYER_PROTO_IPV4 ? sizeof(struct ip) : sizeof(struct ip6_hdr);
|
|
int trim_len = l3_layer->hdr_len + l3_layer->pld_len - l3_payload_len - l3_hdr_len;
|
|
uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim_len;
|
|
struct packet *new_pkt = packet_new(new_pkt_len);
|
|
if (new_pkt == NULL)
|
|
{
|
|
PACKET_BUILD_LOG_ERROR("build L3 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), l3_layer->hdr_offset + l3_hdr_len);
|
|
if (l3_payload)
|
|
{
|
|
memcpy(new_pkt_data + l3_layer->hdr_offset + l3_hdr_len, l3_payload, l3_payload_len);
|
|
}
|
|
// update ip_proto
|
|
if (l3_layer->proto == LAYER_PROTO_IPV4)
|
|
{
|
|
struct ip *ip4 = (struct ip *)(new_pkt_data + l3_layer->hdr_offset);
|
|
ip4_hdr_set_protocol(ip4, ip_proto);
|
|
}
|
|
else
|
|
{
|
|
struct ip6_hdr *ip6 = (struct ip6_hdr *)(new_pkt_data + l3_layer->hdr_offset);
|
|
ip6_hdr_set_next_header(ip6, ip_proto);
|
|
}
|
|
calculate_length_and_checksum(origin_pkt, i + 1, new_pkt_data, new_pkt_len, trim_len);
|
|
|
|
packet_parse(new_pkt, new_pkt_data, new_pkt_len);
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
struct packet_origin origin = {
|
|
.type = ORIGIN_TYPE_USER,
|
|
.ctx = NULL,
|
|
.cb = NULL,
|
|
.args = NULL,
|
|
.thr_idx = -1,
|
|
};
|
|
memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
|
|
packet_set_origin(new_pkt, &origin);
|
|
|
|
return new_pkt;
|
|
} |