int
ipv4_output(struct reply *reply, struct request *req)
{
unsigned short len;
u_int32_t sum;
if (reply->off <= 0)
fatalx("bad reply offset");
len = sizeof reply->pkt.bootp + reply->off;
reply->pkt.l3.ip_v = 4;
reply->pkt.l3.ip_hl = 5;
reply->pkt.l3.ip_ttl = 16;
reply->pkt.l3.ip_len = htons(len +
sizeof reply->pkt.l3 + sizeof reply->pkt.l4);
reply->pkt.l3.ip_tos = IPTOS_LOWDELAY;
reply->pkt.l3.ip_p = IPPROTO_UDP;
reply->pkt.l3.ip_src = ipv4_addr(req->rcvd_on);
reply->pkt.l3.ip_dst = destination(reply, req, &reply->pkt.l4.uh_dport);
/* Fill in IPv4 checksum. */
sum = checksum(&reply->pkt.l3, sizeof reply->pkt.l3, 0);
reply->pkt.l3.ip_sum = wrapsum(sum);
/* Fill in UDP checksum, now that we have the IPv4 header. */
sum = ntohs(reply->pkt.l4.uh_ulen) + IPPROTO_UDP;
sum = checksum(&reply->pkt.l3.ip_src, 2 * sizeof(struct in_addr), sum);
sum = checksum(&reply->pkt.bootp, len, sum);
sum = checksum(&reply->pkt.l4, sizeof reply->pkt.l4, sum);
reply->pkt.l4.uh_sum = wrapsum(sum);
return 20;
}
static inline int32_t validate_udp_packet(struct ipv4_hdr * ip_hdr,
struct udp_hdr * udp_hdr) {
uint16_t temp = 0;
if (unlikely(wrapsum(checksum(ip_hdr, sizeof(struct ipv4_hdr), 0)) != 0)) //checksum ip_hdr fail
return -1;
temp = (udp_hdr->dgram_cksum);
if (unlikely(temp == 0))
return 0;
udp_hdr->dgram_cksum = 0;
udp_hdr->dgram_cksum = wrapsum(
checksum((unsigned char *) udp_hdr, sizeof(struct udp_hdr),
checksum(&udp_hdr[1],
ntohs(ip_hdr->total_length)
- sizeof(struct ipv4_hdr)
- sizeof(struct udp_hdr),
checksum((unsigned char *) &ip_hdr->src_addr,
2 * sizeof(ip_hdr->src_addr),
IPPROTO_UDP
+ (uint32_t) ntohs(
udp_hdr->dgram_len)))));
if (unlikely(temp != udp_hdr->dgram_cksum)) // check sum udp fail
return -2;
return 0;
}
/*
* initialize one packet and prepare for the next one.
* The copy could be done better instead of repeating it each time.
*/
static void
initialize_packet(struct targ *targ)
{
struct pkt *pkt = &targ->pkt;
struct ether_header *eh;
struct ip *ip;
struct udphdr *udp;
uint16_t paylen = targ->g->pkt_size - sizeof(*eh) - sizeof(struct ip);
const char *payload = targ->g->options & OPT_INDIRECT ?
indirect_payload : default_payload;
int i, l, l0 = strlen(payload);
/* create a nice NUL-terminated string */
for (i = 0; i < paylen;) {
l = min(l0, paylen - i);
bcopy(payload, pkt->body + i, l);
i += l;
}
pkt->body[i-1] = '\0';
ip = &pkt->ip;
/* prepare the headers */
ip->ip_v = IPVERSION;
ip->ip_hl = 5;
ip->ip_id = 0;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_len = ntohs(targ->g->pkt_size - sizeof(*eh));
ip->ip_id = 0;
ip->ip_off = htons(IP_DF); /* Don't fragment */
ip->ip_ttl = IPDEFTTL;
ip->ip_p = IPPROTO_UDP;
ip->ip_dst.s_addr = htonl(targ->g->dst_ip.start);
ip->ip_src.s_addr = htonl(targ->g->src_ip.start);
ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0));
udp = &pkt->udp;
udp->uh_sport = htons(targ->g->src_ip.port0);
udp->uh_dport = htons(targ->g->dst_ip.port0);
udp->uh_ulen = htons(paylen);
/* Magic: taken from sbin/dhclient/packet.c */
udp->uh_sum = wrapsum(checksum(udp, sizeof(*udp),
checksum(pkt->body,
paylen - sizeof(*udp),
checksum(&ip->ip_src, 2 * sizeof(ip->ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp->uh_ulen)
)
)
));
eh = &pkt->eh;
bcopy(&targ->g->src_mac.start, eh->ether_shost, 6);
bcopy(&targ->g->dst_mac.start, eh->ether_dhost, 6);
eh->ether_type = htons(ETHERTYPE_IP);
bzero(&pkt->vh, sizeof(pkt->vh));
// dump_payload((void *)pkt, targ->g->pkt_size, NULL, 0);
}
/*
* Fill a packet with some payload.
* We create a UDP packet so the payload starts at
* 14+20+8 = 42 bytes.
*/
#ifdef __linux__
#define uh_sport source
#define uh_dport dest
#define uh_ulen len
#define uh_sum check
#endif /* linux */
static void
initialize_packet(struct targ *targ)
{
struct pkt *pkt = &targ->pkt;
struct ether_header *eh;
struct ip *ip;
struct udphdr *udp;
uint16_t paylen = targ->g->pkt_size - sizeof(*eh) - sizeof(struct ip);
int i, l, l0 = strlen(default_payload);
for (i = 0; i < paylen;) {
l = min(l0, paylen - i);
bcopy(default_payload, pkt->body + i, l);
i += l;
}
pkt->body[i-1] = '\0';
ip = &pkt->ip;
ip->ip_v = IPVERSION;
ip->ip_hl = 5;
ip->ip_id = 0;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_len = ntohs(targ->g->pkt_size - sizeof(*eh));
ip->ip_id = 0;
ip->ip_off = htons(IP_DF); /* Don't fragment */
ip->ip_ttl = IPDEFTTL;
ip->ip_p = IPPROTO_UDP;
ip->ip_dst.s_addr = targ->g->dst_ip.start.s_addr;
ip->ip_src.s_addr = targ->g->src_ip.start.s_addr;
ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0));
udp = &pkt->udp;
udp->uh_sport = htons(4096);
udp->uh_dport = htons(8192);
udp->uh_ulen = htons(paylen);
/* Magic: taken from sbin/dhclient/packet.c */
udp->uh_sum = wrapsum(checksum(udp, sizeof(*udp),
checksum(pkt->body,
paylen - sizeof(*udp),
checksum(&ip->ip_src, 2 * sizeof(ip->ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp->uh_ulen)
)
)
));
eh = &pkt->eh;
bcopy(&targ->g->src_mac.start, eh->ether_shost, 6);
bcopy(&targ->g->dst_mac.start, eh->ether_dhost, 6);
eh->ether_type = htons(ETHERTYPE_IP);
}
static void forge_udp_packet(char *buffer, u_int idx) {
int i;
struct ip_header *ip_header;
struct udp_header *udp_header;
u_int32_t src_ip = (0x0A000000 + idx) % 0xFFFFFFFF /* from 10.0.0.0 */;
u_int32_t dst_ip = 0xC0A80001 /* 192.168.0.1 */;
u_int16_t src_port = 2012, dst_port = 3000;
/* Reset packet */
memset(buffer, 0, sizeof(buffer));
for(i=0; i<12; i++) buffer[i] = i;
buffer[12] = 0x08, buffer[13] = 0x00; /* IP */
if(reforge_mac) memcpy(buffer, mac_address, 6);
ip_header = (struct ip_header*) &buffer[sizeof(struct ether_header)];
ip_header->ihl = 5;
ip_header->version = 4;
ip_header->tos = 0;
ip_header->tot_len = htons(send_len-sizeof(struct ether_header));
ip_header->id = htons(2012);
ip_header->ttl = 64;
ip_header->frag_off = htons(0);
ip_header->protocol = IPPROTO_UDP;
ip_header->daddr = htonl(dst_ip);
ip_header->saddr = htonl(src_ip);
ip_header->check = wrapsum(in_cksum((unsigned char *)ip_header,
sizeof(struct ip_header), 0));
udp_header = (struct udp_header*)(buffer + sizeof(struct ether_header) + sizeof(struct ip_header));
udp_header->source = htons(src_port);
udp_header->dest = htons(dst_port);
udp_header->len = htons(send_len-sizeof(struct ether_header)-sizeof(struct ip_header));
udp_header->check = 0; /* It must be 0 to compute the checksum */
/*
http://www.cs.nyu.edu/courses/fall01/G22.2262-001/class11.htm
http://www.ietf.org/rfc/rfc0761.txt
http://www.ietf.org/rfc/rfc0768.txt
*/
i = sizeof(struct ether_header) + sizeof(struct ip_header) + sizeof(struct udp_header);
udp_header->check = wrapsum(in_cksum((unsigned char *)udp_header, sizeof(struct udp_header),
in_cksum((unsigned char *)&buffer[i], send_len-i,
in_cksum((unsigned char *)&ip_header->saddr,
2*sizeof(ip_header->saddr),
IPPROTO_UDP + ntohs(udp_header->len)))));
}
int
ipv4_input(void *data, size_t len, struct request *req)
{
u_int8_t *buf = data;
int hdrlen, ip_len;
req->l3 = data;
hdrlen = (buf[0] & 0x0F) << 2;
if ((buf[0] & 0xF0) != 0x40) {
log_warnx("%s: wrong IP version byte %#x", __func__, buf[0]);
return (-1);
}
ip_len = ntohs(req->l3->ip_len);
if (len > MTU || (int) len != ip_len) {
++stats[STATS_IP_BAD_LEN];
log_warnx("%s: rcvd packet of length %zu, IPv4 says %u",
__func__, len, ip_len);
return (-1);
}
if (req->l3->ip_p != IPPROTO_UDP)
fatalx("wrong IP protocol: BPF doesn't work.");
if (wrapsum(checksum(data, hdrlen, 0)) != 0) {
log_warnx("%s: bad checksum", __func__);
return (-1);
}
if ((req->rcvd_on = bpf_address(req)) == NULL)
return (-1);
return (hdrlen);
}
int
icmp_echorequest(struct iaddr *addr)
{
struct sockaddr_in to;
struct icmp icmp;
int status;
if (!icmp_protocol_initialized)
error("attempt to use ICMP protocol before initialization.");
bzero(&to, sizeof(to));
to.sin_len = sizeof to;
to.sin_family = AF_INET;
memcpy(&to.sin_addr, addr->iabuf, sizeof to.sin_addr); /* XXX */
icmp.icmp_type = ICMP_ECHO;
icmp.icmp_code = 0;
icmp.icmp_cksum = 0;
icmp.icmp_seq = 0;
icmp.icmp_id = getpid() & 0xffff;
icmp.icmp_cksum = wrapsum(checksum((unsigned char *)&icmp,
sizeof(icmp), 0));
/* Send the ICMP packet... */
status = sendto(icmp_protocol_fd, &icmp, sizeof(icmp), 0,
(struct sockaddr *)&to, sizeof(to));
if (status == -1)
warning("icmp_echorequest %s: %m", inet_ntoa(to.sin_addr));
if (status != sizeof icmp)
return 0;
return 1;
}
void
flowgen_packet_init (void)
{
struct ip * ip;
struct udphdr * udp;
/* fill ip header */
ip = (struct ip *) flowgen.pkt;
ip->ip_v = IPVERSION;
ip->ip_hl = 5;
ip->ip_id = 0;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_len = htons (flowgen.pkt_len);
ip->ip_off = 0;
ip->ip_ttl = 16;
ip->ip_p = IPPROTO_UDP;
ip->ip_dst = flowgen.daddr;
ip->ip_src = flowgen.saddr;
ip->ip_sum = 0;
ip->ip_sum = wrapsum (checksum (ip, sizeof (*ip), 0));
/* fill udp header */
udp = (struct udphdr *) (ip + 1);
udp->uh_dport = htons (DSTPORT);
udp->uh_sport = 0; /* filled when xmitted */
udp->uh_ulen = htons (flowgen.pkt_len - sizeof (*ip));
udp->uh_sum = 0; /* no checksum */
return;
};
void
assemble_udp_ip_header(unsigned char *buf, int *bufix, u_int32_t from,
u_int32_t to, unsigned int port, unsigned char *data, int len)
{
struct ip ip;
struct udphdr udp;
ip.ip_v = 4;
ip.ip_hl = 5;
ip.ip_tos = IPTOS_LOWDELAY;
ip.ip_len = htons(sizeof(ip) + sizeof(udp) + len);
ip.ip_id = 0;
ip.ip_off = 0;
ip.ip_ttl = 16;
ip.ip_p = IPPROTO_UDP;
ip.ip_sum = 0;
ip.ip_src.s_addr = from;
ip.ip_dst.s_addr = to;
ip.ip_sum = wrapsum(checksum((unsigned char *)&ip, sizeof(ip), 0));
/*
* While the BPF -- used for broadcasts -- expects a "true" IP header
* with all the bytes in network byte order, the raw socket interface
* which is used for unicasts expects the ip_len field to be in host
* byte order. In both cases, the checksum has to be correct, so this
* is as good a place as any to turn the bytes around again.
*/
if (to != INADDR_BROADCAST)
ip.ip_len = ntohs(ip.ip_len);
memcpy(&buf[*bufix], &ip, sizeof(ip));
*bufix += sizeof(ip);
udp.uh_sport = htons(LOCAL_PORT); /* XXX */
udp.uh_dport = port; /* XXX */
udp.uh_ulen = htons(sizeof(udp) + len);
memset(&udp.uh_sum, 0, sizeof(udp.uh_sum));
udp.uh_sum = wrapsum(checksum((unsigned char *)&udp, sizeof(udp),
checksum(data, len, checksum((unsigned char *)&ip.ip_src,
2 * sizeof(ip.ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp.uh_ulen)))));
memcpy(&buf[*bufix], &udp, sizeof(udp));
*bufix += sizeof(udp);
}
void
assemble_udp_ip_header(struct interface_info *interface, unsigned char *buf,
int *bufix, u_int32_t from, u_int32_t to, unsigned int port,
unsigned char *data, int len)
{
struct ip ip;
struct udphdr udp;
ip.ip_v = 4;
ip.ip_hl = 5;
ip.ip_tos = IPTOS_LOWDELAY;
ip.ip_len = htons(sizeof(ip) + sizeof(udp) + len);
ip.ip_id = 0;
ip.ip_off = 0;
ip.ip_ttl = 16;
ip.ip_p = IPPROTO_UDP;
ip.ip_sum = 0;
ip.ip_src.s_addr = from;
ip.ip_dst.s_addr = to;
ip.ip_sum = wrapsum(checksum((unsigned char *)&ip, sizeof(ip), 0));
memcpy(&buf[*bufix], &ip, sizeof(ip));
*bufix += sizeof(ip);
udp.uh_sport = server_port; /* XXX */
udp.uh_dport = port; /* XXX */
udp.uh_ulen = htons(sizeof(udp) + len);
memset(&udp.uh_sum, 0, sizeof(udp.uh_sum));
udp.uh_sum = wrapsum(checksum((unsigned char *)&udp, sizeof(udp),
checksum(data, len, checksum((unsigned char *)&ip.ip_src,
2 * sizeof(ip.ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp.uh_ulen)))));
memcpy(&buf[*bufix], &udp, sizeof(udp));
*bufix += sizeof(udp);
}
static void
build_udp_packet(uint8_t *buf, int pkt_size)
{
struct ether_header *eh = (struct ether_header *)buf;
struct ip *ip = (struct ip *)(eh + 1);
struct udphdr *udp = (struct udphdr *)(ip + 1);
char *data = (char *)(udp + 1);
/* Just fake an address */
eh->ether_shost[0] = 0x01;
eh->ether_shost[1] = 0xbd;
eh->ether_shost[2] = 0xbc;
eh->ether_shost[3] = 0x4d;
eh->ether_shost[4] = 0xfb;
eh->ether_shost[5] = 0xfb;
memcpy(eh->ether_dhost, vmeif_ether, ETHER_ADDR_LEN);
eh->ether_type = htons(ETHERTYPE_IP);
ip->ip_v = IPVERSION;
ip->ip_hl = sizeof(*ip) >> 2;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_len = htons(pkt_size - sizeof(*eh));
ip->ip_id = 0;
ip->ip_off = htons(IP_DF); /* XXX: Don't fragment */
ip->ip_ttl = IPDEFTTL;
ip->ip_p = IPPROTO_UDP;
ip->ip_src.s_addr = htonl(0x0a000001 + (rand() % 0xfffffe));
ip->ip_dst.s_addr = vmeif_ip;
ip->ip_sum = 0;
ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0));
udp->uh_sport = htons(1 + rand() % 65535);
udp->uh_dport = htons(SERVER_PORT);
udp->uh_ulen = htons(pkt_size - sizeof(*eh) - sizeof(*ip));
udp->uh_sum = 0;
memcpy(data, payload, min(strlen(payload) + 1,
pkt_size - (sizeof(*eh) + sizeof(*ip) + sizeof(*udp))));
}
int
udp_input(void *data, size_t len, struct request *req)
{
u_int32_t sum, origsum;
size_t uh_ulen;
req->l4 = data;
uh_ulen = ntohs(req->l4->uh_ulen);
if (len > MTU || len != uh_ulen) {
++stats[STATS_UDP_BAD_LEN];
log_warnx("%s: rcvd packet of length %zu, UDP says %u",
__func__, len, uh_ulen);
return (-1);
}
if (req->l4->uh_dport != htons(BOOTP_SERVER_PORT))
fatalx("wrong UDP dport: BPF doesn't work.");
if ((origsum = req->l4->uh_sum) != 0) {
req->l4->uh_sum = 0;
sum = uh_ulen + IPPROTO_UDP;
sum = checksum(&req->l3->ip_src,
2 * sizeof(struct in_addr), sum);
sum = checksum((char *) data + sizeof *req->l4,
len - sizeof *req->l4, sum);
sum = checksum(data, sizeof *req->l4, sum);
if (wrapsum(sum) != origsum) {
log_warnx("%s: bad checksum", __func__);
return (-1);
}
}
return sizeof *req->l4;
}
ssize_t
decode_udp_ip_header(struct interface_info *interface, unsigned char *buf,
int bufix, struct sockaddr_in *from, unsigned char *data, int buflen)
{
struct ip *ip;
struct udphdr *udp;
u_int32_t ip_len = (buf[bufix] & 0xf) << 2;
u_int32_t sum, usum;
static int ip_packets_seen;
static int ip_packets_bad_checksum;
static int udp_packets_seen;
static int udp_packets_bad_checksum;
static int udp_packets_length_checked;
static int udp_packets_length_overflow;
int len = 0;
ip = (struct ip *)(buf + bufix);
udp = (struct udphdr *)(buf + bufix + ip_len);
/* Check the IP header checksum - it should be zero. */
ip_packets_seen++;
if (wrapsum(checksum(buf + bufix, ip_len, 0)) != 0) {
ip_packets_bad_checksum++;
if (ip_packets_seen > 4 &&
(ip_packets_seen / ip_packets_bad_checksum) < 2) {
note("%d bad IP checksums seen in %d packets",
ip_packets_bad_checksum, ip_packets_seen);
ip_packets_seen = ip_packets_bad_checksum = 0;
}
return (-1);
}
if (ntohs(ip->ip_len) != buflen)
debug("ip length %d disagrees with bytes received %d.",
ntohs(ip->ip_len), buflen);
memcpy(&from->sin_addr, &ip->ip_src, 4);
/*
* Compute UDP checksums, including the ``pseudo-header'', the
* UDP header and the data. If the UDP checksum field is zero,
* we're not supposed to do a checksum.
*/
if (!data) {
data = buf + bufix + ip_len + sizeof(*udp);
len = ntohs(udp->uh_ulen) - sizeof(*udp);
udp_packets_length_checked++;
if (len + data > buf + bufix + buflen) {
udp_packets_length_overflow++;
if (udp_packets_length_checked > 4 &&
(udp_packets_length_checked /
udp_packets_length_overflow) < 2) {
note("%d udp packets in %d too long - dropped",
udp_packets_length_overflow,
udp_packets_length_checked);
udp_packets_length_overflow =
udp_packets_length_checked = 0;
}
return (-1);
}
if (len + data != buf + bufix + buflen)
debug("accepting packet with data after udp payload.");
}
usum = udp->uh_sum;
udp->uh_sum = 0;
sum = wrapsum(checksum((unsigned char *)udp, sizeof(*udp),
checksum(data, len, checksum((unsigned char *)&ip->ip_src,
2 * sizeof(ip->ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp->uh_ulen)))));
udp_packets_seen++;
if (usum && usum != sum) {
udp_packets_bad_checksum++;
if (udp_packets_seen > 4 &&
(udp_packets_seen / udp_packets_bad_checksum) < 2) {
note("%d bad udp checksums in %d packets",
udp_packets_bad_checksum, udp_packets_seen);
udp_packets_seen = udp_packets_bad_checksum = 0;
}
return (-1);
}
memcpy(&from->sin_port, &udp->uh_sport, sizeof(udp->uh_sport));
return (ip_len + sizeof(*udp));
}
static int rx_thread(void * param) {
struct port_configure * pconf = NULL; //(struct port_configure *) param;
struct rte_ring* ring_in = NULL; //pconf->ring_in;
struct rte_ring* ring_out = NULL; //pconf->ring_out;
uint8_t port_id = 0; //pconf->portid;
uint16_t nb_rx_pkts;
struct rte_mbuf *m;
struct ether_hdr *eth_hdr;
struct arp_hdr *arp_hdr;
int32_t i, j, k, ret;
struct rte_mbuf *pkts[MAX_PKTS_BURST];
int32_t ether_type;
struct ipv4_hdr *ip_hdr;
struct icmp_hdr* icmp_hdr;
struct udp_hdr* udp_hdr;
while (!quit_signal) {
for (k = 0; k < num_config_port; ++k) {
pconf = &ports_conf[k];
ring_in = pconf->ring_in;
ring_out = pconf->ring_out;
port_id = pconf->portid;
nb_rx_pkts = rte_eth_rx_burst(port_id, 0, pkts, MAX_PKTS_BURST);
if (unlikely(nb_rx_pkts == 0)) {
continue;
}
for (i = 0; i < nb_rx_pkts; ++i) {
m = pkts[i];
eth_hdr = rte_pktmbuf_mtod(m, struct ether_hdr *);
ether_type = eth_hdr->ether_type;
if (unlikely(rte_cpu_to_be_16(ETHER_TYPE_ARP) == ether_type)) {
arp_hdr = (struct arp_hdr *) ((char *) (eth_hdr + 1));
if (arp_hdr->arp_op == rte_cpu_to_be_16(ARP_OP_REQUEST)) {
if (arp_hdr->arp_data.arp_tip == (pconf->ip)) {
arp_hdr->arp_op = rte_cpu_to_be_16(ARP_OP_REPLY);
ether_addr_copy(ð_hdr->s_addr, ð_hdr->d_addr);
ether_addr_copy(&pconf->addr, ð_hdr->s_addr);
ether_addr_copy(&arp_hdr->arp_data.arp_sha,
&arp_hdr->arp_data.arp_tha);
arp_hdr->arp_data.arp_tip =
arp_hdr->arp_data.arp_sip;
ether_addr_copy(&pconf->addr,
&arp_hdr->arp_data.arp_sha);
arp_hdr->arp_data.arp_sip = (pconf->ip);
MMENQUEUE(ring_out, m);
} else {
MMENQUEUE(ring_arp_request, m);
}
} else if (arp_hdr->arp_op == rte_cpu_to_be_16(ARP_OP_REPLY)) {
MMENQUEUE(ring_arp_reply, m);
} else {
rte_pktmbuf_free(m);
}
} else if (likely(
rte_cpu_to_be_16(ETHER_TYPE_IPv4) == ether_type)) {
ip_hdr = (struct ipv4_hdr *) ((char *) (eth_hdr + 1));
switch (ip_hdr->next_proto_id) {
case IPPROTO_ICMP:
//printf("nhan ban tin ping\n");
icmp_hdr = (struct icmp_hdr *) ((unsigned char *) ip_hdr
+ sizeof(struct ipv4_hdr));
if (unlikely(
wrapsum(
checksum(icmp_hdr,
(m->data_len
- sizeof(struct ether_hdr)
- sizeof(struct ipv4_hdr)),
0)))) {
printf("ICMP check sum error\n");
rte_pktmbuf_free(m);
break;
}
if (unlikely(
icmp_hdr->icmp_type == IP_ICMP_ECHO_REQUEST)) {
if (ntohl(ip_hdr->dst_addr) == INADDR_BROADCAST) {
rte_pktmbuf_free(m);
} else {
icmp_hdr->icmp_type = IP_ICMP_ECHO_REPLY;
icmp_hdr->icmp_cksum = 0;
icmp_hdr->icmp_cksum =
wrapsum(
checksum(icmp_hdr,
(m->data_len
- sizeof(struct ether_hdr)
- sizeof(struct ipv4_hdr)),
0));
inetAddrSwap(&ip_hdr->src_addr,
&ip_hdr->dst_addr);
ip_hdr->packet_id = htons(
ntohs(ip_hdr->packet_id) + m->data_len);
ip_hdr->hdr_checksum = 0;
ip_hdr->hdr_checksum = wrapsum(
checksum(ip_hdr,
sizeof(struct ipv4_hdr), 0));
ethAddrSwap(ð_hdr->d_addr, ð_hdr->s_addr);
MMENQUEUE(ring_out, m);
}
} else {
rte_pktmbuf_free(m);
}
break;
case IPPROTO_UDP:
MMENQUEUE(ring_in, m)
;
break;
default:
rte_pktmbuf_free(m);
}
} else {
rte_pktmbuf_free(m);
}
}
}
}
return 0;
}
static int
build_udp_packet_v4(uint8_t *buf, int payloadlen, int *off)
{
struct ether_header *eh = (struct ether_header *)buf;
struct ip *ip = (struct ip *)(eh + 1);
struct udphdr *udp = (struct udphdr *)(ip + 1);
char *data;
int plen, pktsize = 0;
static int id = 0;
plen = min(payloadlen - *off, MAXPAYLOADLEN);
pktsize += plen;
/* Just fake an address */
eh->ether_shost[0] = 0x01;
eh->ether_shost[1] = 0xbd;
eh->ether_shost[2] = 0xbc;
eh->ether_shost[3] = 0x4d;
eh->ether_shost[4] = 0xfb;
eh->ether_shost[5] = 0xfb;
memcpy(eh->ether_dhost, tapif_ether, ETHER_ADDR_LEN);
eh->ether_type = htons(ETHERTYPE_IP);
pktsize += sizeof(*eh);
ip->ip_v = IPVERSION;
ip->ip_hl = sizeof(*ip) >> 2;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_ttl = IPDEFTTL;
ip->ip_p = IPPROTO_UDP;
ip->ip_src = ip_src;
ip->ip_dst = tapif_ip;
if (*off == 0) {
udp->uh_sport = htons(src_port);
udp->uh_dport = htons(SERVER_PORT);
udp->uh_ulen = htons(sizeof(*udp) + plen);
udp->uh_sum = 0;
pktsize += sizeof(*udp);
ip->ip_len = htons(sizeof(*ip) + sizeof(*udp) + plen);
data = (char *)(udp + 1);
memcpy(data, payload, plen);
} else {
data = (char *)(ip + 1);
memcpy(data, payload, plen);
ip->ip_len = htons(sizeof(*ip) + plen);
}
ip->ip_off = htons((*off == 0 ? 0 : sizeof(*udp) + *off) >> 3);
if (*off + plen < payloadlen)
ip->ip_off |= htons(IP_MF);
if (*off == 0)
id++;
ip->ip_id = id;
ip->ip_sum = 0;
ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0));
pktsize += sizeof(*ip);
*off += plen;
return (pktsize);
}
static void forge_udp_packet(u_char *buffer, u_int idx) {
int i;
struct ip_header *ip_header;
struct udp_header *udp_header;
u_int32_t src_ip = 0x0A000000; /* 10.0.0.0 */
u_int32_t dst_ip = 0xC0A80001; /* 192.168.0.1 */
u_int16_t src_port = 2014, dst_port = 3000;
if (num_ips == 0) {
src_ip |= idx & 0xFFFFFF;
} else if (num_ips > 1) {
if (POW2(num_ips))
src_ip |= idx & (num_ips - 1) & 0xFFFFFF;
else
src_ip |= (idx % num_ips) & 0xFFFFFF;
}
#if 0
memset(buffer, 0, packet_len + 4);
#endif
if (idx == 0) { /* first packet, precomputing headers */
for(i = 0; i < 12; i++) buffer[i] = i;
buffer[12] = 0x08, buffer[13] = 0x00; /* IP */
ip_header = (struct ip_header*) &buffer[sizeof(struct ether_header)];
ip_header->ihl = 5;
ip_header->version = 4;
ip_header->tos = 0;
ip_header->tot_len = htons(packet_len-sizeof(struct ether_header));
ip_header->id = htons(2012);
ip_header->ttl = 64;
ip_header->frag_off = htons(0);
ip_header->protocol = IPPROTO_UDP;
ip_header->daddr = htonl(dst_ip);
ip_header->saddr = htonl(src_ip);
ip_header->check = 0;
udp_header = (struct udp_header*)(buffer + sizeof(struct ether_header) + sizeof(struct ip_header));
udp_header->source = htons(src_port);
udp_header->dest = htons(dst_port);
udp_header->len = htons(packet_len-sizeof(struct ether_header)-sizeof(struct ip_header));
udp_header->check = 0;
memcpy(matrix_buffer, buffer, sizeof(struct ether_header) + sizeof(struct ip_header) + sizeof(struct udp_header));
} else {
memcpy(buffer, matrix_buffer, sizeof(struct ether_header) + sizeof(struct ip_header) + sizeof(struct udp_header));
}
ip_header = (struct ip_header*) &buffer[sizeof(struct ether_header)];
ip_header->saddr = htonl(src_ip);
ip_header->check = wrapsum(in_cksum((unsigned char *)ip_header, sizeof(struct ip_header), 0));
#if 0
i = sizeof(struct ether_header) + sizeof(struct ip_header) + sizeof(struct udp_header);
udp_header->check = wrapsum(in_cksum((unsigned char *)udp_header, sizeof(struct udp_header),
in_cksum((unsigned char *)&buffer[i], packet_len-i,
in_cksum((unsigned char *)&ip_header->saddr,
2*sizeof(ip_header->saddr),
IPPROTO_UDP + ntohs(udp_header->len)))));
#endif
}
/*
* initialize one packet and prepare for the next one.
* The copy could be done better instead of repeating it each time.
*/
static void
initialize_packet(struct targ *targ)
{
struct pkt *pkt = &targ->pkt;
struct ether_header *eh;
struct ip *ip;
struct udphdr *udp;
uint16_t paylen = targ->g->pkt_size - sizeof(*eh) - sizeof(struct ip);
const char *payload = targ->g->options & OPT_INDIRECT ?
indirect_payload : default_payload;
int i, l0 = strlen(payload);
char errbuf[PCAP_ERRBUF_SIZE];
pcap_t *file;
struct pcap_pkthdr *header;
const unsigned char *packet;
/* Read a packet from a PCAP file if asked. */
if (targ->g->packet_file != NULL) {
if ((file = pcap_open_offline(targ->g->packet_file,
errbuf)) == NULL)
D("failed to open pcap file %s",
targ->g->packet_file);
if (pcap_next_ex(file, &header, &packet) < 0)
D("failed to read packet from %s",
targ->g->packet_file);
if ((targ->frame = malloc(header->caplen)) == NULL)
D("out of memory");
bcopy(packet, (unsigned char *)targ->frame, header->caplen);
targ->g->pkt_size = header->caplen;
pcap_close(file);
return;
}
/* create a nice NUL-terminated string */
for (i = 0; i < paylen; i += l0) {
if (l0 > paylen - i)
l0 = paylen - i; // last round
bcopy(payload, pkt->body + i, l0);
}
pkt->body[i-1] = '\0';
ip = &pkt->ip;
/* prepare the headers */
ip->ip_v = IPVERSION;
ip->ip_hl = 5;
ip->ip_id = 0;
ip->ip_tos = IPTOS_LOWDELAY;
ip->ip_len = ntohs(targ->g->pkt_size - sizeof(*eh));
ip->ip_id = 0;
ip->ip_off = htons(IP_DF); /* Don't fragment */
ip->ip_ttl = IPDEFTTL;
ip->ip_p = IPPROTO_UDP;
ip->ip_dst.s_addr = htonl(targ->g->dst_ip.start);
ip->ip_src.s_addr = htonl(targ->g->src_ip.start);
ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0));
udp = &pkt->udp;
udp->uh_sport = htons(targ->g->src_ip.port0);
udp->uh_dport = htons(targ->g->dst_ip.port0);
udp->uh_ulen = htons(paylen);
/* Magic: taken from sbin/dhclient/packet.c */
udp->uh_sum = wrapsum(checksum(udp, sizeof(*udp),
checksum(pkt->body,
paylen - sizeof(*udp),
checksum(&ip->ip_src, 2 * sizeof(ip->ip_src),
IPPROTO_UDP + (u_int32_t)ntohs(udp->uh_ulen)
)
)
));
eh = &pkt->eh;
bcopy(&targ->g->src_mac.start, eh->ether_shost, 6);
bcopy(&targ->g->dst_mac.start, eh->ether_dhost, 6);
eh->ether_type = htons(ETHERTYPE_IP);
bzero(&pkt->vh, sizeof(pkt->vh));
#ifdef TRASH_VHOST_HDR
/* set bogus content */
pkt->vh.fields[0] = 0xff;
pkt->vh.fields[1] = 0xff;
pkt->vh.fields[2] = 0xff;
pkt->vh.fields[3] = 0xff;
pkt->vh.fields[4] = 0xff;
pkt->vh.fields[5] = 0xff;
#endif /* TRASH_VHOST_HDR */
// dump_payload((void *)pkt, targ->g->pkt_size, NULL, 0);
}
