1 #include <linux/module.h>
2 #include <linux/version.h>
4 #include <net/protocol.h>
11 MODULE_DESCRIPTION("Multi-Party Transport Protocol");
12 MODULE_AUTHOR("Adrian Bondrescu/Cornel Mercan");
13 MODULE_LICENSE("GPL");
16 struct inet_sock sock;
17 /* mptp socket speciffic data */
22 static struct mptp_sock *sock_port_map[MAX_MPTP_PORT];
24 static inline struct mptp_sock *mptp_sk(struct sock *sock)
26 return (struct mptp_sock *)(sock);
29 static inline struct mptphdr *mptp_hdr(const struct sk_buff *skb)
31 return (struct mptphdr *)skb_transport_header(skb);
34 static inline uint16_t get_next_free_port(void)
37 for (i = MIN_MPTP_PORT; i < MAX_MPTP_PORT; i++)
38 if (sock_port_map[i] == NULL)
43 static inline void mptp_unhash(uint16_t port)
45 sock_port_map[port] = NULL;
48 static inline void mptp_hash(uint16_t port, struct mptp_sock *ssh)
50 sock_port_map[port] = ssh;
53 static inline struct mptp_sock *mptp_lookup(uint16_t port)
55 return sock_port_map[port];
58 static int mptp_release(struct socket *sock)
60 struct sock *sk = sock->sk;
61 struct mptp_sock *ssk = mptp_sk(sk);
66 mptp_unhash(ssk->src);
68 sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
75 skb_queue_purge(&sk->sk_receive_queue);
77 log_debug("mptp_release sock=%p\n", sk);
83 static int mptp_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
85 struct sockaddr_mptp *mptp_addr;
86 struct mptp_sock *ssk;
92 sizeof(struct sockaddr_mptp) + sizeof(struct mptp_dest))) {
93 log_error("Invalid size for sockaddr (%d)\n", addr_len);
98 mptp_addr = (struct sockaddr_mptp *)addr;
100 log_debug("Bind received port=%u (network order)\n",
101 mptp_addr->dests[0].port);
102 port = ntohs(mptp_addr->dests[0].port);
104 port = get_next_free_port();
106 if (unlikely(port == 0 || port >= MAX_MPTP_PORT)) {
107 log_error("Invalid value for sockaddr port (%u)\n", port);
112 if (unlikely(mptp_lookup(port) != NULL)) {
113 log_error("Port %u already in use\n", port);
118 ssk = mptp_sk(sock->sk);
119 sock->sk->sk_rcvbuf = 10 * 1024 * 1024;
122 mptp_hash(port, ssk);
124 log_debug("Socket %p bound to port %u\n", ssk, port);
132 static int mptp_connect(struct socket *sock, struct sockaddr *addr,
133 int addr_len, int flags)
137 struct inet_sock *isk;
138 struct mptp_sock *ssk;
140 log_debug("mptp_connect\n");
142 if (unlikely(sock == NULL)) {
143 log_error("Sock is NULL\n");
149 if (unlikely(sk == NULL)) {
150 log_error("Sock->sk is NULL\n");
158 if (unlikely(ssk->src != 0)) {
159 log_error("ssk->src is not NULL\n");
165 struct sockaddr_mptp *mptp_addr = (struct sockaddr_mptp *)addr;
167 if (unlikely(addr_len < sizeof(*mptp_addr) ||
169 mptp_addr->count * sizeof(struct mptp_dest)
170 || mptp_addr->count <= 0)) {
171 log_error("Invalid size or address family\n");
175 ssk->dst = ntohs(mptp_addr->dests[0].port);
176 if (unlikely(ssk->dst == 0 || ssk->dst >= MAX_MPTP_PORT)) {
177 log_error("Invalid value for destination port(%u)\n",
183 isk->inet_daddr = mptp_addr->dests[0].addr;
185 ("Received from user space destination port=%u and address=%u\n",
186 ssk->dst, isk->inet_daddr);
188 log_error("Invalid mptp_addr (NULL)\n");
193 ssk->src = get_next_free_port();
194 if (unlikely(ssk->src == 0)) {
195 log_error("No free ports\n");
200 mptp_hash(ssk->src, ssk);
208 static int mptp_sendmsg(struct kiocb *iocb, struct socket *sock,
209 struct msghdr *msg, size_t len)
217 struct inet_sock *isk;
218 struct mptp_sock *ssk;
219 struct mptphdr *shdr;
222 struct rtable *rt = NULL;
225 struct sockaddr_mptp *mptp_addr = NULL;
228 if (unlikely(sock == NULL)) {
229 log_error("Sock is NULL\n");
235 if (unlikely(sk == NULL)) {
236 log_error("Sock->sk is NULL\n");
246 sport = get_next_free_port();
247 if (unlikely(sport == 0)) {
248 log_error("No free ports\n");
255 mptp_addr = (struct sockaddr_mptp *) msg->msg_name;
257 if (unlikely(msg->msg_namelen < sizeof(*mptp_addr) + mptp_addr->count * sizeof(struct mptp_dest) ||
258 mptp_addr->count <= 0)) {
259 log_error("Invalid size for msg_name (size=%u, addr_count=%u)\n", msg->msg_namelen, mptp_addr->count);
264 dests = mptp_addr->count;
267 if (unlikely(!ssk->dst || !isk->inet_daddr)) {
268 log_error("No destination port/address\n");
273 daddr = isk->inet_daddr;
275 log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
279 if (msg->msg_iovlen < dests)
280 dests = msg->msg_iovlen;
282 for (i = 0; i < dests; i++) {
283 struct mptp_dest *dest = &mptp_addr->dests[i];
284 struct iovec *iov = &msg->msg_iov[i];
287 dport = ntohs(dest->port);
288 if (unlikely(dport == 0 || dport >= MAX_MPTP_PORT)) {
289 log_error("Invalid value for destination port(%u)\n", dport);
295 log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
298 totlen = len + sizeof(struct mptphdr) + sizeof(struct iphdr);
299 skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
300 if (unlikely(!skb)) {
301 log_error("sock_alloc_send_skb failed\n");
304 log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
306 skb_reset_network_header(skb);
307 skb_reserve(skb, sizeof(struct iphdr));
308 log_debug("Reseted network header\n");
309 skb_reset_transport_header(skb);
310 skb_put(skb, sizeof(struct mptphdr));
311 log_debug("Reseted transport header\n");
313 shdr = (struct mptphdr *) skb_transport_header(skb);
314 shdr->dst = htons(dport);
315 shdr->src = htons(sport);
316 shdr->len = htons(len + sizeof(struct mptphdr));
318 payload = skb_put(skb, len);
319 log_debug("payload=%p\n", payload);
321 err = skb_copy_datagram_from_iovec(skb, sizeof(struct mptphdr), iov, 0, len);
323 log_error("skb_copy_datagram_from_iovec failed\n");
326 log_debug("Copied %u bytes into the skb\n", len);
329 rt = (struct rtable *) __sk_dst_check(sk, 0);
332 struct flowi fl = { .fl4_dst = daddr,
333 .proto = sk->sk_protocol,
334 .flags = inet_sk_flowi_flags(sk),
336 err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
338 log_error("Route lookup failed\n");
341 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
342 sk_dst_set(sk, dst_clone(&rt->u.dst));
344 sk_dst_set(sk, dst_clone(&rt->dst));
349 err = ip_queue_xmit(skb);
351 log_debug("Sent %u bytes on wire\n", len);
355 log_error("ip_queue_xmit failed\n");
369 static int mptp_recvmsg(struct kiocb *iocb, struct socket *sock,
370 struct msghdr *msg, size_t len, int flags)
373 struct sockaddr_mptp *mptp_addr;
374 struct sock *sk = sock->sk;
377 struct sockaddr_mptp *ret_addr = (struct sockaddr_mptp *)msg->msg_name;
380 log_debug("Trying to receive sock=%p sk=%p flags=%d\n", sock, sk,
383 skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
384 if (unlikely(!skb)) {
385 log_error("skb_recv_datagram failed with %d\n", err);
389 for (i = 0; i < msg->msg_iovlen; i++) {
390 log_debug("Received skb %p\n", skb);
392 mptp_addr = (struct sockaddr_mptp *)skb->cb;
395 if (copied > msg->msg_iov[i].iov_len) {
396 copied = msg->msg_iov[i].iov_len;
397 msg->msg_flags |= MSG_TRUNC;
400 err = skb_copy_datagram_iovec(skb, 0, &msg->msg_iov[i], copied);
402 log_error("skb_copy_datagram_iovec\n");
405 log_debug("Received %d bytes\n", copied);
407 sock_recv_ts_and_drops(msg, sk, skb);
410 memcpy(&ret_addr->dests[i], &mptp_addr->dests[0],
411 sizeof(ret_addr->dests[i]));
412 ret_addr->dests[i].bytes = copied;
418 skb_free_datagram(sk, skb);
420 if (i == msg->msg_iovlen - 1)
423 skb = skb_recv_datagram(sk, flags, 1, &err);
424 if (likely(err == -EAGAIN)) {
425 log_debug("No more skbs in the queue, returning...\n");
431 ret_addr->count = i + 1;
433 sizeof(struct sockaddr_mptp) + (i + 1) * sizeof(struct mptp_dest);
439 static int mptp_rcv(struct sk_buff *skb)
441 struct mptphdr *shdr;
442 struct mptp_sock *ssk;
445 struct sockaddr_mptp *mptp_addr;
447 int addr_size = sizeof(struct sockaddr_mptp) + sizeof(struct mptp_dest);
449 if (unlikely(!pskb_may_pull(skb, sizeof(struct mptphdr)))) {
450 log_error("Insufficient space for header\n");
454 shdr = (struct mptphdr *)skb->data;
455 len = ntohs(shdr->len);
457 if (unlikely(skb->len < len)) {
458 log_error("Malformed packet (packet_len=%u, skb_len=%u)\n", len,
463 if (unlikely(len < sizeof(struct mptphdr))) {
465 ("Malformed packet (packet_len=%u sizeof(mptphdr)=%u\n",
466 len, sizeof(struct mptphdr));
470 src = ntohs(shdr->src);
471 dst = ntohs(shdr->dst);
473 (src == 0 || dst == 0 || src >= MAX_MPTP_PORT
474 || dst >= MAX_MPTP_PORT)) {
475 log_error("Malformed packet (src=%u, dst=%u)\n", shdr->src,
480 skb_pull(skb, sizeof(struct mptphdr));
481 len -= sizeof(struct mptphdr);
485 log_debug("Received %u bytes from from port=%u to port=%u\n",
486 len - sizeof(struct mptphdr), src, dst);
488 ssk = mptp_lookup(dst);
490 log_error("MPTP lookup failed for port %u\n", dst);
494 BUG_ON(addr_size > sizeof(skb->cb));
496 mptp_addr = (struct sockaddr_mptp *)skb->cb;
497 mptp_addr->dests[0].port = shdr->src;
498 mptp_addr->dests[0].addr = ip_hdr(skb)->saddr;
500 log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src),
501 mptp_addr->dests[0].addr);
503 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
504 err = ip_queue_rcv_skb((struct sock *)&ssk->sock, skb);
506 err = sock_queue_rcv_skb((struct sock *)&ssk->sock, skb);
509 log_error("ip_queue_rcv_skb failed with %d\n", err);
512 return NET_RX_SUCCESS;
519 static struct proto mptp_prot = {
520 .obj_size = sizeof(struct mptp_sock),
521 .owner = THIS_MODULE,
525 static const struct proto_ops mptp_ops = {
527 .owner = THIS_MODULE,
528 .release = mptp_release,
530 .connect = mptp_connect,
531 .socketpair = sock_no_socketpair,
532 .accept = sock_no_accept,
533 .getname = sock_no_getname,
534 .poll = datagram_poll,
535 .ioctl = sock_no_ioctl,
536 .listen = sock_no_listen,
537 .shutdown = sock_no_shutdown,
538 .setsockopt = sock_no_setsockopt,
539 .getsockopt = sock_no_getsockopt,
540 .sendmsg = mptp_sendmsg,
541 .recvmsg = mptp_recvmsg,
542 .mmap = sock_no_mmap,
543 .sendpage = sock_no_sendpage,
546 static const struct net_protocol mptp_protocol = {
552 static struct inet_protosw mptp_protosw = {
554 .protocol = IPPROTO_MPTP,
560 static int __init mptp_init(void)
564 rc = proto_register(&mptp_prot, 1);
566 log_error("Error registering mptp protocol\n");
570 rc = inet_add_protocol(&mptp_protocol, IPPROTO_MPTP);
572 log_error("Error adding mptp protocol\n");
576 inet_register_protosw(&mptp_protosw);
577 log_debug("MPTP entered\n");
582 proto_unregister(&mptp_prot);
588 static void __exit mptp_exit(void)
590 inet_unregister_protosw(&mptp_protosw);
592 inet_del_protocol(&mptp_protocol, IPPROTO_MPTP);
594 proto_unregister(&mptp_prot);
596 log_debug("MPTP exited\n");
599 module_init(mptp_init);
600 module_exit(mptp_exit);