Browse code

core: basic raw socket support functions

Basic support for raw sockets. Functions for creating, sending and
receiving udp packets over raw sockets.
Initial version supports only linux.

Andrei Pelinescu-Onciul authored on 07/06/2010 22:21:08
Showing 2 changed files
1 1
new file mode 100644
... ...
@@ -0,0 +1,452 @@
0
+/* 
1
+ * $Id$
2
+ *
3
+ * Copyright (C) 2010 iptelorg GmbH
4
+ *
5
+ * Permission to use, copy, modify, and distribute this software for any
6
+ * purpose with or without fee is hereby granted, provided that the above
7
+ * copyright notice and this permission notice appear in all copies.
8
+ *
9
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16
+ */
17
+/** raw socket functions.
18
+ *  @file raw_sock.c
19
+ *  @ingroup core
20
+ *  Module: @ref core
21
+ */
22
+/* 
23
+ * History:
24
+ * --------
25
+ *  2010-06-07  initial version (from older code) andrei
26
+ */
27
+/*
28
+ * FIXME: IP_PKTINFO & IP_HDRINCL - linux specific
29
+ * FIXME: linux specific iphdr and udphdr
30
+ * FIXME: send support for IP_HDRINCL
31
+ */
32
+
33
+#ifdef USE_RAW_SOCKS
34
+
35
+#include "compiler_opt.h"
36
+#include "ip_addr.h"
37
+#include "dprint.h"
38
+#include "str.h"
39
+#include "ut.h"
40
+
41
+#include <errno.h>
42
+#include <string.h>
43
+#include <unistd.h>
44
+#include <sys/types.h>
45
+#include <fcntl.h>
46
+#include <sys/socket.h>
47
+#include <netinet/in.h>
48
+#include <netinet/ip.h>
49
+#include <netinet/udp.h>
50
+#include <arpa/inet.h>
51
+
52
+#include "raw_sock.h"
53
+
54
+
55
+/** create and return a raw socket.
56
+ * @param proto - protocol used (e.g. IPPROTO_UDP, IPPROTO_RAW)
57
+ * @param ip - if not null the socket will be bound on this ip.
58
+ * @param iface - if not null the socket will be bound to this interface
59
+ *                (SO_BINDTODEVICE).
60
+ * @param iphdr_incl - set to 1 if packets send on this socket include
61
+ *                     a pre-built ip header (some fields, like the checksum
62
+ *                     will still be filled by the kernel, OTOH packet
63
+ *                     fragmentation has to be done in user space).
64
+ * @return socket on success, -1 on error
65
+ */
66
+int raw_socket(int proto, struct ip_addr* ip, str* iface, int iphdr_incl)
67
+{
68
+	int sock;
69
+	int t;
70
+	union sockaddr_union su;
71
+	char short_ifname[sizeof(int)];
72
+	int ifname_len;
73
+	char* ifname;
74
+
75
+	sock = socket(PF_INET, SOCK_RAW, proto);
76
+	if (sock==-1){
77
+		ERR("raw_socket: socket() failed: %s [%d]\n",
78
+				strerror(errno), errno);
79
+		goto error;
80
+	}
81
+	/* set socket options */
82
+	if (iphdr_incl) {
83
+		t=1;
84
+		if (setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &t, sizeof(t))<0){
85
+			ERR("raw_socket: setsockopt(IP_HDRINCL) failed: %s [%d]\n",
86
+					strerror(errno), errno);
87
+			goto error;
88
+		}
89
+	} else {
90
+		/* IP_PKTINFO makes no sense if the ip header is included */
91
+		/* using IP_PKTINFO */
92
+		t=1;
93
+		if (setsockopt(sock, IPPROTO_IP, IP_PKTINFO, &t, sizeof(t))<0){
94
+			ERR("raw_socket: setsockopt(IP_PKTINFO) failed: %s [%d]\n",
95
+					strerror(errno), errno);
96
+			goto error;
97
+		}
98
+	}
99
+	t=IP_PMTUDISC_DONT;
100
+	if(setsockopt(sock, IPPROTO_IP, IP_MTU_DISCOVER, &t, sizeof(t)) ==-1){
101
+		LOG(L_ERR, "raw_socket: setsockopt(IP_MTU_DISCOVER): %s\n",
102
+				strerror(errno));
103
+		goto error;
104
+	}
105
+	if (iface && iface->s){
106
+		/* workaround for linux bug: arg to setsockopt must have at least
107
+		 * sizeof(int) size or EINVAL would be returned */
108
+		if (iface->len<sizeof(int)){
109
+			memcpy(short_ifname, iface->s, iface->len);
110
+			short_ifname[iface->len]=0; /* make sure it's zero term */
111
+			ifname_len=sizeof(short_ifname);
112
+			ifname=short_ifname;
113
+		}else{
114
+			ifname_len=iface->len;
115
+			ifname=iface->s;
116
+		}
117
+		if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, ifname, ifname_len)
118
+						<0){
119
+				ERR("raw_socket: could not bind to %.*s: %s [%d]\n",
120
+							iface->len, ZSW(iface->s), strerror(errno), errno);
121
+				goto error;
122
+		}
123
+	}
124
+	/* FIXME: probe_max_receive_buffer(sock) missing */
125
+	if (ip){
126
+		init_su(&su, ip, 0);
127
+		if (bind(sock, &su.s, sockaddru_len(su))==-1){
128
+			ERR("raw_socket: bind(%s) failed: %s [%d]\n",
129
+				ip_addr2a(ip), strerror(errno), errno);
130
+			goto error;
131
+		}
132
+	}
133
+	return sock;
134
+error:
135
+	if (sock!=-1) close(sock);
136
+	return -1;
137
+}
138
+
139
+
140
+
141
+/** create and return an udp over ipv4  raw socket.
142
+ * @param ip - if not null the socket will be bound on this ip.
143
+ * @param iface - if not null the socket will be bound to this interface
144
+ *                (SO_BINDTODEVICE).
145
+ * @param iphdr_incl - set to 1 if packets send on this socket include
146
+ *                     a pre-built ip header (some fields, like the checksum
147
+ *                     will still be filled by the kernel, OTOH packet
148
+ *                     fragmentation has to be done in user space).
149
+ * @return socket on success, -1 on error
150
+ */
151
+int raw_udp4_socket(struct ip_addr* ip, str* iface, int iphdr_incl)
152
+{
153
+	return raw_socket(IPPROTO_UDP, ip, iface, iphdr_incl);
154
+}
155
+
156
+
157
+
158
+/** receives an ipv4 packet suing a raw socket.
159
+ * An ipv4 packet is received in buf, using IP_PKTINFO.
160
+ * from and to are filled (only the ip part the ports are 0 since this
161
+ * function doesn't try to look beyond the IP level).
162
+ * @param sock - raw socket
163
+ * @param buf - detination buffer.
164
+ * @param len - buffer len (should be enough for receiving a packet +
165
+ *               IP header).
166
+ * @param from - result parameter, the IP address part of it will be filled
167
+ *                with the source address and the port with 0.
168
+ * @param to - result parameter, the IP address part of it will be filled
169
+ *                with the destination (local) address and the port with 0.
170
+ * @return packet len or <0 on error: -1 (check errno),
171
+ *        -2 no IP_PKTINFO found or AF mismatch
172
+ */
173
+int recvpkt4(int sock, char* buf, int len, union sockaddr_union* from,
174
+					union sockaddr_union* to)
175
+{
176
+	struct iovec iov[1];
177
+	struct msghdr rcv_msg;
178
+	struct cmsghdr* cmsg;
179
+	struct in_pktinfo* rcv_pktinfo;
180
+	int n, ret;
181
+	char msg_ctrl_buf[1024];
182
+
183
+	iov[0].iov_base=buf;
184
+	iov[0].iov_len=len;
185
+	rcv_msg.msg_name=from;
186
+	rcv_msg.msg_namelen=sockaddru_len(*from);
187
+	rcv_msg.msg_control=msg_ctrl_buf;
188
+	rcv_msg.msg_controllen=sizeof(msg_ctrl_buf);
189
+	rcv_msg.msg_iov=&iov[0];
190
+	rcv_msg.msg_iovlen=1;
191
+	ret=-2; /* no PKT_INFO or AF mismatch */
192
+retry:
193
+	n=recvmsg(sock, &rcv_msg, MSG_WAITALL);
194
+	if (unlikely(n==-1)){
195
+		if (errno==EINTR)
196
+			goto retry;
197
+		ret=n;
198
+		goto end;
199
+	}
200
+	/* find the pkt info */
201
+	rcv_pktinfo=0;
202
+	for (cmsg=CMSG_FIRSTHDR(&rcv_msg); cmsg; cmsg=CMSG_NXTHDR(&rcv_msg, cmsg)){
203
+		if (likely((cmsg->cmsg_level==IPPROTO_IP) &&
204
+					(cmsg->cmsg_type==IP_PKTINFO))) {
205
+			rcv_pktinfo=(struct in_pktinfo*)CMSG_DATA(cmsg);
206
+			to->sin.sin_family=AF_INET;
207
+			memcpy(&to->sin.sin_addr, &rcv_pktinfo->ipi_spec_dst.s_addr, 
208
+									sizeof(to->sin.sin_addr));
209
+			to->sin.sin_port=0; /* not known */
210
+			/* interface no. in ipi_ifindex */
211
+			ret=n; /* success */
212
+			break;
213
+		}
214
+	}
215
+end:
216
+	return ret;
217
+}
218
+
219
+
220
+
221
+/* receive an ipv4 udp packet over a raw socket.
222
+ * The packet is copied in *buf and *buf is advanced to point to the
223
+ * payload.  Fills from and to.
224
+ * @param rsock - raw socket
225
+ * @param buf - the packet will be written to where *buf points intially and
226
+ *              then *buf will be advanced to point to the udp payload.
227
+ * @param len - buffer length (should be enough to hold at least the
228
+ *               ip and udp headers + 1 byte).
229
+ * @param from - result parameter, filled with source address and port of the
230
+ *               packet.
231
+ * @param from - result parameter, filled with destination (local) address and
232
+ *               port of the packet.
233
+ * @param rf   - filter used to decide whether or not the packet is
234
+ *                accepted/processed. If null, all the packets are accepted.
235
+ * @return packet len or  <0 on error (-1 and -2 on recv error @see recvpkt4,
236
+ *         -3 if the headers are invalid and -4 if the packet doesn't
237
+ *         match the  filter).
238
+ */
239
+int raw_udp4_recv(int rsock, char** buf, int len, union sockaddr_union* from,
240
+					union sockaddr_union* to, struct raw_filter* rf)
241
+{
242
+	int n;
243
+	unsigned short dst_port;
244
+	unsigned short src_port;
245
+	struct ip_addr dst_ip;
246
+	char* end;
247
+	char* udph_start;
248
+	char* udp_payload;
249
+	struct iphdr iph;
250
+	struct udphdr udph;
251
+	unsigned short udp_len;
252
+
253
+	n=recvpkt4(rsock, *buf, len, from, to);
254
+	if (unlikely(n<0)) goto error;
255
+	
256
+	end=*buf+n;
257
+	if (unlikely(n<(sizeof(struct iphdr)+sizeof(struct udphdr)))) {
258
+		n=-3;
259
+		goto error;
260
+	}
261
+	/* FIXME: if initial buffer is aligned, one could skip the memcpy
262
+	   and directly cast iphdr and udphdr pointer to the memory */
263
+	memcpy(&iph, *buf, sizeof(struct iphdr));
264
+	udph_start=*buf+iph.ihl*4;
265
+	udp_payload=udph_start+sizeof(struct udphdr);
266
+	if (unlikely(udp_payload>end)){
267
+		n=-3;
268
+		goto error;
269
+	}
270
+	memcpy(&udph, udph_start, sizeof(struct udphdr));
271
+	udp_len=ntohs(udph.len);
272
+	if (unlikely((udph_start+udp_len)!=end)){
273
+		if ((udph_start+udp_len)>end){
274
+			n=-3;
275
+			goto error;
276
+		}else{
277
+			LOG(L_ERR, "udp length too small: %d/%d\n",
278
+					(int)udp_len, (int)(end-udph_start));
279
+			n=-3;
280
+			goto error;
281
+		}
282
+	}
283
+	/* advance buf */
284
+	*buf=udp_payload;
285
+	n=(int)(end-*buf);
286
+	/* fill dst_port */
287
+	dst_port=ntohs(udph.dest);
288
+	su_setport(to, dst_port);
289
+	/* fill src_port */
290
+	src_port=ntohs(udph.source);
291
+	su_setport(from, src_port);
292
+	if (likely(rf)) {
293
+		su2ip_addr(&dst_ip, to);
294
+		if ( (dst_port && rf->port1 && ((dst_port<rf->port1) ||
295
+										(dst_port>rf->port2)) ) ||
296
+			(matchnet(&dst_ip, &rf->dst)!=1) ){
297
+			/* no match */
298
+			n=-4;
299
+			goto error;
300
+		}
301
+	}
302
+	
303
+error:
304
+	return n;
305
+}
306
+
307
+
308
+
309
+/** udp checksum helper: compute the pseudo-header 16-bit "sum".
310
+ * Computes the partial checksum (no complement) of the pseudo-header.
311
+ * It is meant to be used by udpv4_chksum().
312
+ * @param uh - filled udp header
313
+ * @param src - source ip address in network byte order.
314
+ * @param dst - destination ip address in network byte order.
315
+ * @param length - payload lenght (not including the udp header),
316
+ *                 in _host_ order.
317
+ * @return the partial checksum in host order
318
+ */
319
+inline unsigned short udpv4_vhdr_sum(	struct udphdr* uh,
320
+										struct in_addr* src,
321
+										struct in_addr* dst,
322
+										unsigned short length)
323
+{
324
+	unsigned sum;
325
+	
326
+	/* pseudo header */
327
+	sum=(src->s_addr>>16)+(src->s_addr&0xffff)+
328
+		(dst->s_addr>>16)+(dst->s_addr&0xffff)+
329
+		htons(IPPROTO_UDP)+(uh->len);
330
+	/* udp header */
331
+	sum+=(uh->dest)+(uh->source)+(uh->len) + 0 /*chksum*/; 
332
+	/* fold it */
333
+	sum=(sum>>16)+(sum&0xffff);
334
+	sum+=(sum>>16);
335
+	/* no complement */
336
+	return ntohs((unsigned short) sum);
337
+}
338
+
339
+
340
+
341
+/** compute the udp over ipv4 checksum.
342
+ * @param u - filled udp header (except checksum).
343
+ * @param src - source ip v4 address, in _network_ byte order.
344
+ * @param dst - destination ip v4 address, int _network_ byte order.
345
+ * @param data - pointer to the udp payload.
346
+ * @param length - payload length, not including the udp header and in
347
+ *                 _host_ order. The length mist be <= 0xffff - 8
348
+ *                 (to allow space for the udp header).
349
+ * @return the checksum in _host_ order */
350
+inline static unsigned short udpv4_chksum(struct udphdr* u,
351
+							struct in_addr* src, struct in_addr* dst,
352
+							unsigned char* data, unsigned short length)
353
+{
354
+	unsigned sum;
355
+	unsigned char* end;
356
+	sum=udpv4_vhdr_sum(u, src, dst, length);
357
+	end=data+(length&(~0x1)); /* make sure it's even */
358
+	/* TODO: 16 & 32 bit aligned version */
359
+		/* not aligned */
360
+		for(;data<end;data+=2){
361
+			sum+=((data[0]<<8)+data[1]);
362
+		}
363
+		if (length&0x1)
364
+			sum+=((*data)<<8);
365
+	
366
+	/* fold it */
367
+	sum=(sum>>16)+(sum&0xffff);
368
+	sum+=(sum>>16);
369
+	return (unsigned short)~sum;
370
+}
371
+
372
+
373
+
374
+/** fill in an udp header.
375
+ * @param u - udp header that will be filled.
376
+ * @param from - source ip v4 address and port.
377
+ * @param to -   destination ip v4 address and port.
378
+ * @param buf - pointer to the payload.
379
+ * @param len - payload length (not including the udp header).
380
+ * @param do_chk - if set the udp checksum will be computed, else it will
381
+ *                 be set to 0.
382
+ * @return 0 on success, < 0 on error.
383
+ */
384
+inline static int mk_udp_hdr(struct udphdr* u, struct sockaddr_in* from, 
385
+				struct sockaddr_in* to, unsigned char* buf, int len,
386
+					int do_chk)
387
+{
388
+	u->len=htons((unsigned short)len+sizeof(struct udphdr));
389
+	u->source=from->sin_port;
390
+	u->dest=to->sin_port;
391
+	if (do_chk)
392
+		u->check=htons(
393
+				udpv4_chksum(u, &from->sin_addr, &to->sin_addr,  buf, len));
394
+	else
395
+		u->check=0; /* no checksum */
396
+	return 0;
397
+}
398
+
399
+
400
+
401
+/** send an udp packet over a raw socket.
402
+ * @param rsock - raw socket
403
+ * @param buf - data
404
+ * @param len - data len
405
+ * @param from - source address:port (_must_ be non-null, but the ip address
406
+ *                can be 0, in which case it will be filled by the kernel).
407
+ * @param to - destination address:port
408
+ * @return  <0 on error (errno set too), number of bytes sent on success
409
+ *          (including the udp header => on success len + udpheader size).
410
+ */
411
+int raw_udp4_send(int rsock, char* buf, int len, union sockaddr_union* from,
412
+					union sockaddr_union* to)
413
+{
414
+	struct msghdr snd_msg;
415
+	struct cmsghdr* cmsg;
416
+	struct in_pktinfo* snd_pktinfo;
417
+	struct iovec iov[2];
418
+	struct udphdr udp_hdr;
419
+	char msg_ctrl_snd_buf[1024];
420
+	int ret;
421
+
422
+	memset(&snd_msg, 0, sizeof(snd_msg));
423
+	snd_msg.msg_name=&to->sin;
424
+	snd_msg.msg_namelen=sockaddru_len(*to);
425
+	snd_msg.msg_iov=&iov[0];
426
+	/* prepare udp header */
427
+	mk_udp_hdr(&udp_hdr, &from->sin, &to->sin, (unsigned char*)buf, len, 1);
428
+	iov[0].iov_base=(char*)&udp_hdr;
429
+	iov[0].iov_len=sizeof(udp_hdr);
430
+	iov[1].iov_base=buf;
431
+	iov[1].iov_len=len;
432
+	snd_msg.msg_iovlen=2;
433
+	snd_msg.msg_control=msg_ctrl_snd_buf;
434
+	snd_msg.msg_controllen=sizeof(msg_ctrl_snd_buf);
435
+	/* init pktinfo cmsg */
436
+	cmsg=CMSG_FIRSTHDR(&snd_msg);
437
+	cmsg->cmsg_level=IPPROTO_IP;
438
+	cmsg->cmsg_type=IP_PKTINFO;
439
+	cmsg->cmsg_len=CMSG_LEN(sizeof(struct in_pktinfo));
440
+	snd_pktinfo=(struct in_pktinfo*)CMSG_DATA(cmsg);
441
+	snd_pktinfo->ipi_ifindex=0;
442
+	snd_pktinfo->ipi_spec_dst.s_addr=from->sin.sin_addr.s_addr;
443
+	snd_msg.msg_controllen=cmsg->cmsg_len;
444
+	snd_msg.msg_flags=0;
445
+	ret=sendmsg(rsock, &snd_msg, 0);
446
+	return ret;
447
+}
448
+
449
+
450
+
451
+#endif /* USE_RAW_SOCKS */
0 452
new file mode 100644
... ...
@@ -0,0 +1,52 @@
0
+/*
1
+ * $Id$
2
+ *
3
+ * Copyright (C) 2010 iptelorg GmbH
4
+ *
5
+ * Permission to use, copy, modify, and distribute this software for any
6
+ * purpose with or without fee is hereby granted, provided that the above
7
+ * copyright notice and this permission notice appear in all copies.
8
+ *
9
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16
+ */
17
+/** raw socket functions.
18
+ *  @file raw_sock.c
19
+ *  @ingroup core
20
+ *  Module: @ref core
21
+ */
22
+/* 
23
+ * History:
24
+ * --------
25
+ *  2010-06-07  initial version (from older code) andrei
26
+ */
27
+
28
+#ifndef _raw_sock_h
29
+#define _raw_sock_h
30
+
31
+#include "ip_addr.h"
32
+
33
+/** filter for limiting packets received on raw sockets. */
34
+struct raw_filter{
35
+	struct net   dst;
36
+	unsigned short port1;
37
+	unsigned short port2;
38
+	char proto;
39
+};
40
+
41
+
42
+int raw_socket(int proto, struct ip_addr* ip, str* iface, int iphdr_incl);
43
+int raw_udp4_socket(struct ip_addr* ip, str* iface, int iphdr_incl);
44
+int recvpkt4(int sock, char* buf, int len, union sockaddr_union* from,
45
+					union sockaddr_union* to);
46
+int raw_udp4_recv(int rsock, char** buf, int len, union sockaddr_union* from,
47
+					union sockaddr_union* to, struct raw_filter* rf);
48
+int raw_udp4_send(int rsock, char* buf, int len, union sockaddr_union* from,
49
+					union sockaddr_union* to);
50
+
51
+#endif /* _raw_sock_h */