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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
|
/****************************************************************
* (c) 2007 Technische Universitaet Dresden *
* This file is part of DROPS, which is distributed under the *
* terms of the GNU General Public License 2. Please see the *
* COPYING file for details. *
****************************************************************/
#include <linux/netdevice.h>
#include <linux/if_ether.h>
#include "arping.h"
#define PROT_ICMP 1
#define ICMP_REPLY 0
#define ICMP_REQ 8
#define ETH_ALEN 6
/* configuration */
int arping_verbose = 1; // verbose
#define VERBOSE_LOG(fmt, ...) \
do { \
if (arping_verbose) printk(fmt, ##__VA_ARGS__); \
} while (0);
char LOG_tag[9] = "arping";
ssize_t l4libc_heapsize = 32 * 1024;
//static unsigned char broadcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
//static int exit_somewhen = 0;
struct ethernet_hdr
{
unsigned char dest[6];
unsigned char src[6];
unsigned char type[2];
};
struct ip_hdr
{
char version_length;
char type;
int16_t length;
int16_t id;
int16_t flags_offset;
char ttl;
char protocol;
int16_t checksum;
int32_t src_ip;
int32_t dest_ip;
};
struct icmp_hdr
{
char type;
char code;
uint16_t checksum;
uint16_t id;
uint16_t seq_num;
};
static int handle_icmp_packet(struct sk_buff *skb);
static int handle_icmp_packet(struct sk_buff *skb)
{
unsigned char *data = skb->data;
struct ethernet_hdr *eth = NULL;
struct ethernet_hdr *e = NULL;
struct ip_hdr *ip = NULL;
struct ip_hdr *iphdr = NULL;
struct icmp_hdr *icmp = NULL;
struct icmp_hdr *icmp2 = NULL;
int ver, len;
struct sk_buff *snd_skb = NULL;
eth = (struct ethernet_hdr *)data;
VERBOSE_LOG("dest mac = %02x:%02x:%02x:%02x:%02x:%02x\n",
eth->dest[0], eth->dest[1], eth->dest[2],
eth->dest[3], eth->dest[4], eth->dest[5]);
VERBOSE_LOG("src mac = %02x:%02x:%02x:%02x:%02x:%02x\n",
eth->src[0], eth->src[1], eth->src[2],
eth->src[3], eth->src[4], eth->src[5]);
VERBOSE_LOG("type field = %02x%02x\n", eth->type[0], eth->type[1]);
if (eth->type[0] != 0x08 || eth->type[1] != 0x00) {
printk("unknown ethernet packet type!\n");
return -1;
}
ip = (struct ip_hdr *)(data + sizeof(struct ethernet_hdr));
VERBOSE_LOG("protocol = %02x (2x?)\n", ip->protocol, PROT_ICMP);
if (ip->protocol != PROT_ICMP)
{
printk("Unknown packet type.\n");
return -1;
}
VERBOSE_LOG("ICMP packet!\n");
ver = ip->version_length >> 4;
len = ip->version_length & 0x0F;
VERBOSE_LOG("IP version = %d, length = %d\n", ver, len);
VERBOSE_LOG("src IP: "NIPQUAD_FMT"\n", NIPQUAD(ip->src_ip));
VERBOSE_LOG("dest IP: "NIPQUAD_FMT"\n", NIPQUAD(ip->dest_ip));
icmp = (struct icmp_hdr *)(data + sizeof(struct ethernet_hdr)
+ sizeof(struct ip_hdr));
if (icmp->type != ICMP_REQ)
{
printk("This is no ICMP request.\n");
return -1;
}
VERBOSE_LOG("Hey this is an ICMP request just for me. :)\n");
VERBOSE_LOG("ICMP type : %d\n", icmp->type);
VERBOSE_LOG("ICMP code : %d\n", icmp->code);
VERBOSE_LOG("ICMP seq : %d\n", ntohs(icmp->seq_num));
snd_skb = alloc_skb(skb->len + skb->dev->hard_header_len, GFP_KERNEL);
memcpy(snd_skb->data, skb->data, skb->len);
e = (struct ethernet_hdr *)snd_skb->data;
memcpy(e->src, skb->dev->dev_addr, ETH_ALEN);
memcpy(e->dest, eth->src, ETH_ALEN);
VERBOSE_LOG("dest mac = %02x:%02x:%02x:%02x:%02x:%02x\n",
e->dest[0], e->dest[1], e->dest[2],
e->dest[3], e->dest[4], e->dest[5]);
VERBOSE_LOG("src mac = %02x:%02x:%02x:%02x:%02x:%02x\n",
e->src[0], e->src[1], e->src[2],
e->src[3], e->src[4], e->src[5]);
e->type[0] = 0x08;
e->type[1] = 0x00;
iphdr = (struct ip_hdr *)(snd_skb->data + sizeof(struct ethernet_hdr));
*iphdr = *ip;
// also switch src and dest
iphdr->src_ip = ip->dest_ip;
iphdr->dest_ip = ip->src_ip;
VERBOSE_LOG("src IP: "NIPQUAD_FMT"\n", NIPQUAD(iphdr->src_ip));
VERBOSE_LOG("dest IP: "NIPQUAD_FMT"\n", NIPQUAD(iphdr->dest_ip));
icmp2 = (struct icmp_hdr *)(snd_skb->data + sizeof(struct ethernet_hdr)
+ sizeof(struct ip_hdr));
*icmp2 = *icmp;
icmp2->type = ICMP_REPLY;
snd_skb->dev = skb->dev;
snd_skb->len = skb->len;
VERBOSE_LOG("sending reply\n");
skb->dev->netdev_ops->ndo_start_xmit(snd_skb, skb->dev);
VERBOSE_LOG("done\n");
return 0;
}
ddekit_sem_t *arping_semaphore = NULL;
struct arping_elem *arping_list = NULL;
int arping(void)
{
arping_semaphore = ddekit_sem_init(0);
while(1)
{
ddekit_sem_down(arping_semaphore);
struct arping_elem *elem = arping_list;
arping_list = arping_list->next;
/* parse packet */
int err = handle_icmp_packet(elem->skb);
VERBOSE_LOG("handle_icmp_packet: %d\n", err);
kfree_skb(elem->skb);
kfree(elem);
}
}
|
