everything: shotgun attempt to put PROTO_WS and PROTO_WSS across core and in modules...
[sip-router] / raw_sock.c
1 /* 
2  * $Id$
3  *
4  * Copyright (C) 2010 iptelorg GmbH
5  *
6  * Permission to use, copy, modify, and distribute this software for any
7  * purpose with or without fee is hereby granted, provided that the above
8  * copyright notice and this permission notice appear in all copies.
9  *
10  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17  */
18 /** raw socket functions.
19  *  @file raw_sock.c
20  *  @ingroup core
21  *  Module: @ref core
22  */
23 /* 
24  * History:
25  * --------
26  *  2010-06-07  initial version (from older code) andrei
27  *  2010-06-15  IP_HDRINCL raw socket support, including on-send
28  *               fragmentation (andrei)
29  */
30
31 #ifdef USE_RAW_SOCKS
32
33 #include "compiler_opt.h"
34 #include "ip_addr.h"
35 #include "dprint.h"
36 #include "str.h"
37 #include "rand/fastrand.h"
38 #include "globals.h"
39
40 #include <errno.h>
41 #include <string.h>
42 #include <unistd.h>
43 #include <sys/types.h>
44 #include <fcntl.h>
45 #include <sys/socket.h>
46 #include <netinet/in.h>
47 #include <netinet/in_systm.h>
48 #include <arpa/inet.h>
49 #ifndef __USE_BSD
50 #define __USE_BSD  /* on linux use bsd version of iphdr (more portable) */
51 #endif /* __USE_BSD */
52 #include <netinet/ip.h>
53 #define __FAVOR_BSD /* on linux use bsd version of udphdr (more portable) */
54 #include <netinet/udp.h>
55
56 #include "raw_sock.h"
57 #include "cfg/cfg.h"
58 #include "cfg_core.h"
59
60
61 #if defined (__OS_freebsd) || defined (__OS_netbsd) || defined(__OS_openbsd) \
62         || defined (__OS_darwin)
63 /** fragmentation is done by the kernel (no need to do it in userspace) */
64 #define RAW_IPHDR_INC_AUTO_FRAG
65 #endif /* __OS_* */
66
67 /* macros for converting values in the expected format */
68 #if defined (__OS_freebsd) || defined (__OS_netbsd) || defined (__OS_darwin)
69 /* on freebsd and netbsd the ip offset (along with flags) and the
70    ip header length must be filled in _host_ bytes order format.
71    The same is true for openbsd < 2.1.
72 */
73 /** convert the ip offset in the format expected by the kernel. */
74 #define RAW_IPHDR_IP_OFF(off) (unsigned short)(off)
75 /** convert the ip total length in the format expected by the kernel. */
76 #define RAW_IPHDR_IP_LEN(tlen) (unsigned short)(tlen)
77
78 #else /* __OS_* */
79 /* linux, openbsd >= 2.1 a.s.o. */
80 /** convert the ip offset in the format expected by the kernel. */
81 #define RAW_IPHDR_IP_OFF(off)  htons((unsigned short)(off))
82 /** convert the ip total length in the format expected by the kernel. */
83 #define RAW_IPHDR_IP_LEN(tlen) htons((unsigned short)(tlen))
84
85 #endif /* __OS_* */
86
87 int raw_ipip = 0; /* set if raw socket is in capture mode for IPIP */
88
89
90 /** create and return a raw socket.
91  * @param proto - protocol used (e.g. IPPROTO_UDP, IPPROTO_RAW)
92  * @param ip - if not null the socket will be bound on this ip.
93  * @param iface - if not null the socket will be bound to this interface
94  *                (SO_BINDTODEVICE). This is supported only on linux.
95  * @param iphdr_incl - set to 1 if packets send on this socket include
96  *                     a pre-built ip header (some fields, like the checksum
97  *                     will still be filled by the kernel, OTOH packet
98  *                     fragmentation has to be done in user space).
99  * @return socket on success, -1 on error
100  */
101 int raw_socket(int proto, struct ip_addr* ip, str* iface, int iphdr_incl)
102 {
103         int sock;
104         int t;
105         union sockaddr_union su;
106 #if defined (SO_BINDTODEVICE)
107         char short_ifname[sizeof(int)];
108         int ifname_len;
109         char* ifname;
110 #endif /* SO_BINDTODEVICE */
111
112         sock = socket(PF_INET, SOCK_RAW, proto);
113         if (sock==-1)
114                 goto error;
115         /* set socket options */
116         if (iphdr_incl) {
117                 t=1;
118                 if (setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &t, sizeof(t))<0){
119                         ERR("raw_socket: setsockopt(IP_HDRINCL) failed: %s [%d]\n",
120                                         strerror(errno), errno);
121                         goto error;
122                 }
123         } else {
124                 /* IP_PKTINFO makes no sense if the ip header is included */
125                 /* using IP_PKTINFO */
126                 t=1;
127 #ifdef IP_PKTINFO
128                 if (setsockopt(sock, IPPROTO_IP, IP_PKTINFO, &t, sizeof(t))<0){
129                         ERR("raw_socket: setsockopt(IP_PKTINFO) failed: %s [%d]\n",
130                                         strerror(errno), errno);
131                         goto error;
132                 }
133 #elif defined(IP_RECVDSTADDR)
134                 if (setsockopt(sock, IPPROTO_IP, IP_RECVDSTADDR, &t, sizeof(t))<0){
135                         ERR("raw_socket: setsockop(IP_RECVDSTADDR) failed: %s [%d]\n",
136                                         strerror(errno), errno);
137                         goto error;
138                 }
139 #else
140 #error "no method of getting the destination ip address supported"
141 #endif /* IP_RECVDSTADDR / IP_PKTINFO */
142         }
143 #if defined (IP_MTU_DISCOVER) && defined (IP_PMTUDISC_DONT)
144         t=IP_PMTUDISC_DONT;
145         if(setsockopt(sock, IPPROTO_IP, IP_MTU_DISCOVER, &t, sizeof(t)) ==-1){
146                 ERR("raw_socket: setsockopt(IP_MTU_DISCOVER): %s\n",
147                                 strerror(errno));
148                 goto error;
149         }
150 #endif /* IP_MTU_DISCOVER && IP_PMTUDISC_DONT */
151         if (iface && iface->s){
152 #if defined (SO_BINDTODEVICE)
153                 /* workaround for linux bug: arg to setsockopt must have at least
154                  * sizeof(int) size or EINVAL would be returned */
155                 if (iface->len<sizeof(int)){
156                         memcpy(short_ifname, iface->s, iface->len);
157                         short_ifname[iface->len]=0; /* make sure it's zero term */
158                         ifname_len=sizeof(short_ifname);
159                         ifname=short_ifname;
160                 }else{
161                         ifname_len=iface->len;
162                         ifname=iface->s;
163                 }
164                 if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, ifname, ifname_len)
165                                                 <0){
166                                 ERR("raw_socket: could not bind to %.*s: %s [%d]\n",
167                                                         iface->len, ZSW(iface->s), strerror(errno), errno);
168                                 goto error;
169                 }
170 #else /* !SO_BINDTODEVICE */
171                 /* SO_BINDTODEVICE is linux specific => cannot bind to a device */
172                 ERR("raw_socket: bind to device supported only on linux\n");
173                 goto error;
174 #endif /* SO_BINDTODEVICE */
175         }
176         /* FIXME: probe_max_receive_buffer(sock) missing */
177         if (ip){
178                 init_su(&su, ip, 0);
179                 if (bind(sock, &su.s, sockaddru_len(su))==-1){
180                         ERR("raw_socket: bind(%s) failed: %s [%d]\n",
181                                 ip_addr2a(ip), strerror(errno), errno);
182                         goto error;
183                 }
184         }
185         return sock;
186 error:
187         if (sock!=-1) close(sock);
188         return -1;
189 }
190
191
192
193 /** create and return an udp over ipv4  raw socket.
194  * @param ip - if not null the socket will be bound on this ip.
195  * @param iface - if not null the socket will be bound to this interface
196  *                (SO_BINDTODEVICE).
197  * @param iphdr_incl - set to 1 if packets send on this socket include
198  *                     a pre-built ip header (some fields, like the checksum
199  *                     will still be filled by the kernel, OTOH packet
200  *                     fragmentation has to be done in user space).
201  * @return socket on success, -1 on error
202  */
203 int raw_udp4_socket(struct ip_addr* ip, str* iface, int iphdr_incl)
204 {
205         return raw_socket(IPPROTO_UDP, ip, iface, iphdr_incl);
206 }
207
208
209
210 /** receives an ipv4 packet using a raw socket.
211  * An ipv4 packet is received in buf, using IP_PKTINFO or IP_RECVDSTADDR.
212  * from and to are filled (only the ip part the ports are 0 since this
213  * function doesn't try to look beyond the IP level).
214  * @param sock - raw socket
215  * @param buf - detination buffer.
216  * @param len - buffer len (should be enough for receiving a packet +
217  *               IP header).
218  * @param from - result parameter, the IP address part of it will be filled
219  *                with the source address and the port with 0.
220  * @param to - result parameter, the IP address part of it will be filled
221  *                with the destination (local) address and the port with 0.
222  * @return packet len or <0 on error: -1 (check errno),
223  *        -2 no IP_PKTINFO/IP_RECVDSTADDR found or AF mismatch
224  */
225 int recvpkt4(int sock, char* buf, int len, union sockaddr_union* from,
226                                         union sockaddr_union* to)
227 {
228         struct iovec iov[1];
229         struct msghdr rcv_msg;
230         struct cmsghdr* cmsg;
231 #ifdef IP_PKTINFO
232         struct in_pktinfo* rcv_pktinfo;
233 #endif /* IP_PKTINFO */
234         int n, ret;
235         char msg_ctrl_buf[1024];
236
237         iov[0].iov_base=buf;
238         iov[0].iov_len=len;
239         rcv_msg.msg_name=from;
240         rcv_msg.msg_namelen=sockaddru_len(*from);
241         rcv_msg.msg_control=msg_ctrl_buf;
242         rcv_msg.msg_controllen=sizeof(msg_ctrl_buf);
243         rcv_msg.msg_iov=&iov[0];
244         rcv_msg.msg_iovlen=1;
245         ret=-2; /* no PKT_INFO or AF mismatch */
246 retry:
247         n=recvmsg(sock, &rcv_msg, MSG_WAITALL);
248         if (unlikely(n==-1)){
249                 if (errno==EINTR)
250                         goto retry;
251                 ret=n;
252                 goto end;
253         }
254         /* find the pkt info */
255         for (cmsg=CMSG_FIRSTHDR(&rcv_msg); cmsg; cmsg=CMSG_NXTHDR(&rcv_msg, cmsg)){
256 #ifdef IP_PKTINFO
257                 if (likely((cmsg->cmsg_level==IPPROTO_IP) &&
258                                         (cmsg->cmsg_type==IP_PKTINFO))) {
259                         rcv_pktinfo=(struct in_pktinfo*)CMSG_DATA(cmsg);
260                         to->sin.sin_family=AF_INET;
261                         memcpy(&to->sin.sin_addr, &rcv_pktinfo->ipi_spec_dst.s_addr, 
262                                                                         sizeof(to->sin.sin_addr));
263                         to->sin.sin_port=0; /* not known */
264                         /* interface no. in ipi_ifindex */
265                         ret=n; /* success */
266                         break;
267                 }
268 #elif defined (IP_RECVDSTADDR)
269                 if (likely((cmsg->cmsg_level==IPPROTO_IP) &&
270                                         (cmsg->cmsg_type==IP_RECVDSTADDR))) {
271                         to->sin.sin_family=AF_INET;
272                         memcpy(&to->sin.sin_addr, CMSG_DATA(cmsg),
273                                                                         sizeof(to->sin.sin_addr));
274                         to->sin.sin_port=0; /* not known */
275                         ret=n; /* success */
276                         break;
277                 }
278 #else
279 #error "no method of getting the destination ip address supported"
280 #endif /* IP_PKTINFO / IP_RECVDSTADDR */
281         }
282 end:
283         return ret;
284 }
285
286
287
288 /* receive an ipv4 udp packet over a raw socket.
289  * The packet is copied in *buf and *buf is advanced to point to the
290  * payload.  Fills from and to.
291  * @param rsock - raw socket
292  * @param buf - the packet will be written to where *buf points intially and
293  *              then *buf will be advanced to point to the udp payload.
294  * @param len - buffer length (should be enough to hold at least the
295  *               ip and udp headers + 1 byte).
296  * @param from - result parameter, filled with source address and port of the
297  *               packet.
298  * @param from - result parameter, filled with destination (local) address and
299  *               port of the packet.
300  * @param rf   - filter used to decide whether or not the packet is
301  *                accepted/processed. If null, all the packets are accepted.
302  * @return packet len or  <0 on error (-1 and -2 on recv error @see recvpkt4,
303  *         -3 if the headers are invalid and -4 if the packet doesn't
304  *         match the  filter).
305  */
306 int raw_udp4_recv(int rsock, char** buf, int len, union sockaddr_union* from,
307                                         union sockaddr_union* to, struct raw_filter* rf)
308 {
309         int n;
310         unsigned short dst_port;
311         unsigned short src_port;
312         struct ip_addr dst_ip;
313         char* end;
314         char* udph_start;
315         char* udp_payload;
316         struct ip iph;
317         struct udphdr udph;
318         unsigned short udp_len;
319
320         n=recvpkt4(rsock, *buf, len, from, to);
321         if (unlikely(n<0)) goto error;
322         
323         end=*buf+n;
324         if (unlikely(n<((sizeof(struct ip) * raw_ipip ? 2 : 1)+sizeof(struct udphdr)))) {
325                 n=-3;
326                 goto error;
327         }
328         
329         if(raw_ipip) 
330                 *buf = *buf + sizeof(struct ip);
331         
332         /* FIXME: if initial buffer is aligned, one could skip the memcpy
333            and directly cast ip and udphdr pointer to the memory */
334         memcpy(&iph, *buf, sizeof(struct ip));
335         udph_start=*buf+iph.ip_hl*4;
336         udp_payload=udph_start+sizeof(struct udphdr);
337         if (unlikely(udp_payload>end)){
338                 n=-3;
339                 goto error;
340         }
341         memcpy(&udph, udph_start, sizeof(struct udphdr));
342         udp_len=ntohs(udph.uh_ulen);
343         if (unlikely((udph_start+udp_len)!=end)){
344                 if ((udph_start+udp_len)>end){
345                         n=-3;
346                         goto error;
347                 }else{
348                         ERR("udp length too small: %d/%d\n",
349                                         (int)udp_len, (int)(end-udph_start));
350                         n=-3;
351                         goto error;
352                 }
353         }
354         /* advance buf */
355         *buf=udp_payload;
356         n=(int)(end-*buf);
357         /* fill ip from the packet (needed if no PKT_INFO is used) */
358         dst_ip.af=AF_INET;
359         dst_ip.len=4;
360         dst_ip.u.addr32[0]=iph.ip_dst.s_addr;
361         /* fill dst_port */
362         dst_port=ntohs(udph.uh_dport);
363         ip_addr2su(to, &dst_ip, dst_port);
364         /* fill src_port */
365         src_port=ntohs(udph.uh_sport);
366         su_setport(from, src_port);
367         if (likely(rf)) {
368                 su2ip_addr(&dst_ip, to);
369                 if ( (dst_port && rf->port1 && ((dst_port<rf->port1) ||
370                                                                                 (dst_port>rf->port2)) ) ||
371                         (matchnet(&dst_ip, &rf->dst)!=1) ){
372                         /* no match */
373                         n=-4;
374                         goto error;
375                 }
376         }
377         
378 error:
379         return n;
380 }
381
382
383
384 /** udp checksum helper: compute the pseudo-header 16-bit "sum".
385  * Computes the partial checksum (no complement) of the pseudo-header.
386  * It is meant to be used by udpv4_chksum().
387  * @param uh - filled udp header
388  * @param src - source ip address in network byte order.
389  * @param dst - destination ip address in network byte order.
390  * @param length - payload length (not including the udp header),
391  *                 in _host_ order.
392  * @return the partial checksum in host order
393  */
394 inline unsigned short udpv4_vhdr_sum(   struct udphdr* uh,
395                                                                                 struct in_addr* src,
396                                                                                 struct in_addr* dst,
397                                                                                 unsigned short length)
398 {
399         unsigned sum;
400         
401         /* pseudo header */
402         sum=(src->s_addr>>16)+(src->s_addr&0xffff)+
403                 (dst->s_addr>>16)+(dst->s_addr&0xffff)+
404                 htons(IPPROTO_UDP)+(uh->uh_ulen);
405         /* udp header */
406         sum+=(uh->uh_dport)+(uh->uh_sport)+(uh->uh_ulen) + 0 /*chksum*/; 
407         /* fold it */
408         sum=(sum>>16)+(sum&0xffff);
409         sum+=(sum>>16);
410         /* no complement */
411         return ntohs((unsigned short) sum);
412 }
413
414
415
416 /** compute the udp over ipv4 checksum.
417  * @param u - filled udp header (except checksum).
418  * @param src - source ip v4 address, in _network_ byte order.
419  * @param dst - destination ip v4 address, int _network_ byte order.
420  * @param data - pointer to the udp payload.
421  * @param length - payload length, not including the udp header and in
422  *                 _host_ order. The length mist be <= 0xffff - 8
423  *                 (to allow space for the udp header).
424  * @return the checksum in _host_ order */
425 inline static unsigned short udpv4_chksum(struct udphdr* u,
426                                                         struct in_addr* src, struct in_addr* dst,
427                                                         unsigned char* data, unsigned short length)
428 {
429         unsigned sum;
430         unsigned char* end;
431         sum=udpv4_vhdr_sum(u, src, dst, length);
432         end=data+(length&(~0x1)); /* make sure it's even */
433         /* TODO: 16 & 32 bit aligned version */
434                 /* not aligned */
435                 for(;data<end;data+=2){
436                         sum+=((data[0]<<8)+data[1]);
437                 }
438                 if (length&0x1)
439                         sum+=((*data)<<8);
440         
441         /* fold it */
442         sum=(sum>>16)+(sum&0xffff);
443         sum+=(sum>>16);
444         return (unsigned short)~sum;
445 }
446
447
448
449 /** fill in an udp header.
450  * @param u - udp header that will be filled.
451  * @param from - source ip v4 address and port.
452  * @param to -   destination ip v4 address and port.
453  * @param buf - pointer to the payload.
454  * @param len - payload length (not including the udp header).
455  * @param do_chk - if set the udp checksum will be computed, else it will
456  *                 be set to 0.
457  * @return 0 on success, < 0 on error.
458  */
459 inline static int mk_udp_hdr(struct udphdr* u, struct sockaddr_in* from, 
460                                 struct sockaddr_in* to, unsigned char* buf, int len,
461                                         int do_chk)
462 {
463         u->uh_ulen=htons((unsigned short)len+sizeof(struct udphdr));
464         u->uh_sport=from->sin_port;
465         u->uh_dport=to->sin_port;
466         if (do_chk)
467                 u->uh_sum=htons(
468                                 udpv4_chksum(u, &from->sin_addr, &to->sin_addr,  buf, len));
469         else
470                 u->uh_sum=0; /* no checksum */
471         return 0;
472 }
473
474
475
476 /** fill in an ip header.
477  * Note: the checksum is _not_ computed.
478  * WARNING: The ip header length and offset might be filled in
479  * _host_ byte order or network byte order (depending on the OS, for example
480  *  freebsd needs host byte order for raw sockets with IPHDR_INC, while
481  *  linux needs network byte order).
482  * @param iph - ip header that will be filled.
483  * @param from - source ip v4 address (network byte order).
484  * @param to -   destination ip v4 address (network byte order).
485  * @param payload len - payload length (not including the ip header).
486  * @param proto - protocol.
487  * @return 0 on success, < 0 on error.
488  */
489 inline static int mk_ip_hdr(struct ip* iph, struct in_addr* from,
490                                 struct in_addr* to, int payload_len, unsigned char proto)
491 {
492         iph->ip_hl = sizeof(struct ip)/4;
493         iph->ip_v = 4;
494         iph->ip_tos = tos;
495         /* on freebsd ip_len _must_ be in _host_ byte order instead
496            of network byte order. On linux the length is ignored (it's filled
497            automatically every time). */
498         iph->ip_len = RAW_IPHDR_IP_LEN(payload_len + sizeof(struct ip));
499         iph->ip_id = 0; /* 0 => will be filled automatically by the kernel */
500         iph->ip_off = 0; /* frag.: first 3 bits=flags=0, last 13 bits=offset */
501         iph->ip_ttl = cfg_get(core, core_cfg, udp4_raw_ttl);
502         iph->ip_p = proto;
503         iph->ip_src = *from;
504         iph->ip_dst = *to;
505         iph->ip_sum = 0;
506
507         return 0;
508 }
509
510
511
512 /** send an udp packet over a non-ip_hdrincl raw socket.
513  * @param rsock - raw socket
514  * @param buf - data
515  * @param len - data len
516  * @param from - source address:port (_must_ be non-null, but the ip address
517  *                can be 0, in which case it will be filled by the kernel).
518  * @param to - destination address:port
519  * @return  <0 on error (errno set too), number of bytes sent on success
520  *          (including the udp header => on success len + udpheader size).
521  */
522 int raw_udp4_send(int rsock, char* buf, unsigned int len,
523                                         union sockaddr_union* from,
524                                         union sockaddr_union* to)
525 {
526         struct msghdr snd_msg;
527         struct cmsghdr* cmsg;
528 #ifdef IP_PKTINFO
529         struct in_pktinfo* snd_pktinfo;
530 #endif /* IP_PKTINFO */
531         struct iovec iov[2];
532         struct udphdr udp_hdr;
533         char msg_ctrl_snd_buf[1024];
534         int ret;
535
536         memset(&snd_msg, 0, sizeof(snd_msg));
537         snd_msg.msg_name=&to->sin;
538         snd_msg.msg_namelen=sockaddru_len(*to);
539         snd_msg.msg_iov=&iov[0];
540         /* prepare udp header */
541         mk_udp_hdr(&udp_hdr, &from->sin, &to->sin, (unsigned char*)buf, len, 1);
542         iov[0].iov_base=(char*)&udp_hdr;
543         iov[0].iov_len=sizeof(udp_hdr);
544         iov[1].iov_base=buf;
545         iov[1].iov_len=len;
546         snd_msg.msg_iovlen=2;
547         snd_msg.msg_control=msg_ctrl_snd_buf;
548         snd_msg.msg_controllen=sizeof(msg_ctrl_snd_buf);
549         /* init pktinfo cmsg */
550         cmsg=CMSG_FIRSTHDR(&snd_msg);
551         cmsg->cmsg_level=IPPROTO_IP;
552 #ifdef IP_PKTINFO
553         cmsg->cmsg_type=IP_PKTINFO;
554         cmsg->cmsg_len=CMSG_LEN(sizeof(struct in_pktinfo));
555         snd_pktinfo=(struct in_pktinfo*)CMSG_DATA(cmsg);
556         snd_pktinfo->ipi_ifindex=0;
557         snd_pktinfo->ipi_spec_dst.s_addr=from->sin.sin_addr.s_addr;
558 #elif defined (IP_SENDSRCADDR)
559         cmsg->cmsg_type=IP_SENDSRCADDR;
560         cmsg->cmsg_len=CMSG_LEN(sizeof(struct in_addr));
561         memcpy(CMSG_DATA(cmsg), &from->sin.sin_addr.s_addr,
562                                                         sizeof(struct in_addr));
563 #else
564 #error "no method of setting the source ip supported"
565 #endif /* IP_PKTINFO / IP_SENDSRCADDR */
566         snd_msg.msg_controllen=cmsg->cmsg_len;
567         snd_msg.msg_flags=0;
568         ret=sendmsg(rsock, &snd_msg, 0);
569         return ret;
570 }
571
572
573
574 /** send an udp packet over an IP_HDRINCL raw socket.
575  * If needed, send several fragments.
576  * @param rsock - raw socket
577  * @param buf - data
578  * @param len - data len
579  * @param from - source address:port (_must_ be non-null, but the ip address
580  *                can be 0, in which case it will be filled by the kernel).
581  * @param to - destination address:port
582  * @param mtu - maximum datagram size (including the ip header, excluding
583  *              link layer headers). Minimum allowed size is 28
584  *               (sizeof(ip_header + udp_header)). If mtu is lower, it will
585  *               be ignored (the packet will be sent un-fragmented).
586  *              0 can be used to disable fragmentation.
587  * @return  <0 on error (-2: datagram too big, -1: check errno),
588  *          number of bytes sent on success
589  *          (including the ip & udp headers =>
590  *               on success len + udpheader + ipheader size).
591  */
592 int raw_iphdr_udp4_send(int rsock, char* buf, unsigned int len,
593                                                 union sockaddr_union* from,
594                                                 union sockaddr_union* to, unsigned short mtu)
595 {
596         struct msghdr snd_msg;
597         struct iovec iov[2];
598         struct ip_udp_hdr {
599                 struct ip ip;
600                 struct udphdr udp;
601         } hdr;
602         unsigned int totlen;
603 #ifndef RAW_IPHDR_INC_AUTO_FRAG
604         unsigned int ip_frag_size; /* fragment size */
605         unsigned int last_frag_extra; /* extra bytes possible in the last frag */
606         unsigned int ip_payload;
607         unsigned int last_frag_offs;
608         void* last_frag_start;
609         int frg_no;
610 #endif /* RAW_IPHDR_INC_AUTO_FRAG */
611         int ret;
612
613         totlen = len + sizeof(hdr);
614         if (unlikely(totlen) > 65535)
615                 return -2;
616         memset(&snd_msg, 0, sizeof(snd_msg));
617         snd_msg.msg_name=&to->sin;
618         snd_msg.msg_namelen=sockaddru_len(*to);
619         snd_msg.msg_iov=&iov[0];
620         /* prepare the udp & ip headers */
621         mk_udp_hdr(&hdr.udp, &from->sin, &to->sin, (unsigned char*)buf, len, 1);
622         mk_ip_hdr(&hdr.ip, &from->sin.sin_addr, &to->sin.sin_addr,
623                                 len + sizeof(hdr.udp), IPPROTO_UDP);
624         iov[0].iov_base=(char*)&hdr;
625         iov[0].iov_len=sizeof(hdr);
626         snd_msg.msg_iovlen=2;
627         snd_msg.msg_control=0;
628         snd_msg.msg_controllen=0;
629         snd_msg.msg_flags=0;
630         /* this part changes for different fragments */
631         /* packets are fragmented if mtu has a valid value (at least an
632            IP header + UDP header fit in it) and if the total length is greater
633            then the mtu */
634 #ifndef RAW_IPHDR_INC_AUTO_FRAG
635         if (likely(totlen <= mtu || mtu <= sizeof(hdr))) {
636 #endif /* RAW_IPHDR_INC_AUTO_FRAG */
637                 iov[1].iov_base=buf;
638                 iov[1].iov_len=len;
639                 ret=sendmsg(rsock, &snd_msg, 0);
640 #ifndef RAW_IPHDR_INC_AUTO_FRAG
641         } else {
642                 ip_payload = len + sizeof(hdr.udp);
643                 /* a fragment offset must be a multiple of 8 => its size must
644                    also be a multiple of 8, except for the last fragment */
645                 ip_frag_size = (mtu -sizeof(hdr.ip)) & (~7);
646                 last_frag_extra = (mtu - sizeof(hdr.ip)) & 7; /* rest */
647                 frg_no = ip_payload / ip_frag_size +
648                                  ((ip_payload % ip_frag_size) > last_frag_extra);
649                 /*ip_last_frag_size = ip_payload % frag_size +
650                                                         ((ip_payload % frag_size) <= last_frag_extra) *
651                                                         ip_frag_size; */
652                 last_frag_offs = (frg_no - 1) * ip_frag_size;
653                 /* if we are here mtu => sizeof(ip_h+udp_h) && payload > mtu
654                    => last_frag_offs >= sizeof(hdr.udp) */
655                 last_frag_start = buf + last_frag_offs - sizeof(hdr.udp);
656                 hdr.ip.ip_id = fastrand_max(65534) + 1; /* random id, should be != 0
657                                                                                           (if 0 the kernel will fill it) */
658                 /* send the first fragment */
659                 iov[1].iov_base=buf;
660                 /* ip_frag_size >= sizeof(hdr.udp) because we are here only
661                    if mtu >= sizeof(hdr.ip) + sizeof(hdr.udp) */
662                 iov[1].iov_len=ip_frag_size - sizeof(hdr.udp);
663                 hdr.ip.ip_len = RAW_IPHDR_IP_LEN(ip_frag_size + sizeof(hdr.ip));
664                 hdr.ip.ip_off = RAW_IPHDR_IP_OFF(0x2000); /* set MF */
665                 ret=sendmsg(rsock, &snd_msg, 0);
666                 if (unlikely(ret < 0))
667                         goto end;
668                 /* all the other fragments, include only the ip header */
669                 iov[0].iov_len = sizeof(hdr.ip);
670                 iov[1].iov_base =  (char*)iov[1].iov_base + iov[1].iov_len;
671                 /* fragments between the first and the last */
672                 while(unlikely(iov[1].iov_base < last_frag_start)) {
673                         iov[1].iov_len = ip_frag_size;
674                         hdr.ip.ip_len = RAW_IPHDR_IP_LEN(iov[1].iov_len + sizeof(hdr.ip));
675                         /* set MF  */
676                         hdr.ip.ip_off = RAW_IPHDR_IP_OFF( (unsigned short)
677                                                                         (((char*)iov[1].iov_base - (char*)buf +
678                                                                                 sizeof(hdr.udp)) / 8) | 0x2000 );
679                         ret=sendmsg(rsock, &snd_msg, 0);
680                         if (unlikely(ret < 0))
681                                 goto end;
682                         iov[1].iov_base =  (char*)iov[1].iov_base + iov[1].iov_len;
683                 }
684                 /* last fragment */
685                 iov[1].iov_len = buf + len - (char*)iov[1].iov_base;
686                 hdr.ip.ip_len = RAW_IPHDR_IP_LEN(iov[1].iov_len + sizeof(hdr.ip));
687                 /* don't set MF (last fragment) */
688                 hdr.ip.ip_off = RAW_IPHDR_IP_OFF((unsigned short)
689                                                                         (((char*)iov[1].iov_base - (char*)buf +
690                                                                                 sizeof(hdr.udp)) / 8) );
691                 ret=sendmsg(rsock, &snd_msg, 0);
692                 if (unlikely(ret < 0))
693                         goto end;
694         }
695 end:
696 #endif /* RAW_IPHDR_INC_AUTO_FRAG */
697         return ret;
698 }
699
700
701
702 #endif /* USE_RAW_SOCKS */