1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
|
#include "icmp.h"
extern struct rte_mempool *mempool;
extern struct rte_ring *send_ring;
extern struct rte_ring *recv_ring;
extern struct rte_ether_addr *local_mac;
void handle_ping_echo_packet(struct rte_icmp_hdr *icmp_hdr, struct rte_ipv4_hdr *ip_hdr, struct rte_ether_hdr *eth_hdr)
{
// Calculate ICMP length
uint16_t ip_hdr_len = (ip_hdr->version_ihl & 0x0F) * 4;
uint16_t ip_total_len = rte_be_to_cpu_16(ip_hdr->total_length);
uint16_t icmp_len = ip_total_len - ip_hdr_len;
// Allocate new mbuf for reply
struct rte_mbuf *reply_mbuf = rte_pktmbuf_alloc(mempool);
if (!reply_mbuf)
{
printf("[PING] Failed to allocate mbuf for ICMP reply\n");
return;
}
// Total packet length = Ethernet + IP + ICMP
uint16_t pkt_len = sizeof(struct rte_ether_hdr) + ip_hdr_len + icmp_len;
char *pkt_data = rte_pktmbuf_append(reply_mbuf, pkt_len);
if (!pkt_data)
{
printf("[PING] Failed to append data for ICMP reply\n");
rte_pktmbuf_free(reply_mbuf);
return;
}
// Construct headers
struct rte_ether_hdr *r_eth_hdr = (struct rte_ether_hdr *)pkt_data;
struct rte_ipv4_hdr *r_ip_hdr = (struct rte_ipv4_hdr *)(r_eth_hdr + 1);
struct rte_icmp_hdr *r_icmp_hdr = (struct rte_icmp_hdr *)((char *)r_ip_hdr + sizeof(struct rte_ipv4_hdr));
// Ethernet header
rte_ether_addr_copy(ð_hdr->src_addr, &r_eth_hdr->dst_addr);
rte_ether_addr_copy(local_mac, &r_eth_hdr->src_addr);
r_eth_hdr->ether_type = rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV4);
// IPv4 header
memset(r_ip_hdr, 0, sizeof(struct rte_ipv4_hdr));
r_ip_hdr->version_ihl = 0x45;
r_ip_hdr->total_length = rte_cpu_to_be_16(ip_hdr_len + icmp_len);
r_ip_hdr->time_to_live = 64;
r_ip_hdr->next_proto_id = IPPROTO_ICMP;
r_ip_hdr->src_addr = ip_hdr->dst_addr;
r_ip_hdr->dst_addr = ip_hdr->src_addr;
r_ip_hdr->hdr_checksum = 0;
r_ip_hdr->hdr_checksum = rte_ipv4_cksum(r_ip_hdr);
// ICMP header and payload
memcpy(r_icmp_hdr, icmp_hdr, icmp_len);
r_icmp_hdr->icmp_type = ICMP_ECHOREPLY;
r_icmp_hdr->icmp_code = 0;
r_icmp_hdr->icmp_ident = icmp_hdr->icmp_ident;
r_icmp_hdr->icmp_seq_nb = icmp_hdr->icmp_seq_nb;
r_icmp_hdr->icmp_cksum = 0;
r_icmp_hdr->icmp_cksum = ~rte_raw_cksum(r_icmp_hdr, icmp_len);
// Enqueue reply
if (rte_ring_enqueue(send_ring, reply_mbuf) < 0)
{
printf("[PING] Failed to enqueue ICMP reply\n");
rte_pktmbuf_free(reply_mbuf);
}
else
{
struct in_addr dst;
dst.s_addr = r_ip_hdr->dst_addr;
printf("[PING] ICMP reply sent to %s (%u bytes)\n", inet_ntoa(dst), pkt_len);
}
}
void handle_ping(struct rte_icmp_hdr *icmp_hdr, struct rte_ipv4_hdr *ip_hdr, struct rte_ether_hdr *eth_hdr)
{
if (icmp_hdr->icmp_type == ICMP_ECHO) // "RTE_IP_ICMP_ECHO_REQUEST" is deprecated
{
// ICMP Echo Request received
handle_ping_echo_packet(icmp_hdr, ip_hdr, eth_hdr);
}
}
|