udp.c 22.1 KB
Newer Older
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
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
/***
 *
 *  ipv4/udp.c - UDP implementation for RTnet
 *
 *  Copyright (C) 1999, 2000 Zentropic Computing, LLC
 *                2002       Ulrich Marx <marx@kammer.uni-hannover.de>
 *                2003-2005  Jan Kiszka <jan.kiszka@web.de>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include <linux/moduleparam.h>
#include <linux/socket.h>
#include <linux/in.h>
#include <linux/ip.h>
29
#include <linux/err.h>
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
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
#include <linux/udp.h>
#include <linux/tcp.h>
#include <net/checksum.h>
#include <linux/list.h>

#include <rtskb.h>
#include <rtnet_internal.h>
#include <rtnet_port.h>
#include <rtnet_iovec.h>
#include <rtnet_socket.h>
#include <ipv4/ip_fragment.h>
#include <ipv4/ip_output.h>
#include <ipv4/ip_sock.h>
#include <ipv4/protocol.h>
#include <ipv4/route.h>
#include <ipv4/udp.h>


/***
 *  This structure is used to register a UDP socket for reception. All
 +  structures are kept in the port_registry array to increase the cache
 *  locality during the critical port lookup in rt_udp_v4_lookup().
 */
struct udp_socket {
    u16             sport;      /* local port */
    u32             saddr;      /* local ip-addr */
    struct rtsocket *sock;
    struct hlist_node link;
};

/***
 *  Automatic port number assignment

 *  The automatic assignment of port numbers to unbound sockets is realised as
 *  a simple addition of two values:
 *   - the socket ID (lower 8 bits of file descriptor) which is set during
 *     initialisation and left unchanged afterwards
 *   - the start value auto_port_start which is a module parameter

 *  auto_port_mask, also a module parameter, is used to define the range of
 *  port numbers which are used for automatic assignment. Any number within
 *  this range will be rejected when passed to bind_rt().

 */
static unsigned int         auto_port_start = 1024;
static unsigned int         auto_port_mask  = ~(RT_UDP_SOCKETS-1);
static int                  free_ports      = RT_UDP_SOCKETS;
#define RT_PORT_BITMAP_WORDS \
    ((RT_UDP_SOCKETS + BITS_PER_LONG - 1) / BITS_PER_LONG)
static unsigned long        port_bitmap[RT_PORT_BITMAP_WORDS];
static struct udp_socket    port_registry[RT_UDP_SOCKETS];
static DEFINE_RTDM_LOCK(udp_socket_base_lock);

static struct hlist_head port_hash[RT_UDP_SOCKETS * 2];
#define port_hash_mask (RT_UDP_SOCKETS * 2 - 1)

MODULE_LICENSE("GPL");

module_param(auto_port_start, uint, 0444);
module_param(auto_port_mask, uint, 0444);
MODULE_PARM_DESC(auto_port_start, "Start of automatically assigned port range");
MODULE_PARM_DESC(auto_port_mask,
92
                 "Mask that defines port range for automatic assignment");
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
93
94
95

static inline struct udp_socket *port_hash_search(u32 saddr, u16 sport)
{
96
97
        unsigned bucket = sport & port_hash_mask;
        struct udp_socket *sock;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
98

99
100
101
102
103
104
        hlist_for_each_entry(sock, &port_hash[bucket], link)
                if (sock->sport == sport &&
                    (saddr == INADDR_ANY
                     || sock->saddr == saddr
                     || sock->saddr == INADDR_ANY))
                        return sock;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
105

106
        return NULL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
107
108
109
110
}

static inline int port_hash_insert(struct udp_socket *sock, u32 saddr, u16 sport)
{
111
        unsigned bucket;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
112

113
114
        if (port_hash_search(saddr, sport))
                return -EADDRINUSE;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
115

116
117
118
119
120
        bucket = sport & port_hash_mask;
        sock->saddr = saddr;
        sock->sport = sport;
        hlist_add_head(&sock->link, &port_hash[bucket]);
        return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
121
122
123
124
}

static inline void port_hash_del(struct udp_socket *sock)
{
125
        hlist_del(&sock->link);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
126
127
128
129
130
131
132
133
134
135
136
137
}

/***
 *  rt_udp_v4_lookup
 */
static inline struct rtsocket *rt_udp_v4_lookup(u32 daddr, u16 dport)
{
    rtdm_lockctx_t  context;
    struct udp_socket *sock;

    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);
    sock = port_hash_search(daddr, dport);
138
    if (sock && rt_socket_reference(sock->sock) == 0) {
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
139

140
            rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
141

142
            return sock->sock;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
143
144
145
    }

    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
146

Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
147
148
149
150
151
152
153
154
155
156
    return NULL;
}



/***
 *  rt_udp_bind - bind socket to local address
 *  @s:     socket
 *  @addr:  local address
 */
157
158
int rt_udp_bind(struct rtdm_fd *fd, struct rtsocket *sock,
		const struct sockaddr __user *addr, socklen_t addrlen)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
159
{
160
    struct sockaddr_in  _sin, *sin;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
161
162
163
164
165
    rtdm_lockctx_t      context;
    int                 index;
    int                 err = 0;


166
167
168
169
170
171
172
173
174
    if (addrlen < sizeof(struct sockaddr_in))
	    return -EINVAL;
    
    sin = rtnet_get_arg(fd, &_sin, addr, sizeof(_sin));
    if (IS_ERR(sin))
	    return PTR_ERR(sin);

    if ((sin->sin_port & auto_port_mask) == auto_port_start)
	    return -EINVAL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
175
176
177
178

    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);

    if ((index = sock->prot.inet.reg_index) < 0) {
179
180
181
        /* socket is being closed */
        err = -EBADF;
        goto unlock_out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
182
183
    }
    if (sock->prot.inet.state != TCP_CLOSE) {
184
185
        err = -EINVAL;
        goto unlock_out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
186
187
188
189
    }

    port_hash_del(&port_registry[index]);
    if (port_hash_insert(&port_registry[index],
190
191
                         sin->sin_addr.s_addr,
                         sin->sin_port ?: index + auto_port_start)) {
192
193
194
195
196
            port_hash_insert(&port_registry[index],
                             port_registry[index].saddr,
                             port_registry[index].sport);
            rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
            return -EADDRINUSE;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
    }

    /* set the source-addr */
    sock->prot.inet.saddr = port_registry[index].saddr;

    /* set source port, if not set by user */
    sock->prot.inet.sport = port_registry[index].sport;

 unlock_out:
    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);

    return err;
}



/***
 *  rt_udp_connect
 */
216
217
int rt_udp_connect(struct rtdm_fd *fd, struct rtsocket *sock,
		   const struct sockaddr __user *serv_addr, socklen_t addrlen)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
218
{
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
	struct sockaddr _sa, *sa;
	struct sockaddr_in _sin, *sin;
	rtdm_lockctx_t      context;
	int                 index;

	if (addrlen < sizeof(struct sockaddr))
		return -EINVAL;
	
	sa = rtnet_get_arg(fd, &_sa, serv_addr, sizeof(_sa));
	if (IS_ERR(sa))
		return PTR_ERR(sa);

	if (sa->sa_family == AF_UNSPEC) {
		if ((index = sock->prot.inet.reg_index) < 0)
			/* socket is being closed */
			return -EBADF;

		rtdm_lock_get_irqsave(&udp_socket_base_lock, context);

		sock->prot.inet.saddr = INADDR_ANY;
		/* Note: The following line differs from standard
		   stacks, and we also don't remove the socket from
		   the port list. Might get fixed in the future... */
		sock->prot.inet.sport = index + auto_port_start;
		sock->prot.inet.daddr = INADDR_ANY;
		sock->prot.inet.dport = 0;
		sock->prot.inet.state = TCP_CLOSE;

		rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
	} else {
		if (addrlen < sizeof(struct sockaddr_in))
			return -EINVAL;

		sin = rtnet_get_arg(fd, &_sin, serv_addr, sizeof(_sin));
		if (IS_ERR(sin))
			return PTR_ERR(sin);

		if (sin->sin_family != AF_INET)
			return -EINVAL;
		
		rtdm_lock_get_irqsave(&udp_socket_base_lock, context);

		if (sock->prot.inet.state != TCP_CLOSE) {
			rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
			return -EINVAL;
		}

		sock->prot.inet.state = TCP_ESTABLISHED;
		sock->prot.inet.daddr = sin->sin_addr.s_addr;
		sock->prot.inet.dport = sin->sin_port;

		rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
	}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
272

273
	return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
274
275
276
277
278
279
280
281
}



/***
 *  rt_udp_socket - create a new UDP-Socket
 *  @s: socket
 */
282
int rt_udp_socket(struct rtdm_fd *fd)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
283
{
284
    struct rtsocket *sock = rtdm_fd_to_private(fd);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
285
286
287
288
289
290
    int             ret;
    int             i;
    int             index;
    rtdm_lockctx_t  context;


291
    if ((ret = rt_socket_init(fd, IPPROTO_UDP)) != 0)
292
        return ret;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
293
294
295
296
297
298
299
300
301

    sock->prot.inet.saddr = INADDR_ANY;
    sock->prot.inet.state = TCP_CLOSE;
    sock->prot.inet.tos   = 0;

    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);

    /* enforce maximum number of UDP sockets */
    if (free_ports == 0) {
302
303
304
        rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
        rt_socket_cleanup(fd);
        return -EAGAIN;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
305
306
307
308
309
    }
    free_ports--;

    /* find free auto-port in bitmap */
    for (i = 0; i < RT_PORT_BITMAP_WORDS; i++)
310
311
        if (port_bitmap[i] != (unsigned long)-1)
            break;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
    index = ffz(port_bitmap[i]);
    set_bit(index, &port_bitmap[i]);
    index += i*32;
    sock->prot.inet.reg_index = index;
    sock->prot.inet.sport     = index + auto_port_start;

    /* register UDP socket */
    port_hash_insert(&port_registry[index], INADDR_ANY, sock->prot.inet.sport);
    port_registry[index].sock  = sock;

    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);

    return 0;
}



/***
 *  rt_udp_close
 */
332
void rt_udp_close(struct rtdm_fd *fd)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
333
{
334
    struct rtsocket *sock = rtdm_fd_to_private(fd);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
335
336
337
338
339
340
341
342
343
344
    struct rtskb    *del;
    int             port;
    rtdm_lockctx_t  context;


    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);

    sock->prot.inet.state = TCP_CLOSE;

    if (sock->prot.inet.reg_index >= 0) {
345
346
347
        port = sock->prot.inet.reg_index;
        clear_bit(port % BITS_PER_LONG, &port_bitmap[port / BITS_PER_LONG]);
        port_hash_del(&port_registry[port]);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
348

349
        free_ports++;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
350

351
        sock->prot.inet.reg_index = -1;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
352
353
354
355
356
357
358
359
360
    }

    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);

    /* cleanup already collected fragments */
    rt_ip_frag_invalidate_socket(sock);

    /* free packets in incoming queue */
    while ((del = rtskb_dequeue(&sock->incoming)) != NULL)
361
        kfree_rtskb(del);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
362

363
    rt_socket_cleanup(fd);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
364
365
366
367
}



368
int rt_udp_ioctl(struct rtdm_fd *fd, unsigned int request, void __user *arg)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
369
{
370
371
372
	struct rtsocket *sock = rtdm_fd_to_private(fd);
	const struct _rtdm_setsockaddr_args *setaddr;
	struct _rtdm_setsockaddr_args _setaddr;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
373

374
375
376
	/* fast path for common socket IOCTLs */
	if (_IOC_TYPE(request) == RTIOC_TYPE_NETWORK)
		return rt_socket_common_ioctl(fd, request, arg);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
377

378
	switch (request) {
379
380
        case _RTIOC_BIND:
        case _RTIOC_CONNECT:
381
382
383
384
		setaddr = rtnet_get_arg(fd, &_setaddr, arg, sizeof(_setaddr));
		if (IS_ERR(setaddr))
			return PTR_ERR(setaddr);
		if (request == _RTIOC_BIND)
385
			return rt_udp_bind(fd, sock, setaddr->addr, setaddr->addrlen);
386

387
		return rt_udp_connect(fd, sock, setaddr->addr, setaddr->addrlen);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
388

389
        default:
390
391
		return rt_ip_ioctl(fd, request, arg);
	}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
392
393
394
395
396
397
398
}



/***
 *  rt_udp_recvmsg
 */
399
400
401
402
/***
 *  rt_udp_recvmsg
 */
ssize_t rt_udp_recvmsg(struct rtdm_fd *fd, struct user_msghdr *u_msg, int msg_flags)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
403
{
404
    struct rtsocket     *sock = rtdm_fd_to_private(fd);
405
    size_t              len;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
406
407
408
409
410
411
    struct rtskb        *skb;
    struct rtskb        *first_skb;
    size_t              copied = 0;
    size_t              block_size;
    size_t              data_len;
    struct udphdr       *uh;
412
    struct sockaddr_in  sin;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
413
    nanosecs_rel_t      timeout = sock->timeout;
414
415
416
417
418
419
420
421
422
423
424
    int                 ret, flags;
    struct user_msghdr _msg, *msg;
    socklen_t namelen;
    struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov;

    msg = rtnet_get_arg(fd, &_msg, u_msg, sizeof(_msg));
    if (IS_ERR(msg))
	    return PTR_ERR(msg);

    if (msg->msg_iovlen < 0)
	    return -EINVAL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
425

426
427
428
429
430
431
    if (msg->msg_iovlen == 0)
	    return 0;

    ret = rtdm_get_iovec(fd, &iov, msg, iov_fast);
    if (ret)
	    return ret;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
432
433

    /* non-blocking receive? */
434
    if (msg_flags & MSG_DONTWAIT)
435
        timeout = -1;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
436
437
438

    ret = rtdm_sem_timeddown(&sock->pending_sem, timeout, NULL);
    if (unlikely(ret < 0))
439
440
441
442
443
444
445
446
447
	switch (ret) {
	    default:
		ret = -EBADF;   /* socket has been closed */
	    case -EWOULDBLOCK:
	    case -ETIMEDOUT:
	    case -EINTR:
		rtdm_drop_iovec(iov, iov_fast);
		return ret;
	}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
448
449
450
451

    skb = rtskb_dequeue_chain(&sock->incoming);
    RTNET_ASSERT(skb != NULL, return -EFAULT;);
    uh = skb->h.uh;
452
    first_skb = skb;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
453

454
    /* copy the address if required. */
455
456
457
458
459
460
461
462
    if (msg->msg_name) {
	    memset(&sin, 0, sizeof(sin));
	    sin.sin_family      = AF_INET;
	    sin.sin_port        = uh->source;
	    sin.sin_addr.s_addr = skb->nh.iph->saddr;
	    ret = rtnet_put_arg(fd, msg->msg_name, &sin, sizeof(sin));
	    if (ret)
		    goto fail;
463
464
465
466
467
468

	    namelen = sizeof(sin);
	    ret = rtnet_put_arg(fd, &u_msg->msg_namelen, &namelen, sizeof(namelen));
	    if (ret)
		    goto fail;
       }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
469

470
471
    data_len = ntohs(uh->len) - sizeof(struct udphdr);

Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
472
473
474
    /* remove the UDP header */
    __rtskb_pull(skb, sizeof(struct udphdr));

475
    flags = msg->msg_flags & ~MSG_TRUNC;
476
    len = rtdm_get_iov_flatlen(iov, msg->msg_iovlen);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
477
478
479

    /* iterate over all IP fragments */
    do {
480
        rtskb_trim(skb, data_len);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
481

482
483
484
        block_size = skb->len;
        copied += block_size;
        data_len -= block_size;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
485

486
487
488
489
        /* The data must not be longer than the available buffer size */
        if (copied > len) {
            block_size -= copied - len;
            copied = len;
490
	    flags |= MSG_TRUNC;
491
        }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
492

493
        /* copy the data */
494
495
496
	ret = rtnet_write_to_iov(fd, iov, msg->msg_iovlen, skb->data, block_size);
	if (ret)
		goto fail;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
497

498
499
        /* next fragment */
        skb = skb->next;
500
    } while (skb && !(flags & MSG_TRUNC));
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
501
502
503

    /* did we copied all bytes? */
    if (data_len > 0)
504
	    flags |= MSG_TRUNC;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
505

506
507
508
509
510
511
    if (flags != msg->msg_flags) {
	    ret = rtnet_put_arg(fd, &u_msg->msg_flags, &flags, sizeof(flags));
	    if (ret)
		    goto fail;
    }
out:
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
512
    if ((msg_flags & MSG_PEEK) == 0)
513
        kfree_rtskb(first_skb);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
514
    else {
515
516
517
        __rtskb_push(first_skb, sizeof(struct udphdr));
        rtskb_queue_head(&sock->incoming, first_skb);
        rtdm_sem_up(&sock->pending_sem);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
518
    }
519
    rtdm_drop_iovec(iov, iov_fast);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
520
521

    return copied;
522
523
524
fail:
    copied = ret;
    goto out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
525
526
527
528
529
530
531
532
533
534
535
536
}



/***
 *  struct udpfakehdr
 */
struct udpfakehdr
{
    struct udphdr uh;
    u32 daddr;
    u32 saddr;
537
    struct rtdm_fd *fd;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
538
539
540
541
542
543
544
545
546
547
548
    struct iovec *iov;
    int iovlen;
    u32 wcheck;
};



/***
 *
 */
static int rt_udp_getfrag(const void *p, unsigned char *to,
549
                          unsigned int offset, unsigned int fraglen)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
550
551
{
    struct udpfakehdr *ufh = (struct udpfakehdr *)p;
552
    int ret;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
553
554
555


    // We should optimize this function a bit (copy+csum...)!
556
557
558
559
    if (offset) {
	    ret = rtnet_read_from_iov(ufh->fd, ufh->iov, ufh->iovlen, to, fraglen);
	    return ret < 0 ? ret : 0;
    }
560
561
562
563

    ret = rtnet_read_from_iov(ufh->fd, ufh->iov, ufh->iovlen,
			      to + sizeof(struct udphdr),
			      fraglen - sizeof(struct udphdr));
564
    if (ret < 0)
565
	    return ret;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
566

567
568
569
570
571
    /* Checksum of the complete data part of the UDP message: */
    ufh->wcheck = csum_partial(to + sizeof(struct udphdr),
			       fraglen - sizeof(struct udphdr),
			       ufh->wcheck);

572
573
574
575
576
577
    /* Checksum of the udp header: */
    ufh->wcheck = csum_partial((unsigned char *)ufh,
			       sizeof(struct udphdr), ufh->wcheck);
    
    ufh->uh.check = csum_tcpudp_magic(ufh->saddr, ufh->daddr, ntohs(ufh->uh.len),
				      IPPROTO_UDP, ufh->wcheck);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
578

579
    if (ufh->uh.check == 0)
580
            ufh->uh.check = -1;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
581

582
    memcpy(to, ufh, sizeof(struct udphdr));
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
583
584
585
586
587
588
589
590
591

    return 0;
}



/***
 *  rt_udp_sendmsg
 */
592
ssize_t rt_udp_sendmsg(struct rtdm_fd *fd, const struct user_msghdr *msg, int msg_flags)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
593
{
594
    struct rtsocket     *sock = rtdm_fd_to_private(fd);
595
596
597
    size_t              len;
    int                 ulen;
    struct sockaddr_in  _sin, *sin;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
598
599
600
601
602
603
604
    struct udpfakehdr   ufh;
    struct dest_route   rt;
    u32                 saddr;
    u32                 daddr;
    u16                 dport;
    int                 err;
    rtdm_lockctx_t      context;
605
606
    struct user_msghdr _msg;
    struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
607
608

    if (msg_flags & MSG_OOB)   /* Mirror BSD error message compatibility */
609
        return -EOPNOTSUPP;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
610
611

    if (msg_flags & ~(MSG_DONTROUTE|MSG_DONTWAIT) )
612
        return -EINVAL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
613

614
615
616
617
618
619
620
621
622
623
624
625
626
627
    msg = rtnet_get_arg(fd, &_msg, msg, sizeof(*msg));
    if (IS_ERR(msg))
	    return PTR_ERR(msg);

    if (msg->msg_iovlen < 0)
	    return -EINVAL;

    if (msg->msg_iovlen == 0)
	    return 0;
    
    err = rtdm_get_iovec(fd, &iov, msg, iov_fast);
    if (err)
	    return err;

628
    len = rtdm_get_iov_flatlen(iov, msg->msg_iovlen);
629
630
631
632
633
634
    if ((len < 0) || (len > 0xFFFF-sizeof(struct iphdr)-sizeof(struct udphdr))) {
	    err = -EMSGSIZE;
	    goto out;
    }

    ulen = len + sizeof(struct udphdr);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
635

636
637
638
639
640
641
    if (msg->msg_name && msg->msg_namelen == sizeof(*sin)) {
	    sin = rtnet_get_arg(fd, &_sin, msg->msg_name, sizeof(_sin));
	    if (IS_ERR(sin)) {
		    err = PTR_ERR(sin);
		    goto out;
	    }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
642

643
644
645
646
	    if (sin->sin_family != AF_INET && sin->sin_family != AF_UNSPEC) {
		    err = -EINVAL;
		    goto out;
	    }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
647

648
649
650
	    daddr = sin->sin_addr.s_addr;
	    dport = sin->sin_port;
	    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
651
    } else {
652
	    rtdm_lock_get_irqsave(&udp_socket_base_lock, context);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
653

654
655
656
657
658
	    if (sock->prot.inet.state != TCP_ESTABLISHED) {
		    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);
		    err = -ENOTCONN;
		    goto out;
	    }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
659

660
661
	    daddr = sock->prot.inet.daddr;
	    dport = sock->prot.inet.dport;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
662
    }
663
    
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
664
665
666
667
668
    saddr         = sock->prot.inet.saddr;
    ufh.uh.source = sock->prot.inet.sport;

    rtdm_lock_put_irqrestore(&udp_socket_base_lock, context);

669
670
671
672
    if ((daddr | dport) == 0) {
	    err = -EINVAL;
	    goto out;
    }
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
673
674
675
676

    /* get output route */
    err = rt_ip_route_output(&rt, daddr, saddr);
    if (err)
677
	    goto out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
678
679
680
681
682
683
684

    /* we found a route, remember the routing dest-addr could be the netmask */
    ufh.saddr     = saddr != INADDR_ANY ? saddr : rt.rtdev->local_ip;
    ufh.daddr     = daddr;
    ufh.uh.dest   = dport;
    ufh.uh.len    = htons(ulen);
    ufh.uh.check  = 0;
685
    ufh.fd        = fd;
686
    ufh.iov       = iov;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
687
688
689
690
691
    ufh.iovlen    = msg->msg_iovlen;
    ufh.wcheck    = 0;

    err = rt_ip_build_xmit(sock, rt_udp_getfrag, &ufh, ulen, &rt, msg_flags);

692
    /* Drop the reference obtained in rt_ip_route_output() */
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
693
    rtdev_dereference(rt.rtdev);
694
695
out:
    rtdm_drop_iovec(iov, iov_fast);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
696

697
    return err ?: len;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
698
699
700
701
702
703
704
705
}



/***
 *  rt_udp_check
 */
static inline unsigned short rt_udp_check(struct udphdr *uh, int len,
706
707
708
                                          unsigned long saddr,
                                          unsigned long daddr,
                                          unsigned long base)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
{
    return(csum_tcpudp_magic(saddr, daddr, len, IPPROTO_UDP, base));
}



struct rtsocket *rt_udp_dest_socket(struct rtskb *skb)
{
    struct udphdr           *uh   = skb->h.uh;
    unsigned short          ulen  = ntohs(uh->len);
    u32                     saddr = skb->nh.iph->saddr;
    u32                     daddr = skb->nh.iph->daddr;
    struct rtnet_device*    rtdev = skb->rtdev;


    if (uh->check == 0)
725
        skb->ip_summed = CHECKSUM_UNNECESSARY;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
726
727
/* ip_summed (yet) never equals CHECKSUM_PARTIAL
    else
728
729
        if (skb->ip_summed == CHECKSUM_PARTIAL) {
            skb->ip_summed = CHECKSUM_UNNECESSARY;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
730

731
732
            if ( !rt_udp_check(uh, ulen, saddr, daddr, skb->csum) )
                return NULL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
733

734
735
            skb->ip_summed = CHECKSUM_NONE;
        }*/
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
736
737

    if (skb->ip_summed != CHECKSUM_UNNECESSARY)
738
        skb->csum = csum_tcpudp_nofold(saddr, daddr, ulen, IPPROTO_UDP, 0);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
739
740
741

    /* patch broadcast daddr */
    if (daddr == rtdev->broadcast_ip)
742
        daddr = rtdev->local_ip;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
743
744
745
746
747
748
749
750
751
752
753
754
755

    /* find the destination socket */
    skb->sk = rt_udp_v4_lookup(daddr, uh->dest);

    return skb->sk;
}

/***
 *  rt_udp_rcv
 */
void rt_udp_rcv (struct rtskb *skb)
{
    struct rtsocket *sock = skb->sk;
756
    void            (*callback_func)(struct rtdm_fd *, void *);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
757
758
759
760
761
762
763
764
765
766
767
768
    void            *callback_arg;
    rtdm_lockctx_t  context;

    rtskb_queue_tail(&sock->incoming, skb);
    rtdm_sem_up(&sock->pending_sem);

    rtdm_lock_get_irqsave(&sock->param_lock, context);
    callback_func = sock->callback_func;
    callback_arg  = sock->callback_arg;
    rtdm_lock_put_irqrestore(&sock->param_lock, context);

    if (callback_func)
769
        callback_func(rt_socket_fd(sock), callback_arg);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
}



/***
 *  rt_udp_rcv_err
 */
void rt_udp_rcv_err (struct rtskb *skb)
{
    rtdm_printk("RTnet: rt_udp_rcv err\n");
}



/***
 *  UDP-Initialisation
 */
static struct rtinet_protocol udp_protocol = {
    .protocol =     IPPROTO_UDP,
    .dest_socket =  &rt_udp_dest_socket,
    .rcv_handler =  &rt_udp_rcv,
    .err_handler =  &rt_udp_rcv_err,
    .init_socket =  &rt_udp_socket
};

795
796
static struct rtdm_driver udp_driver = {
    .profile_info =     RTDM_PROFILE_INFO(udp,
797
798
799
                                        RTDM_CLASS_NETWORK,
                                        RTDM_SUBCLASS_RTNET,
                                        RTNET_RTDM_VER),
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
800
    .device_flags =     RTDM_PROTOCOL_DEVICE,
801
    .device_count =	1,
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
802
803
804
805
806
807
808
    .context_size =     sizeof(struct rtsocket),

    .protocol_family =  PF_INET,
    .socket_type =      SOCK_DGRAM,

    /* default is UDP */
    .ops = {
809
810
811
812
813
814
815
        .socket =       rt_inet_socket,
        .close =        rt_udp_close,
        .ioctl_rt =     rt_udp_ioctl,
        .ioctl_nrt =    rt_udp_ioctl,
        .recvmsg_rt =   rt_udp_recvmsg,
        .sendmsg_rt =   rt_udp_sendmsg,
        .select =       rt_socket_select_bind,
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
816
    },
817
};
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
818

819
820
821
static struct rtdm_device udp_device = {
    .driver = &udp_driver,
    .label = "udp",
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
822
823
824
825
826
827
828
829
830
};

/***
 *  rt_udp_init
 */
static int __init rt_udp_init(void)
{
    int i;
    if ((auto_port_start < 0) || (auto_port_start >= 0x10000 - RT_UDP_SOCKETS))
831
        auto_port_start = 1024;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
832
833
834
835
836
837
    auto_port_start = htons(auto_port_start & (auto_port_mask & 0xFFFF));
    auto_port_mask  = htons(auto_port_mask | 0xFFFF0000);

    rt_inet_add_protocol(&udp_protocol);

    for (i = 0; i < ARRAY_SIZE(port_hash); i++)
838
            INIT_HLIST_HEAD(&port_hash[i]);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
839
840
841
842
843
844
845
846
847
848
849

    return rtdm_dev_register(&udp_device);
}



/***
 *  rt_udp_release
 */
static void __exit rt_udp_release(void)
{
850
    rtdm_dev_unregister(&udp_device);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
851
852
853
854
855
    rt_inet_del_protocol(&udp_protocol);
}

module_init(rt_udp_init);
module_exit(rt_udp_release);