From cbe90da626146b19786c7a9816d7c0fdaaa4fe44 Mon Sep 17 00:00:00 2001 From: theMIROn Date: Sat, 14 May 2011 16:00:01 -0400 Subject: [PATCH] l2tp: backport Asus' l2tp driver --- .../src-rt/linux/linux-2.6/drivers/net/pppol2tp.c | 719 +++++++++++---------- 1 file changed, 362 insertions(+), 357 deletions(-) diff --git a/release/src-rt/linux/linux-2.6/drivers/net/pppol2tp.c b/release/src-rt/linux/linux-2.6/drivers/net/pppol2tp.c index 77c81fad31..7d69f2847e 100644 --- a/release/src-rt/linux/linux-2.6/drivers/net/pppol2tp.c +++ b/release/src-rt/linux/linux-2.6/drivers/net/pppol2tp.c @@ -5,8 +5,9 @@ * PPPoL2TP --- PPP over L2TP (RFC 2661) * * - * Version: 0.17.1 + * Version: 0.18.3 * + * 230411 : Added ASUS backports from 2.6.3x. * 251003 : Copied from pppoe.c version 0.6.9. * * Authors: Martijn van Oosterhout @@ -97,14 +98,25 @@ #include #include - -#define PPPOL2TP_DRV_VERSION "V0.17" +#define PPPOL2TP_DRV_VERSION "V0.18.3" /* Developer debug code. */ #if 0 #define DEBUG /* Define to compile in very verbose developer debug */ #endif +/* Old L2TP daemons semi-compatibility */ +//#define PPPOL2TP_UDP_CONNECT + +/* Pre 2.6.22 kernels compatibility */ +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22) +#define udp_hdr(skb) (struct udphdr *) (skb)->h.raw +#define skb_network_header(skb) skb->nh.raw +#define skb_transport_header(skb) skb->h.raw +#define skb_reset_network_header(skb) (skb)->nh.raw = (skb)->data +#define skb_reset_transport_header(skb) (skb)->h.raw = (skb)->data +#endif + /* Timeouts are specified in milliseconds to/from userspace */ #define JIFFIES_TO_MS(t) ((t) * 1000 / HZ) #define MS_TO_JIFFIES(j) ((j * HZ) / 1000) @@ -175,20 +187,6 @@ #define PPPOL2TP_L2TP_HDR_SIZE_SEQ 10 #define PPPOL2TP_L2TP_HDR_SIZE_NOSEQ 6 -struct pppol2tp_send { - struct pppol2tp_session *session; - struct pppol2tp_tunnel *tunnel; - struct msghdr *msg; - struct sk_buff *skb; - struct sock *tunnel_sock; - struct iovec *iov; - struct work_struct send_task; - int total_len; - struct kiocb iocb; - struct sock_iocb siocb; - u8 hdr[PPPOL2TP_L2TP_HDR_SIZE_SEQ]; -}; - struct pppol2tp_tunnel; /* Describes a session. It is the sk_user_data field in the PPPoL2TP @@ -217,11 +215,11 @@ struct pppol2tp_session int mru; int flags; /* accessed by PPPIOCGFLAGS. * Unused. */ - int recv_seq:1; /* expect receive packets with + unsigned recv_seq:1; /* expect receive packets with * sequence numbers? */ - int send_seq:1; /* send packets with sequence + unsigned send_seq:1; /* send packets with sequence * numbers? */ - int lns_mode:1; /* behave as LNS? LAC enables + unsigned lns_mode:1; /* behave as LNS? LAC enables * sequence numbers under * control of LNS. */ int debug; /* bitmask of debug message @@ -242,10 +240,6 @@ struct pppol2tp_tunnel { int magic; /* Should be L2TP_TUNNEL_MAGIC */ - struct workqueue_struct *wq; /* Per-tunnel work queue */ - - struct proto *old_proto; /* original proto */ - struct proto l2tp_proto; /* L2TP proto */ rwlock_t hlist_lock; /* protect session_hlist */ struct hlist_head session_hlist[PPPOL2TP_HASH_SIZE]; /* hashed list of sessions, @@ -272,8 +266,8 @@ struct pppol2tp_tunnel struct pppol2tp_skb_cb { u16 ns; u16 nr; - int has_seq; - int length; + u16 has_seq; + u16 length; unsigned long expires; }; @@ -334,12 +328,16 @@ pppol2tp_session_find(struct pppol2tp_tunnel *tunnel, u16 session_id) struct hlist_node *walk; struct pppol2tp_session *session; + //TODO: Switch to RCU + read_lock_bh(&tunnel->hlist_lock); hlist_for_each_safe(walk, tmp, session_list) { session = hlist_entry(walk, struct pppol2tp_session, hlist); if (session->tunnel_addr.s_session == session_id) { + read_unlock_bh(&tunnel->hlist_lock); return session; } } + read_unlock_bh(&tunnel->hlist_lock); return NULL; } @@ -348,32 +346,28 @@ pppol2tp_session_find(struct pppol2tp_tunnel *tunnel, u16 session_id) * Receive data handling *****************************************************************************/ -/* Queue a skb in order. If the skb has no sequence number, queue it - * at the tail. +/* Queue a skb in order. We come here only if the skb has an L2TP sequence + * number. */ static void pppol2tp_recv_queue_skb(struct pppol2tp_session *session, struct sk_buff *skb) { - struct sk_buff *next; - struct sk_buff *prev; + struct sk_buff *skbp; + struct sk_buff *tmp; u16 ns = PPPOL2TP_SKB_CB(skb)->ns; ENTER_FUNCTION; spin_lock(&session->reorder_q.lock); - - prev = (struct sk_buff *) &session->reorder_q; - next = prev->next; - while (next != prev) { - if (PPPOL2TP_SKB_CB(next)->ns > ns) { - __skb_insert(skb, next->prev, next, &session->reorder_q); + skb_queue_walk_safe(&session->reorder_q, skbp, tmp) { + if (PPPOL2TP_SKB_CB(skbp)->ns > ns) { + __skb_insert(skb, skbp->prev, skbp, &session->reorder_q); PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG, "%s: pkt %hu, inserted before %hu, reorder_q len=%d\n", - session->name, ns, PPPOL2TP_SKB_CB(next)->ns, + session->name, ns, PPPOL2TP_SKB_CB(skbp)->ns, skb_queue_len(&session->reorder_q)); session->stats.rx_oos_packets++; goto out; } - next = next->next; } __skb_queue_tail(&session->reorder_q, skb); @@ -393,10 +387,9 @@ static void pppol2tp_recv_dequeue_skb(struct pppol2tp_session *session, struct s ENTER_FUNCTION; - /* We're about to requeue the skb, so unlink it and return resources + /* We're about to requeue the skb, so return resources * to its current owner (a socket receive buffer). */ - skb_unlink(skb, &session->reorder_q); skb_orphan(skb); tunnel->stats.rx_packets++; @@ -411,7 +404,7 @@ static void pppol2tp_recv_dequeue_skb(struct pppol2tp_session *session, struct s "%s: updated nr to %hu\n", session->name, session->nr); } - /* If the socket is bound, send it in to PPP's input queue. Otherwise + /* If the socket is bound, send it in to PPP's input queue. Otherwise * queue it on the session socket. */ session_sock = session->sock; @@ -463,55 +456,55 @@ static void pppol2tp_recv_dequeue_skb(struct pppol2tp_session *session, struct s */ static void pppol2tp_recv_dequeue(struct pppol2tp_session *session) { - struct sk_buff *next; - struct sk_buff *prev; + struct sk_buff *skb; + struct sk_buff *tmp; ENTER_FUNCTION; - prev = (struct sk_buff *) &session->reorder_q; - spin_lock(&session->reorder_q.lock); - next = prev->next; - /* If the pkt at the head of the queue has the nr that we * expect to send up next, dequeue it and any other * in-sequence packets behind it. */ - while (next != prev) { - struct sk_buff *skb = next; - next = next->next; - spin_unlock(&session->reorder_q.lock); - + spin_lock(&session->reorder_q.lock); + skb_queue_walk_safe(&session->reorder_q, skb, tmp) { if (time_after(jiffies, PPPOL2TP_SKB_CB(skb)->expires)) { session->stats.rx_seq_discards++; session->stats.rx_errors++; PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG, - "%s: oos pkt %hu len %d discarded (too old), waiting for %hu, reorder_q_len=%d\n", + "%s: oos pkt %hu len %d discarded (too old), " + "waiting for %hu, reorder_q_len=%d\n", session->name, PPPOL2TP_SKB_CB(skb)->ns, PPPOL2TP_SKB_CB(skb)->length, session->nr, skb_queue_len(&session->reorder_q)); - skb_unlink(skb, &session->reorder_q); + __skb_unlink(skb, &session->reorder_q); kfree_skb(skb); sock_put(session->sock); - goto again; + continue; } if (PPPOL2TP_SKB_CB(skb)->has_seq) { if (PPPOL2TP_SKB_CB(skb)->ns != session->nr) { PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG, - "%s: holding oos pkt %hu len %d, waiting for %hu, reorder_q_len=%d\n", - session->name, PPPOL2TP_SKB_CB(skb)->ns, - PPPOL2TP_SKB_CB(skb)->length, session->nr, - skb_queue_len(&session->reorder_q)); + "%s: holding oos pkt %hu len %d, " + "waiting for %hu, reorder_q_len=%d\n", + session->name, PPPOL2TP_SKB_CB(skb)->ns, + PPPOL2TP_SKB_CB(skb)->length, session->nr, + skb_queue_len(&session->reorder_q)); goto out; } } + __skb_unlink(skb, &session->reorder_q); + + /* Process the skb. We release the queue lock while we + * do so to let other contexts process the queue. + */ + spin_unlock(&session->reorder_q.lock); pppol2tp_recv_dequeue_skb(session, skb); -again: spin_lock(&session->reorder_q.lock); } - spin_unlock(&session->reorder_q.lock); out: + spin_unlock(&session->reorder_q.lock); EXIT_FUNCTION; } @@ -524,48 +517,53 @@ out: static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) { struct pppol2tp_session *session = NULL; - int error = 0; struct pppol2tp_tunnel *tunnel; - unsigned char *ptr; + int error; + unsigned char *ptr, *optr; u16 hdrflags; u16 tunnel_id, session_id; - int length, i; - struct udphdr *uh; + int length; + int offset; ENTER_FUNCTION; - SOCK_2_TUNNEL(sock, tunnel, error, 1, end, 0); + SOCK_2_TUNNEL(sock, tunnel, error, 1, error, 0); + + if (skb->pkt_type != PACKET_HOST) + goto error; + + /* UDP always verifies the packet length. */ + __skb_pull(skb, sizeof(struct udphdr)); /* Short packet? */ - if (skb->len < sizeof(struct udphdr)) { + if (!pskb_may_pull(skb, 12)) { PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_INFO, "%s: recv short packet (len=%d)\n", tunnel->name, skb->len); goto end; } - /* Get length of L2TP packet */ - uh = (struct udphdr *) skb_transport_header(skb); - length = ntohs(uh->len) - sizeof(struct udphdr); - /* Point to L2TP header */ - ptr = skb->data + sizeof(struct udphdr); + optr = ptr = skb->data; + + /* Get length of L2TP packet */ + length = skb->len; /* Trace packet contents, if enabled */ if (tunnel->debug & PPPOL2TP_MSG_DATA) { - printk(KERN_DEBUG "%s: recv: ", tunnel->name); + int i; + unsigned char *datap = skb->data; - for (i = 0; i < length && i < 16; i++) - printk(" %02X", ptr[i]); + printk(KERN_DEBUG "%s: recv:", tunnel->name); + for (i = 0; i < length; i++) { + printk(" %02X", *datap++); + if (i == 15) { + printk(" ..."); + break; + } + } printk("\n"); } - /* Too short? */ - if (length < 12) { - PRINTK(tunnel->debug, PPPOL2TP_MSG_DATA, KERN_INFO, - "%s: recv short L2TP packet (len=%d)\n", tunnel->name, length); - goto end; - } - /* Get L2TP header flags */ hdrflags = ntohs(*(u16*)ptr); @@ -648,7 +646,6 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) "%s: recv data has no seq numbers when required. " "Discarding\n", session->name); session->stats.rx_seq_discards++; - session->stats.rx_errors++; goto discard; } @@ -667,7 +664,6 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) "%s: recv data has no seq numbers when required. " "Discarding\n", session->name); session->stats.rx_seq_discards++; - session->stats.rx_errors++; goto discard; } @@ -679,7 +675,11 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) if (hdrflags & L2TP_HDRFLAG_O) ptr += 2 + ntohs(*(u16 *) ptr); - skb_pull(skb, ptr - skb->data); + offset = ptr - optr; + if (!pskb_may_pull(skb, offset)) + goto discard; + + __skb_pull(skb, offset); /* Skip PPP header, if present. In testing, Microsoft L2TP clients * don't send the PPP header (PPP header compression enabled), but @@ -689,8 +689,11 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) * Note that skb->data[] isn't dereferenced from a u16 ptr here since * the field may be unaligned. */ + if (!pskb_may_pull(skb, 2)) + goto discard; + if ((skb->data[0] == 0xff) && (skb->data[1] == 0x03)) - skb_pull(skb, 2); + __skb_pull(skb, 2); /* Prepare skb for adding to the session's reorder_q. Hold * packets for max reorder_timeout or 1 second if not @@ -715,9 +718,9 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) */ if (PPPOL2TP_SKB_CB(skb)->ns != session->nr) { session->stats.rx_seq_discards++; - session->stats.rx_errors++; PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG, - "%s: oos pkt %hu len %d discarded, waiting for %hu, reorder_q_len=%d\n", + "%s: oos pkt %hu len %d discarded, " + "waiting for %hu, reorder_q_len=%d\n", session->name, PPPOL2TP_SKB_CB(skb)->ns, PPPOL2TP_SKB_CB(skb)->length, session->nr, skb_queue_len(&session->reorder_q)); @@ -741,15 +744,21 @@ static int pppol2tp_recv_core(struct sock *sock, struct sk_buff *skb) discard: DPRINTK(session->debug, "discarding skb, len=%d\n", skb->len); + session->stats.rx_errors++; kfree_skb(skb); DPRINTK(session->debug, "calling sock_put; refcnt=%d\n", session->sock->sk_refcnt.counter); sock_put(session->sock); + EXIT_FUNCTION; return 0; end: + /* Put UDP header back */ + __skb_push(skb, sizeof(struct udphdr)); + +error: EXIT_FUNCTION; return 1; } @@ -823,20 +832,21 @@ static int pppol2tp_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags) { - int err = 0; - struct sk_buff *skb = NULL; + int err; + struct sk_buff *skb; struct sock *sk = sock->sk; ENTER_FUNCTION; err = -EIO; - if (sock->state & PPPOX_BOUND) + if (sk->sk_state & PPPOX_BOUND) goto error; msg->msg_namelen = 0; - skb=skb_recv_datagram(sk, flags & ~MSG_DONTWAIT, - flags & MSG_DONTWAIT, &err); + err = 0; + skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT, + flags & MSG_DONTWAIT, &err); if (!skb) goto error; @@ -851,6 +861,7 @@ static int pppol2tp_recvmsg(struct kiocb *iocb, struct socket *sock, if (skb) kfree_skb(skb); + error: EXIT_FUNCTION; return err; @@ -860,76 +871,20 @@ error: * Transmit handling ***********************************************************************/ -/* Internal UDP socket transmission +/* Tell how big L2TP headers are for a particular session. This + * depends on whether sequence numbers are being used. */ -static int pppol2tp_udp_sock_send(struct kiocb *iocb, - struct pppol2tp_session *session, - struct pppol2tp_tunnel *tunnel, - struct msghdr *msg, int total_len) +static inline int pppol2tp_l2tp_header_len(struct pppol2tp_session *session) { - mm_segment_t fs; - int error; - - ENTER_FUNCTION; - - DPRINTK(session->debug, "%s: udp_sendmsg call...\n", session->name); -#ifdef DEBUG - /* Catch bad socket parameter errors */ - if (msg->msg_name) { - struct sockaddr_in * usin = (struct sockaddr_in*)msg->msg_name; - if (msg->msg_namelen < sizeof(*usin)) { - printk(KERN_ERR "msg->msg_namelen wrong, %d\n", msg->msg_namelen); - return -EINVAL; - } - if (usin->sin_family != AF_INET) { - if (usin->sin_family != AF_UNSPEC) { - printk(KERN_ERR "addr family wrong: %d\n", usin->sin_family); - return -EINVAL; - } - } - if ((usin->sin_addr.s_addr == 0) || (usin->sin_port == 0)) { - printk(KERN_ERR "udp addr=%x/%hu\n", usin->sin_addr.s_addr, usin->sin_port); - return -EINVAL; - } - } -#endif /* DEBUG */ - - /* Set to userspace data segment while we do a sendmsg() call. We're - * actually calling a userspace API from the kernel here... - */ - fs = get_fs(); - set_fs(get_ds()); - - /* The actual sendmsg() call... */ - error = tunnel->old_proto->sendmsg(iocb, session->tunnel_sock, msg, total_len); - if (error == -EIOCBQUEUED) - error = wait_on_sync_kiocb(iocb); - - /* Back to kernel space */ - set_fs(fs); - - if (error >= 0) { - tunnel->stats.tx_packets++; - tunnel->stats.tx_bytes += error; - session->stats.tx_packets++; - session->stats.tx_bytes += error; - } else { - tunnel->stats.tx_errors++; - session->stats.tx_errors++; - } - - DPRINTK(session->debug, "%s: %s: returning result %d\n", __FUNCTION__, - session->name, error); - kfree(msg->msg_iov); - kfree(msg); + if (session->send_seq) + return PPPOL2TP_L2TP_HDR_SIZE_SEQ; - EXIT_FUNCTION; - return error; + return PPPOL2TP_L2TP_HDR_SIZE_NOSEQ; } /* Build an L2TP header for the session into the buffer provided. */ -static int pppol2tp_build_l2tp_header(struct pppol2tp_session *session, +static void pppol2tp_build_l2tp_header(struct pppol2tp_session *session, void *buf) { u16 *bufp = buf; @@ -953,10 +908,6 @@ static int pppol2tp_build_l2tp_header(struct pppol2tp_session *session, PRINTK(session->debug, PPPOL2TP_MSG_SEQ, KERN_DEBUG, "%s: updated ns to %hu\n", session->name, session->ns); } - /* This is the PPP header really */ - *bufp = htons(0xff03); - - return ((void *) bufp) - buf; } /* This is the sendmsg for the PPPoL2TP pppol2tp_session socket. We come here @@ -966,14 +917,18 @@ static int pppol2tp_build_l2tp_header(struct pppol2tp_session *session, static int pppol2tp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *m, size_t total_len) { - static unsigned char ppph[2] = { 0xff, 0x03 }; + static const u8 ppph[2] = { 0xff, 0x03 }; struct sock *sk = sock->sk; - int error = 0; - u8 hdr[PPPOL2TP_L2TP_HDR_SIZE_SEQ]; + struct sock *sk_tun; + struct sk_buff *skb; + int error; int hdr_len; - struct msghdr *msg; + u16 udp_len; struct pppol2tp_session *session; struct pppol2tp_tunnel *tunnel; + struct inet_sock *inet; + struct udphdr *uh; + unsigned int len; ENTER_FUNCTION; @@ -984,10 +939,57 @@ static int pppol2tp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msgh /* Get session and tunnel contexts */ SOCK_2_SESSION(sk, session, error, -EBADF, end, 0); - SOCK_2_TUNNEL(session->tunnel_sock, tunnel, error, -EBADF, end, 0); + sk_tun = session->tunnel_sock; + SOCK_2_TUNNEL(sk_tun, tunnel, error, -EBADF, end, 0); + + /* What header length is configured for this session? */ + hdr_len = pppol2tp_l2tp_header_len(session); + + /* Allocate a socket buffer */ + skb = sock_wmalloc(sk_tun, NET_SKB_PAD + sizeof(struct iphdr) + + sizeof(struct udphdr) + hdr_len + + sizeof(ppph) + total_len, + 0, GFP_KERNEL); + if (!skb) { + error = -ENOMEM; + goto end; + } - /* Setup L2TP header */ - hdr_len = pppol2tp_build_l2tp_header(session, &hdr); + /* Reserve space for headers. */ + skb_reserve(skb, NET_SKB_PAD); + skb_reset_network_header(skb); + skb_reserve(skb, sizeof(struct iphdr)); + skb_reset_transport_header(skb); + + /* Build UDP header */ + inet = inet_sk(sk_tun); + udp_len = hdr_len + sizeof(ppph) + total_len; + uh = (struct udphdr *) skb->data; + uh->source = inet->sport; + uh->dest = inet->dport; + uh->len = htons(udp_len); + uh->check = 0; + skb_put(skb, sizeof(struct udphdr)); + + /* Build L2TP header */ + pppol2tp_build_l2tp_header(session, skb->data); + skb_put(skb, hdr_len); + + /* Add PPP header */ + skb->data[0] = ppph[0]; + skb->data[1] = ppph[1]; + skb_put(skb, 2); + + /* Copy user data into skb */ + error = memcpy_fromiovec(skb->data, m->msg_iov, total_len); + if (error < 0) { + kfree_skb(skb); + goto end; + } + skb_put(skb, total_len); + + /* Calculate UDP checksum if configured to do so */ + skb->ip_summed = CHECKSUM_NONE; if (session->send_seq) PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG, @@ -998,128 +1000,89 @@ static int pppol2tp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msgh "%s: send %d bytes\n", session->name, total_len); if (session->debug & PPPOL2TP_MSG_DATA) { - int i, j, count; + int i; + unsigned char *datap = skb->data - hdr_len - 2; printk(KERN_DEBUG "%s: xmit:", session->name); - count = 0; - for (i = 0; i < m->msg_iovlen; i++) { - for (j = 0; j < m->msg_iov[i].iov_len; j++) { - printk(" %02X", ((unsigned char *) m->msg_iov[i].iov_base)[j]); - count++; - if (count == 15) { - printk(" ..."); - break; - } + for (i = 0; i < total_len; i++) { + printk(" %02X", *datap++); + if (i == 15) { + printk(" ..."); + break; } } printk("\n"); } - /* Unfortunately, there is no direct way for us to pass an skb to the - * UDP layer, we have to pretend to be sending ordinary data and use - * sendmsg. - * - * We add the L2TP and PPP headers here. To do so, we create a new - * struct msghdr and insert the headers as the first iovecs. - */ - msg = kmalloc(sizeof(struct msghdr), GFP_ATOMIC); - if (msg == NULL) { - error = -ENOBUFS; - tunnel->stats.tx_errors++; - session->stats.tx_errors++; - goto end; - } + /* Queue the packet to IP for output */ + len = skb->len; + error = ip_queue_xmit(skb, 1); + error = net_xmit_eval(error); - msg->msg_iov = kmalloc((m->msg_iovlen + 2) * sizeof(struct iovec), - GFP_ATOMIC); - if (msg->msg_iov == NULL) { - error = -ENOBUFS; + /* Update stats */ + if (error >= 0) { + tunnel->stats.tx_packets++; + tunnel->stats.tx_bytes += len; + session->stats.tx_packets++; + session->stats.tx_bytes += len; + } else { tunnel->stats.tx_errors++; session->stats.tx_errors++; - kfree(msg); - goto end; } - msg->msg_iov[0].iov_base = &hdr; - msg->msg_iov[0].iov_len = hdr_len; - msg->msg_iov[1].iov_base = &ppph; - msg->msg_iov[1].iov_len = sizeof(ppph); - memcpy(&msg->msg_iov[2], &m->msg_iov[0], - m->msg_iovlen * sizeof(struct iovec)); - msg->msg_iovlen = m->msg_iovlen + 2; - - /* If the user calls sendto() that's just too bad */ - msg->msg_name = &session->tunnel_addr.addr; - msg->msg_namelen = sizeof(session->tunnel_addr.addr); - - msg->msg_control = m->msg_control; - msg->msg_controllen = m->msg_controllen; - msg->msg_flags = m->msg_flags; - - /* Do the real work. This always frees msg, regardless of whether - * there was an error - */ - error = pppol2tp_udp_sock_send(iocb, session, tunnel, msg, - total_len + hdr_len + sizeof(ppph)); - end: EXIT_FUNCTION; return error; } -/* Work queue handler for pppol2tp_xmit(). +/* Automatically called when the skb is freed. */ -static void pppol2tp_wq_send(struct work_struct *work) +static void pppol2tp_sock_wfree(struct sk_buff *skb) { - struct pppol2tp_send *send = container_of(work, struct pppol2tp_send, send_task); - int error; - mm_segment_t oldfs; - - oldfs = get_fs(); - set_fs(KERNEL_DS); - - error = send->tunnel->old_proto->sendmsg(&send->iocb, - send->session->tunnel_sock, - send->msg, send->total_len); - if (error == -EIOCBQUEUED) - error = wait_on_sync_kiocb(&send->iocb); - - set_fs(oldfs); - - if (error >= 0) { - send->tunnel->stats.tx_packets++; - send->tunnel->stats.tx_bytes += error; - send->session->stats.tx_packets++; - send->session->stats.tx_bytes += error; - } else { - send->tunnel->stats.tx_errors++; - send->session->stats.tx_errors++; - } + sock_put(skb->sk); +} - kfree(send->iov); - kfree(send->msg); - kfree_skb(send->skb); - kfree(send); +/* For data skbs that we transmit, we associate with the tunnel socket + * but don't do accounting. + */ +static inline void pppol2tp_skb_set_owner_w(struct sk_buff *skb, struct sock *sk) +{ + sock_hold(sk); + skb->sk = sk; + skb->destructor = pppol2tp_sock_wfree; } -/* Transmit function called by generic PPP driver. Sends PPP frame over - * PPPoL2TP socket. +/* Transmit function called by generic PPP driver. Sends PPP frame + * over PPPoL2TP socket. + * + * This is almost the same as pppol2tp_sendmsg(), but rather than + * being called with a msghdr from userspace, it is called with a skb + * from the kernel. * - * This is almost the same as pppol2tp_sendmsg(), but rather than being called - * with a msghdr from userspace, it is called with a skb from the kernel. - * Since this function cannot block, we must queue up the actual socket send - * on a work queue. + * The supplied skb from ppp doesn't have enough headroom for the + * insertion of L2TP, UDP and IP headers so we need to allocate more + * headroom in the skb. This will create a cloned skb. But we must be + * careful in the error case because the caller will expect to free + * the skb it supplied, not our cloned skb. So we take care to always + * leave the original skb unfreed if we return an error. */ static int pppol2tp_xmit(struct ppp_channel *chan, struct sk_buff *skb) { - static unsigned char ppph[2] = { 0xff, 0x03 }; + static const u8 ppph[2] = { 0xff, 0x03 }; struct sock *sk = (struct sock *) chan->private; - int error = 0; + struct sock *sk_tun; + int error; + u16 udp_len; int hdr_len; - struct msghdr *msg = NULL; + int headroom; + int data_len = skb->len; struct pppol2tp_session *session; struct pppol2tp_tunnel *tunnel; - struct pppol2tp_send *send = NULL; + struct inet_sock *inet; + struct udphdr *uh; + unsigned int len; + int old_headroom; + int new_headroom; ENTER_FUNCTION; @@ -1131,33 +1094,64 @@ static int pppol2tp_xmit(struct ppp_channel *chan, struct sk_buff *skb) /* Get session and tunnel contexts from the socket */ SOCK_2_SESSION(sk, session, error, -EBADF, end, 0); - SOCK_2_TUNNEL(session->tunnel_sock, tunnel, error, -EBADF, end, 0); + sk_tun = session->tunnel_sock; + SOCK_2_TUNNEL(sk_tun, tunnel, error, -EBADF, end, 0); - send = kmalloc(sizeof(struct pppol2tp_send), GFP_ATOMIC); - if (send == NULL) { - error = -ENOBUFS; - tunnel->stats.tx_errors++; - session->stats.tx_errors++; + /* What header length is configured for this session? */ + hdr_len = pppol2tp_l2tp_header_len(session); + + /* Check that there's enough headroom in the skb to insert IP, + * UDP and L2TP and PPP headers. If not enough, expand it to + * make room. Adjust truesize. + */ + headroom = NET_SKB_PAD + sizeof(struct iphdr) + + sizeof(struct udphdr) + hdr_len + sizeof(ppph); + old_headroom = skb_headroom(skb); + if (skb_cow_head(skb, headroom)) { + error = -ENOMEM; goto end; } + new_headroom = skb_headroom(skb); + skb_orphan(skb); + skb->truesize += new_headroom - old_headroom; + + /* Setup PPP header */ + __skb_push(skb, sizeof(ppph)); + skb->data[0] = ppph[0]; + skb->data[1] = ppph[1]; /* Setup L2TP header */ - hdr_len = pppol2tp_build_l2tp_header(session, &send->hdr); + pppol2tp_build_l2tp_header(session, __skb_push(skb, hdr_len)); + + /* Build UDP header */ + inet = inet_sk(sk_tun); + udp_len = sizeof(struct udphdr) + hdr_len + sizeof(ppph) + data_len; + __skb_push(skb, sizeof(*uh)); + skb_reset_transport_header(skb); + uh = udp_hdr(skb); + uh->source = inet->sport; + uh->dest = inet->dport; + uh->len = htons(udp_len); + uh->check = 0; + + /* Calculate UDP checksum if configured to do so */ + skb->ip_summed = CHECKSUM_NONE; if (session->send_seq) PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG, "%s: send %d bytes, ns=%hu\n", - session->name, skb->len, session->ns - 1); + session->name, data_len, session->ns - 1); else PRINTK(session->debug, PPPOL2TP_MSG_DATA, KERN_DEBUG, - "%s: send %d bytes\n", session->name, skb->len); + "%s: send %d bytes\n", session->name, data_len); if (session->debug & PPPOL2TP_MSG_DATA) { int i; + unsigned char *datap = skb->data + sizeof(struct udphdr); printk(KERN_DEBUG "%s: xmit:", session->name); - for (i = 0; i < skb->len; i++) { - printk(" %02X", skb->data[i]); + for (i = 0; i < data_len; i++) { + printk(" %02X", *datap++); if (i == 15) { printk(" ..."); break; @@ -1166,64 +1160,42 @@ static int pppol2tp_xmit(struct ppp_channel *chan, struct sk_buff *skb) printk("\n"); } - /* Unfortunately there doesn't appear to be a way for us to pass an skb - * to the UDP layer, we have to pretend to be sending ordinary data - * and use sendmsg - */ - msg = kmalloc(sizeof(struct msghdr), GFP_ATOMIC); - if (msg == NULL) { - error = -ENOBUFS; - tunnel->stats.tx_errors++; - session->stats.tx_errors++; - goto end; - } + /* Reset skb netfilter state */ + memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt)); + IPCB(skb)->flags &= ~(IPSKB_XFRM_TUNNEL_SIZE | IPSKB_XFRM_TRANSFORMED | + IPSKB_REROUTED); + nf_reset(skb); + + /* Get routing info from the tunnel socket */ + dst_release(skb->dst); + skb->dst = dst_clone(__sk_dst_get(sk_tun)); + pppol2tp_skb_set_owner_w(skb, sk_tun); - msg->msg_iov = kmalloc(3 * sizeof(struct iovec), GFP_ATOMIC); - if (msg->msg_iov == NULL) { - error = -ENOBUFS; + /* Queue the packet to IP for output */ + len = skb->len; + error = ip_queue_xmit(skb, 1); + error = net_xmit_eval(error); + + /* Update stats */ + if (error >= 0) { + tunnel->stats.tx_packets++; + tunnel->stats.tx_bytes += len; + session->stats.tx_packets++; + session->stats.tx_bytes += len; + } else { tunnel->stats.tx_errors++; session->stats.tx_errors++; - goto end; } - msg->msg_iov[0].iov_base = &send->hdr; - msg->msg_iov[0].iov_len = hdr_len; - /* FIXME: do we need to handle skb fragments here? */ - msg->msg_iov[1].iov_base = &ppph; - msg->msg_iov[1].iov_len = sizeof(ppph); - msg->msg_iov[2].iov_base = skb->data; - msg->msg_iov[2].iov_len = skb->len; - msg->msg_iovlen = 3; - - /* If the user calls sendto() that's just too bad */ - msg->msg_name = &session->tunnel_addr.addr; - msg->msg_namelen = sizeof(session->tunnel_addr.addr); - - msg->msg_control = NULL; - msg->msg_controllen = 0; - msg->msg_flags = MSG_DONTWAIT; /* Need this to prevent blocking */ - - send->session = session; - send->tunnel = tunnel; - send->msg = msg; - send->skb = skb; - send->tunnel_sock = session->tunnel_sock; - send->iov = msg->msg_iov; - send->total_len = skb->len + hdr_len + sizeof(ppph); - init_sync_kiocb(&send->iocb, NULL); - send->iocb.private = &send->siocb; - - INIT_WORK(&send->send_task, pppol2tp_wq_send); - queue_work(tunnel->wq, &send->send_task); + + EXIT_FUNCTION; return 1; end: - if (msg != NULL) - kfree(msg); - if (send != NULL) - kfree(send); + /* Free the original skb */ + kfree_skb(skb); EXIT_FUNCTION; - return error; + return 1; } /***************************************************************************** @@ -1249,23 +1221,28 @@ static void pppol2tp_tunnel_closeall(struct pppol2tp_tunnel *tunnel) PRINTK(tunnel->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO, "%s: closing all sessions...\n", tunnel->name); + write_lock_bh(&tunnel->hlist_lock); for (hash = 0; hash < PPPOL2TP_HASH_SIZE; hash++) { +again: hlist_for_each_safe(walk, tmp, &tunnel->session_hlist[hash]) { struct sk_buff *skb; session = hlist_entry(walk, struct pppol2tp_session, hlist); - sk = session->sock; PRINTK(session->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO, "%s: closing session\n", session->name); - write_lock_bh(&tunnel->hlist_lock); hlist_del_init(&session->hlist); - write_unlock_bh(&tunnel->hlist_lock); + /* Since we should hold the sock lock while + * doing any unbinding, we need to release the + * lock we're holding before taking that lock. + * Hold a reference to the sock so it doesn't + * disappear as we're jumping between locks. + */ sock_hold(sk); - + write_unlock_bh(&tunnel->hlist_lock); lock_sock(sk); if (sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND)) { @@ -1287,8 +1264,17 @@ static void pppol2tp_tunnel_closeall(struct pppol2tp_tunnel *tunnel) DPRINTK(session->debug, "calling sock_put; refcnt=%d\n", sk->sk_refcnt.counter); sock_put(sk); + + /* Now restart from the beginning of this hash + * chain. We always remove a session from the + * list so we are guaranteed to make forward + * progress. + */ + write_lock_bh(&tunnel->hlist_lock); + goto again; } } + write_unlock_bh(&tunnel->hlist_lock); EXIT_FUNCTION; } @@ -1299,26 +1285,30 @@ static void pppol2tp_tunnel_closeall(struct pppol2tp_tunnel *tunnel) static void pppol2tp_tunnel_free(struct pppol2tp_tunnel *tunnel) { struct sock *sk = tunnel->sock; +#ifdef PPPOL2TP_UDP_CONNECT + struct sockaddr any = { .sa_family = AF_UNSPEC }; +#endif ENTER_FUNCTION; +#ifdef PPPOL2TP_UDP_CONNECT + /* Disconnect the tunnel socket */ + kernel_connect(sk->sk_socket, &any, sizeof(any), 0); +#endif + /* Remove from socket list */ list_del_init(&tunnel->list); - sk->sk_prot = tunnel->old_proto; #ifndef UDP_ENCAP_L2TPINUDP - sk->sk_data_ready = tunnel->old_data_ready; + sk->sk_data_ready = tunnel->old_data_ready; #else /* No longer an encapsulation socket. See net/ipv4/udp.c */ (udp_sk(sk))->encap_type = 0; (udp_sk(sk))->encap_rcv = NULL; -#endif +#endif sk->sk_destruct = tunnel->old_sk_destruct; sk->sk_user_data = NULL; - flush_workqueue(tunnel->wq); - destroy_workqueue(tunnel->wq); - DPRINTK(tunnel->debug, "%s: MOD_DEC_USE_COUNT\n", tunnel->name); kfree(tunnel); @@ -1526,6 +1516,9 @@ static struct socket *pppol2tp_sockfd_lookup(pid_t pid, int fd, int *err) * attached to it */ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, +#ifdef PPPOL2TP_UDP_CONNECT + struct sockaddr_in *addr, +#endif u16 tunnel_id, int *error) { int err; @@ -1546,16 +1539,18 @@ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, goto err; } + sk = sock->sk; + /* Quick sanity checks */ - err = -ESOCKTNOSUPPORT; - if (sock->type != SOCK_DGRAM) { + err = -EPROTONOSUPPORT; + if (sk->sk_protocol != IPPROTO_UDP) { PRINTK(-1, PPPOL2TP_MSG_CONTROL, KERN_ERR, - "tunl %hu: fd %d wrong type, got %d, expected %d\n", - tunnel_id, fd, sock->type, SOCK_DGRAM); + "tunl %hu: fd %d wrong protocol, got %d, expected %d\n", + tunnel_id, fd, sk->sk_protocol, IPPROTO_UDP); goto err; } err = -EAFNOSUPPORT; - if (sock->ops->family!=AF_INET) { + if (sock->ops->family != AF_INET) { PRINTK(-1, PPPOL2TP_MSG_CONTROL, KERN_ERR, "tunl %hu: fd %d wrong family, got %d, expected %d\n", tunnel_id, fd, sock->ops->family, AF_INET); @@ -1563,7 +1558,6 @@ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, } err = -ENOTCONN; - sk = sock->sk; /* Check if this socket has already been prepped */ tunnel = (struct pppol2tp_tunnel *)sk->sk_user_data; @@ -1580,6 +1574,20 @@ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, /* This socket has already been prepped */ ret = tunnel->sock; +#ifdef PPPOL2TP_UDP_CONNECT + } + + /* (Re)connect tunnel socket to the peer */ + err = kernel_connect(sock, (struct sockaddr *) addr, sizeof(*addr), 0); + DPRINTK(-1, "tunl %hu: fd %d connect to %x/%hu returns %d\n", + tunnel_id, fd, + ntohl(addr->sin_addr.s_addr), ntohs(addr->sin_port), err); + if (err < 0 && err != -EALREADY) + goto err; + + /* This socket has already been prepped */ + if (tunnel != NULL) { +#endif goto out; } @@ -1604,21 +1612,10 @@ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, DPRINTK(tunnel->debug, "tunl %hu: allocated tunnel=%p, sk=%p, sock=%p\n", tunnel_id, tunnel, sk, sock); - tunnel->wq = create_workqueue("kl2tpd"); - if (!tunnel->wq) { - err = -ENOMEM; - goto err_free_tunnel; - } - /* Setup the new protocol stuff */ - tunnel->old_proto = sk->sk_prot; - tunnel->l2tp_proto = *sk->sk_prot; - - sk->sk_prot = &tunnel->l2tp_proto; - #ifndef UDP_ENCAP_L2TPINUDP tunnel->old_data_ready = sk->sk_data_ready; - sk->sk_data_ready = &pppol2tp_data_ready; + sk->sk_data_ready = pppol2tp_data_ready; #else /* Mark socket as an encapsulation socket. See net/ipv4/udp.c */ (udp_sk(sk))->encap_type = UDP_ENCAP_L2TPINUDP; @@ -1626,7 +1623,7 @@ static struct sock *pppol2tp_prepare_tunnel_socket(pid_t pid, int fd, #endif tunnel->old_sk_destruct = sk->sk_destruct; - sk->sk_destruct = &pppol2tp_tunnel_destruct; + sk->sk_destruct = pppol2tp_tunnel_destruct; tunnel->sock = sk; sk->sk_allocation = GFP_ATOMIC; @@ -1647,8 +1644,6 @@ out: return ret; -err_free_tunnel: - kfree(tunnel); err: *error = err; goto out; @@ -1739,10 +1734,14 @@ int pppol2tp_connect(struct socket *sock, struct sockaddr *uservaddr, /* Look up the tunnel socket and configure it if necessary */ tunnel_sock = pppol2tp_prepare_tunnel_socket(sp->pppol2tp.pid, sp->pppol2tp.fd, +#ifdef PPPOL2TP_UDP_CONNECT + &sp->pppol2tp.addr, +#endif sp->pppol2tp.s_tunnel, &error); if (tunnel_sock == NULL) goto end; + tunnel = tunnel_sock->sk_user_data; /* Allocate and initialize a new session context. @@ -1780,9 +1779,9 @@ int pppol2tp_connect(struct socket *sock, struct sockaddr *uservaddr, session->mtu = session->mru = 1500 - PPPOL2TP_HEADER_OVERHEAD; /* If PMTU discovery was enabled, use the MTU that was discovered */ - dst = sk_dst_get(sk); + dst = sk_dst_get(tunnel_sock); if (dst != NULL) { - u32 pmtu = dst_mtu(__sk_dst_get(sk)); + u32 pmtu = dst_mtu(__sk_dst_get(tunnel_sock)); if (pmtu != 0) { session->mtu = session->mru = pmtu - PPPOL2TP_HEADER_OVERHEAD; @@ -1828,6 +1827,8 @@ int pppol2tp_connect(struct socket *sock, struct sockaddr *uservaddr, * whether sequence numbers are enabled for the data channel. */ po->chan.hdrlen = PPPOL2TP_L2TP_HDR_SIZE_NOSEQ; + po->chan.hdrlen += NET_SKB_PAD + sizeof(struct iphdr) + + sizeof(struct udphdr) + 2; po->chan.private = sk; po->chan.ops = &pppol2tp_chan_ops; @@ -1943,7 +1944,7 @@ static int pppol2tp_session_ioctl(struct pppol2tp_session *session, break; session->mtu = ifr.ifr_mtu; -; + PRINTK(session->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO, "%s: set mtu=%d\n", session->name, session->mtu); err = 0; @@ -2198,6 +2199,8 @@ static int pppol2tp_session_setsockopt(struct sock *sk, struct pppox_sock *po = pppox_sk(sk); po->chan.hdrlen = val ? PPPOL2TP_L2TP_HDR_SIZE_SEQ : PPPOL2TP_L2TP_HDR_SIZE_NOSEQ; + po->chan.hdrlen += NET_SKB_PAD + sizeof(struct iphdr) + + sizeof(struct udphdr) + 2; } PRINTK(session->debug, PPPOL2TP_MSG_CONTROL, KERN_INFO, "%s: set send_seq=%d\n", session->name, session->send_seq); @@ -2637,12 +2640,12 @@ static struct proto_ops pppol2tp_ops = { .ioctl = pppox_ioctl, }; -struct pppox_proto pppol2tp_proto = { +static struct pppox_proto pppol2tp_proto = { .create = pppol2tp_create, .ioctl = pppol2tp_ioctl }; -int __init pppol2tp_init(void) +static int __init pppol2tp_init(void) { int err; @@ -2669,12 +2672,13 @@ out: out_unregister_pppox_proto: unregister_pppox_proto(PX_PROTO_OL2TP); + out_unregister_pppol2tp_proto: proto_unregister(&pppol2tp_sk_proto); goto out; } -void __exit pppol2tp_exit(void) +static void __exit pppol2tp_exit(void) { unregister_pppox_proto(PX_PROTO_OL2TP); @@ -2687,7 +2691,8 @@ void __exit pppol2tp_exit(void) module_init(pppol2tp_init); module_exit(pppol2tp_exit); -MODULE_AUTHOR("Martijn van Oosterhout "); +MODULE_AUTHOR("Martijn van Oosterhout , " + "James Chapman "); MODULE_DESCRIPTION("PPP over L2TP over UDP"); MODULE_LICENSE("GPL"); MODULE_VERSION(PPPOL2TP_DRV_VERSION); -- 2.11.4.GIT