diff --git a/include/aps_if.h b/include/aps_if.h index 3fa25fe..1a52be1 100644 --- a/include/aps_if.h +++ b/include/aps_if.h @@ -246,6 +246,19 @@ extern int babylon_release_call(struct chan *dev, struct file *filp) ; #define BIOC_GET_MAX_FRAGS _IOR('x', 0x35, unsigned) /* arg = unsigned * */ #define BIOCSETBUNDLENETNS _IO('x', 0x36) /* arg = int netns_pid */ +struct l2tp_hardwire_info { + char dev_name[32]; + u32 hw_src_ip; + u32 hw_dst_ip; + u16 hw_src_port; + u16 hw_dst_port; + u16 hw_peer_tunnel; + u16 hw_peer_session; + u8 hw_gw_mac[6]; +}; + +#define BIOC_L2TP_HARDWIRE_SESSION _IOR('x', 0x37, void *) /**/ + #ifdef __cplusplus }; #endif diff --git a/include/vercomp.h b/include/vercomp.h index ad4070e..21b9b53 100644 --- a/include/vercomp.h +++ b/include/vercomp.h @@ -193,6 +193,7 @@ static inline int sock_flag(struct sock *sk, enum sock_flags flag) #if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,21) #define skb_reset_network_header(skb) ((skb)->nh.raw = (skb)->data) +#define skb_set_network_header(skb,off) ((skb)->nh.raw = (u8*)(skb)->data + (off)) #define skb_reset_mac_header(skb) ((skb)->mac.raw = (skb)->data) #define skb_reset_transport_header(skb) ((skb)->h.raw = (u8*)(skb)->data) #define skb_set_transport_header(skb,off) ((skb)->h.raw = (u8*)(skb)->data + (off)) diff --git a/kernel/bab_l2tp.c b/kernel/bab_l2tp.c index a87cb6c..cb0ca07 100644 --- a/kernel/bab_l2tp.c +++ b/kernel/bab_l2tp.c @@ -10,6 +10,8 @@ * as published by the Free Software Foundation; either version * 2 of the License, or (at your option) any later version. */ +#define L2TP_HARDWIRE 1 + #include "../include/bab_module.h" #include @@ -41,6 +43,18 @@ struct l2tp_session { int recursion; struct l2tp_tunnel *tunnel; + +#ifdef L2TP_HARDWIRE + /* Used for hardwired outgoing */ + struct net_device *hw_ether_dev; + u32 hw_src_ip; + u32 hw_dst_ip; + u16 hw_src_port; + u16 hw_dst_port; + u16 hw_peer_tunnel; + u16 hw_peer_session; + u8 hw_gw_mac[6]; +#endif }; struct l2tp_tunnel { @@ -387,7 +401,9 @@ queue: /* assuming we have a channel, receive the skb. */ skb->dev = NULL; +#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) dst_release(skb_dst(skb)); +#endif skb_dst_set(skb, NULL); #if defined(CONFIG_NF_CONNTRACK) || (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,21) && defined(CONFIG_NETFILTER)) nf_conntrack_put(skb->nfct); @@ -774,6 +790,92 @@ void l2tp_bab_unuse(channel_t *ch) printk("l2tp_bab_unuse\n"); } +#ifdef L2TP_HARDWIRE +int l2tp_hardwire_output(struct l2tp_session *session, struct sk_buff *skb) +{ + u16 len = skb->len; + u8 *data = skb_push(skb, 48); + struct iphdr *iph; + struct udphdr *uh; + + if (!data) { + session->ch.stats.collisions++; + dev_kfree_skb(skb); + return 0; + } + + /* ether header */ + memcpy(data + 0, session->hw_gw_mac, 6); /* dst mac */ + memcpy(data + 6, session->hw_ether_dev->dev_addr, 6); /* src mac */ + *(u16 *)&data[12] = htons(0x0800); /* proto */ + + /* ip hdr */ + data[14] = 0x45; /* version(4) ihl(5) */ + data[15] = 0x00; /* tos */ + *(u16 *)(data+16) = htons(len + 48 - 14); /* tot_len */ + *(u16 *)(data+18) = htons(0x0000); /* id */ + *(u16 *)(data+20) = htons(0x0000); /* frag_off */ + data[22] = 0x40; /* ttl */ + data[23] = 0x11; /* protocol (UDP) */ + *(u16 *)(data+24) = htons(0x0000); /* check */ + *(u32 *)(data+26) = session->hw_src_ip; /* saddr */ + *(u32 *)(data+30) = session->hw_dst_ip; /* daddr */ + + iph = (struct iphdr *)(data+14); + iph->check = ip_fast_csum(data + 14, 5); + + /* udp hdr */ + *(u16 *)(data+34) = session->hw_src_port; /* source port */ + *(u16 *)(data+36) = session->hw_dst_port; /* dest port */ + *(u16 *)(data+38) = htons(len + 6 + 8); /* len + l2tp + udp */ + *(u16 *)(data+40) = htons(0x0000); /* check */ + + /* l2tp header */ + *(u16 *)(data+42) = htons(0x0002); /* flags = data pkt, no seq */ + *(u16 *)(data+44) = session->hw_peer_tunnel; /* peer tunnel id */ + *(u16 *)(data+46) = session->hw_peer_session; /* peer session id */ + + skb->dev = session->hw_ether_dev; + skb_set_mac_header(skb, 0); + skb_set_network_header(skb, 14); + skb_set_transport_header(skb, 34); + skb->protocol = __constant_htons(0x0800); + + if (!(skb->dev->features & NETIF_F_IP_CSUM)) { + skb->csum = skb_checksum(skb, /*skb_transport_offset*/34, skb->len - 34, 0); + } else + skb->csum = 0; + + /* set up UDP checksum offload, only for the UDP+L2TP header */ + uh = (struct udphdr *)(data + 34); + uh->check = csum_tcpudp_magic(iph->saddr, iph->daddr, len + 6 + 8/*48*/, + IPPROTO_UDP, skb->csum); + + if (!(skb->dev->features & NETIF_F_IP_CSUM)) { + if (uh->check == 0) + uh->check = 0xffff; + skb->ip_summed = CHECKSUM_NONE; + } else { + uh->check = ~uh->check; + +#if defined(CHECKSUM_HW) /* pre-2.6.19 */ + skb->ip_summed = CHECKSUM_HW; + skb->csum = offsetof(struct udphdr, check); +#else + skb->ip_summed = CHECKSUM_PARTIAL; + skb->csum_offset = offsetof(struct udphdr, check); +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22) + skb->csum_start = 34; +#else + skb->csum = offsetof(struct udphdr, check); +#endif +#endif + } + + return dev_queue_xmit(skb); +} +#endif /* def L2TP_HARDWIRE */ + int l2tp_bab_output(channel_t *ch, struct sk_buff *skb) { struct l2tp_session *session = (void *)ch; @@ -790,7 +892,8 @@ int l2tp_bab_output(channel_t *ch, struct sk_buff *skb) old_headroom = skb_headroom(skb); new_headroom = NET_SKB_PAD / 4 + sizeof(struct iphdr) + - sizeof(struct udphdr) + 6; + sizeof(struct udphdr) + 6 + + 6 + 6 + 2; /* src mac, dst mac, proto */ pr_debug("l2tp_bab_output: old headeroom = %d new headroom = %d\n", old_headroom, new_headroom); @@ -822,6 +925,16 @@ int l2tp_bab_output(channel_t *ch, struct sk_buff *skb) memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt)); pr_debug("dst'd\n"); dst_release(skb_dst(skb)); + skb_dst_set(skb, NULL); + +#ifdef L2TP_HARDWIRE + if (session->hw_ether_dev) { + err = l2tp_hardwire_output(session, skb); + session->recursion--; + return err; + } +#endif + skb_dst_set(skb, dst_clone(__sk_dst_get(session->tunnel->tx_socket->sk))); skb->ip_summed = CHECKSUM_NONE; @@ -911,7 +1024,46 @@ int l2tp_bab_hangup(channel_t *ch) int l2tp_bab_ioctl(channel_t *ch, unsigned int cmd, unsigned long arg) { - printk("l2tp_bab_ioctl\n"); +#ifdef L2TP_HARDWIRE + if (cmd == BIOC_L2TP_HARDWIRE_SESSION) { + struct l2tp_hardwire_info info; + struct l2tp_session *session = (void *)ch; + struct net_device *dev; + + if (copy_from_user(&info, (void *)arg, sizeof(info))) + return -EFAULT; + + if (!info.dev_name[0]) { + dev = session->hw_ether_dev; + session->hw_ether_dev = NULL; + + if (dev) + dev_put(dev); + + return 0; + } + + /* FIXME: netns */ + dev = dev_get_by_name(&init_net, info.dev_name); + if (!dev) + return -ENOENT; + + /* FIXME: should really lock here */ + session->hw_src_ip = info.hw_src_ip; + session->hw_dst_ip = info.hw_dst_ip; + session->hw_src_port = info.hw_src_port; + session->hw_dst_port = info.hw_dst_port; + session->hw_peer_tunnel = info.hw_peer_tunnel; + session->hw_peer_session = info.hw_peer_session; + memcpy(session->hw_gw_mac, info.hw_gw_mac, 6); + + barrier(); + + session->hw_ether_dev = dev; + + return 0; + } +#endif return -EINVAL; } @@ -967,10 +1119,23 @@ static struct l2tp_session *setup_session(struct l2tp_tunnel *tunnel, u16 sessio static void free_l2tp_session(struct l2tp_tunnel *tunnel, struct l2tp_session *session) { +#ifdef L2TP_HARDWIRE + struct net_device *dev; +#endif + if (tunnel->sessions[session->session_id] != session) BUG(); tunnel->sessions[session->session_id] = NULL; UnregisterChannel(&session->ch); + +#ifdef L2TP_HARDWIRE + dev = session->hw_ether_dev; + session->hw_ether_dev = NULL; + + if (dev) + dev_put(dev); +#endif + kmem_cache_free(session_cachep, session); } @@ -1017,7 +1182,11 @@ static int l2tp_join_bundle(struct l2tp_info *l2tp, unsigned int cmd, struct l2t return -ENOMEM; } - ret = ch_ioctl(NULL, NULL, session->ch.link, cmd, j->arg); + if (cmd == BIOC_L2TP_HARDWIRE_SESSION) + ret = l2tp_bab_ioctl(&session->ch, cmd, j->arg); + else + ret = ch_ioctl(NULL, NULL, session->ch.link, cmd, j->arg); + put_l2tp_tunnel(tunnel); return ret; } diff --git a/src/l2tpd.cc b/src/l2tpd.cc index 185a935..b774821 100644 --- a/src/l2tpd.cc +++ b/src/l2tpd.cc @@ -817,6 +817,171 @@ void list_tunnels(ctrlfd_t *cfd) cfd->done(0); } +static int l2tpctl_hw_session(ctrlfd_t *cfd, char *str) +{ + char *session_str = str; + char *next = strchr(str, ' '); + + if (!next) { + cfd->printf("l2tpctl hw-session - missing parameters\n"); + cfd->done(-2); + return 2; + } + + *next = 0; + next++; + + if (strncmp(session_str, "l2tp", 4)) { + cfd->printf("channel '%s' not an L2TP session\n", session_str); + cfd->done(-2); + return 2; + } + + channel_t *ch = findchan(session_str); + if (!ch) { + cfd->printf("could not find channel '%s'\n", session_str); + cfd->done(-2); + return 2; + } + + l2tp_session_t *session = (l2tp_session_t *)ch->link; + struct l2tp_hardwire_info info = { "" }; + + if (0 == strcmp(next, "off")) { + int err = session->ch_ioctl(BIOC_L2TP_HARDWIRE_SESSION, + (unsigned long)&info); + if (err) { + cfd->printf("off - error %d\n", err); + cfd->done(-2); + return 2; + } + cfd->done(0); + return 0; + } + + char *dev = next; + next = strchr(next, ' '); + if (!next) { + cfd->printf("nothing after dev\n"); + cfd->done(-2); + return 2; + } + *next = 0; + next++; + + if (strlen(dev) >= sizeof(info.dev_name)) { + cfd->printf("dev(%s) too long\n", dev); + cfd->done(-2); + return 2; + } + + char *src_ip = next; + next = strchr(next, ' '); + if (!next) { + cfd->printf("nothing after src ip\n"); + cfd->done(-2); + return 2; + } + *next = 0; + next ++; + + char *sport = strchr(src_ip, ':'); + u16 src_port = 1701; + if (sport) + src_port = atoi(sport + 1); + + char *dst_ip = next; + next = strchr(next, ' '); + if (!next) { + cfd->printf("nothing after dst ip\n"); + cfd->done(-2); + return 2; + } + *next = 0; + next ++; + + char *dport = strchr(dst_ip, ':'); + u16 dst_port = 1701; + if (dport) + dst_port = atoi(dport + 1); + + char *peer_tunnel = next; + next = strchr(next, ' '); + if (!next) { + cfd->printf("nothing after peer_tunnel\n"); + cfd->done(-2); + return 2; + } + *next = 0; + next ++; + + char *peer_session = next; + next = strchr(next, ' '); + if (!next) { + cfd->printf("nothing after peer_session\n"); + cfd->done(-2); + return 2; + } + *next = 0; + next ++; + + char *gw_mac = next; + next = strchr(next, ' '); + if (next) { + cfd->printf("garbage(%s) after gw_mac\n", next); + cfd->done(-2); + return 2; + } + + strcpy(info.dev_name, dev); + info.hw_src_ip = htonl(strtoip(src_ip)); + info.hw_dst_ip = htonl(strtoip(dst_ip)); + info.hw_src_port = htons(src_port); + info.hw_dst_port = htons(dst_port); + info.hw_peer_tunnel = htons(atoi(peer_tunnel)); + info.hw_peer_session = htons(atoi(peer_session)); + int mac[6]; + + if (6 != sscanf(gw_mac, "%x:%x:%x:%x:%x:%x", mac+0, mac+1, mac+2, + mac+3, mac+4, mac+5)) { + cfd->printf("parse error on mac(%s)\n", gw_mac); + cfd->done(-2); + return 2; + } + info.hw_gw_mac[0] = mac[0]; + info.hw_gw_mac[1] = mac[1]; + info.hw_gw_mac[2] = mac[2]; + info.hw_gw_mac[3] = mac[3]; + info.hw_gw_mac[4] = mac[4]; + info.hw_gw_mac[5] = mac[5]; + + cfd->printf("hw-session %s %s:%u %s:%u %u %u %02x:%02x:%02x:%02x:%02x:%02x\n", + info.dev_name, + iptostr(ntohl(info.hw_src_ip)), src_port, + iptostr(ntohl(info.hw_dst_ip)), dst_port, + ntohs(info.hw_peer_tunnel), + ntohs(info.hw_peer_session), + info.hw_gw_mac[0], + info.hw_gw_mac[1], + info.hw_gw_mac[2], + info.hw_gw_mac[3], + info.hw_gw_mac[4], + info.hw_gw_mac[5] + ); + + int err = session->ch_ioctl(BIOC_L2TP_HARDWIRE_SESSION, + (unsigned long)&info); + if (err) { + cfd->printf("hw-session - error %d\n", err); + cfd->done(-2); + return 2; + } + + cfd->printf("ok!\n"); + cfd->done(0); + return 0; +} + int do_l2tpctl(ctrlfd_t *cfd, char *str) { while (isspace(*str)) @@ -893,6 +1058,8 @@ int do_l2tpctl(ctrlfd_t *cfd, char *str) do_bcluster(cfd, str + 9); } else if (!strncmp("monitor ", str, 8)) { l2tpctl_monitor(cfd, str + 8); + } else if (!strncmp("hw-session ", str, 11)) { + l2tpctl_hw_session(cfd, str + 11); } else { cfd->printf("usage: l2tpctl\n"); cfd->printf(" l2tpctl add \n"); @@ -920,6 +1087,8 @@ int do_l2tpctl(ctrlfd_t *cfd, char *str) cfd->printf(" l2tpctl all-set-bw [credits|bps|overhead] \n"); cfd->printf(" l2tpctl show-running-config\n"); cfd->printf(" l2tpctl monitor \n"); + cfd->printf(" l2tpctl hw-session \n"); + cfd->printf(" l2tpctl hw-session off\n"); cfd->done(-2); return 2; }