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
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
#include <stdio.h>
#include <sys/socket.h>
#include <stdlib.h>
#include <string.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <stdint.h>
#include <arpa/inet.h>
char encoded_pkt[4096], workspace[4096];
unsigned char pkt[2048];
typedef uint32_t u_int32_t;
u_int32_t checksum(unsigned char *buf, unsigned nbytes, u_int32_t sum)
{
int i;
/* Checksum all the pairs of bytes first... */
for (i = 0; i < (nbytes & ~1U); i += 2) {
sum += (u_int16_t)ntohs(*((u_int16_t *)(buf + i)));
if (sum > 0xFFFF)
sum -= 0xFFFF;
}
/*
* If there's a single byte left over, checksum it, too.
* Network byte order is big-endian, so the remaining byte is
* the high byte.
*/
if (i < nbytes) {
sum += buf[i] << 8;
if (sum > 0xFFFF)
sum -= 0xFFFF;
}
return (sum);
}
u_int32_t wrapsum(u_int32_t sum)
{
sum = ~sum & 0xFFFF;
return (htons(sum));
}
int main(int argc, char **argv)
{
int sock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
if (sock == -1) {
perror("socket");
exit(1);
}
for ( ;; ) {
int num_pkts;
int ret = scanf("%d %s", &num_pkts, encoded_pkt);
if (ret != 2) {
fprintf(stderr, "error parsing! ret=%d\n", ret);
exit(1);
}
if ((strlen(encoded_pkt) % 2) != 0) {
fprintf(stderr, "hex packet has odd length\n");
exit(1);
}
// de-hex packet
for (int i = 0; i < strlen(encoded_pkt); i += 2) {
char c[3];
c[0] = encoded_pkt[i];
c[1] = encoded_pkt[i + 1];
c[2] = 0;
int h = strtol(c, NULL, 16);
pkt[i / 2] = h;
}
int datalen = strlen(encoded_pkt) / 2;
for (int i = 0; i < num_pkts; ++i) {
char from_ip[256], to_ip[256];
int sport, dport;
if (scanf("%s %d %s %d", from_ip, &sport, to_ip, &dport) != 4) {
fprintf(stderr, "error parsing packet %d!\n", i);
exit(1);
}
// IP header
struct iphdr *ip = (struct iphdr *)workspace;
ip->version = 4;
ip->ihl = 5;
ip->tos = 0;
ip->tot_len = htons(datalen + sizeof(struct iphdr) + sizeof(struct udphdr));
ip->id = 0;
ip->frag_off = 0;
ip->ttl = 64;
ip->protocol = 17; // UDP
ip->saddr = inet_addr(from_ip);
ip->daddr = inet_addr(to_ip);
ip->check = 0;
ip->check = wrapsum(checksum((unsigned char *)ip, sizeof(*ip), 0));
// UDP header
struct udphdr *udp = (struct udphdr *)(workspace + sizeof(struct iphdr));
udp->source = htons(sport);
udp->dest = htons(dport);
udp->len = htons(datalen + sizeof(struct udphdr));
udp->check = 0;
int sum = checksum((unsigned char *)&ip->saddr, 2 * sizeof(ip->saddr), IPPROTO_UDP + ntohs(udp->len));
sum = checksum((unsigned char *)pkt, datalen, sum);
sum = checksum((unsigned char *)udp, sizeof(*udp), sum);
udp->check = wrapsum(sum);
// Data
memcpy(workspace + sizeof(struct iphdr) + sizeof(struct udphdr),
pkt, datalen);
// Send out the packet physically
struct sockaddr_in to;
to.sin_family = AF_INET;
to.sin_addr.s_addr = inet_addr(to_ip);
to.sin_port = htons(dport);
sendto(sock, workspace, ntohs(ip->tot_len), 0, (struct sockaddr *)&to, sizeof(to));
}
}
}
|
