--- zzzz-none-000/linux-2.6.39.4/net/l2tp/l2tp_eth.c 2011-08-03 19:43:28.000000000 +0000 +++ puma6-atom-6490-729/linux-2.6.39.4/net/l2tp/l2tp_eth.c 2021-11-10 13:38:18.000000000 +0000 @@ -9,6 +9,8 @@ * 2 of the License, or (at your option) any later version. */ +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + #include #include #include @@ -21,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -40,11 +43,20 @@ struct sock *tunnel_sock; struct l2tp_session *session; struct list_head list; + atomic_long_t tx_bytes; + atomic_long_t tx_packets; + atomic_long_t tx_dropped; + atomic_long_t rx_bytes; + atomic_long_t rx_packets; + atomic_long_t rx_errors; + atomic_long_t rx_dropped; }; /* via l2tp_session_priv() */ struct l2tp_eth_sess { struct net_device *dev; + u16 mtu; + u16 mss; }; /* per-net private data for this module */ @@ -64,7 +76,7 @@ struct l2tp_eth *priv = netdev_priv(dev); priv->dev = dev; - random_ether_addr(dev->dev_addr); + eth_hw_addr_random(dev); memset(&dev->broadcast[0], 0xff, 6); return 0; @@ -78,32 +90,180 @@ spin_lock(&pn->l2tp_eth_lock); list_del_init(&priv->list); spin_unlock(&pn->l2tp_eth_lock); - dev_put(dev); +} + +static inline unsigned int +optlen(const u_int8_t *opt, unsigned int offset) +{ + /* Beware zero-length options: make finite progress */ + if (opt[offset] <= TCPOPT_NOP || opt[offset+1] == 0) + return 1; + else + return opt[offset+1]; +} + +#define INTERNET_MAX_ENCAP (40+8+8) + +static void l2tp_eth_calc_mss(struct l2tp_session *session) +{ + struct l2tp_eth_sess *spriv = l2tp_session_priv(session); + unsigned iphlen, udphlen; +#ifdef CONFIG_IPV6 + if (session->tunnel->sock->sk_family == PF_INET6) + iphlen = sizeof(struct ipv6hdr); + else +#endif + iphlen = sizeof(struct iphdr); + if (session->tunnel->encap == L2TP_ENCAPTYPE_UDP) + udphlen = sizeof(struct udphdr); + else + udphlen = 0; + spriv->mtu = session->mtu; + spriv->mss = spriv->mtu + - INTERNET_MAX_ENCAP + - sizeof(struct ethhdr) + - iphlen + - udphlen + - session->hdr_len + - sizeof(struct iphdr) + - sizeof(struct tcphdr); +} + +static +struct sk_buff *patchmss(struct l2tp_session *session, struct sk_buff *skb) +{ + struct l2tp_eth_sess *spriv; + struct ethhdr *ethh; + struct iphdr *iph; + struct tcphdr *tcph; + int tcphoff; + __be16 newlen, oldval; + unsigned int i, tcplen; + u8 *opt; + + ethh = (struct ethhdr *)skb->data; + if (ethh->h_proto != __constant_htons(ETH_P_IP)) + return skb; + iph = (struct iphdr *)(skb->data+ETH_HLEN); + if (iph->protocol != IPPROTO_TCP) + return skb; + if (iph->frag_off & __constant_htons(IP_MF|IP_OFFSET)) + return skb; + tcphoff = iph->ihl << 2; + tcph = (struct tcphdr *)(((unsigned char *)iph) + tcphoff); + if (likely(!tcph->syn)) + return skb; + + spriv = l2tp_session_priv(session); + if (spriv->mtu != session->mtu) + l2tp_eth_calc_mss(session); + + opt = (u8 *)tcph; + for (i = sizeof(struct tcphdr); i < tcph->doff*4; i += optlen(opt, i)) { + if (opt[i] == TCPOPT_MSS && tcph->doff*4 - i >= TCPOLEN_MSS && + opt[i+1] == TCPOLEN_MSS) { + u16 oldmss; + + oldmss = (opt[i+2] << 8) | opt[i+3]; + + if (oldmss <= spriv->mss) + return skb; + + opt[i+2] = (spriv->mss & 0xff00) >> 8; + opt[i+3] = spriv->mss & 0x00ff; + + inet_proto_csum_replace2(&tcph->check, skb, + htons(oldmss), htons(spriv->mss), 0); + return skb; + } + } + + tcplen = skb->len - tcphoff; + + /* + * MSS Option not found ?! add it.. + */ + if (skb_tailroom(skb) < TCPOLEN_MSS) { + if (pskb_expand_head(skb, 0, + TCPOLEN_MSS - skb_tailroom(skb), + GFP_ATOMIC)) + return 0; + tcph = (struct tcphdr *)(skb_network_header(skb) + tcphoff); + } + + skb_put(skb, TCPOLEN_MSS); + + opt = (u_int8_t *)tcph + sizeof(struct tcphdr); + memmove(opt + TCPOLEN_MSS, opt, tcplen - sizeof(struct tcphdr)); + + inet_proto_csum_replace2(&tcph->check, skb, + htons(tcplen), htons(tcplen + TCPOLEN_MSS), 1); + opt[0] = TCPOPT_MSS; + opt[1] = TCPOLEN_MSS; + opt[2] = (spriv->mss & 0xff00) >> 8; + opt[3] = spriv->mss & 0x00ff; + + inet_proto_csum_replace4(&tcph->check, skb, 0, *((__be32 *)opt), 0); + + oldval = ((__be16 *)tcph)[6]; + tcph->doff += TCPOLEN_MSS/4; + inet_proto_csum_replace2(&tcph->check, skb, + oldval, ((__be16 *)tcph)[6], 0); + newlen = htons(ntohs(iph->tot_len) + TCPOLEN_MSS); + csum_replace2(&iph->check, iph->tot_len, newlen); + iph->tot_len = newlen; + + return skb; } static int l2tp_eth_dev_xmit(struct sk_buff *skb, struct net_device *dev) { struct l2tp_eth *priv = netdev_priv(dev); struct l2tp_session *session = priv->session; + unsigned int len = skb->len; + int ret; - l2tp_xmit_skb(session, skb, session->hdr_len); + if (likely((skb = patchmss(session, skb)) != 0)) + ret = l2tp_xmit_skb(session, skb, session->hdr_len); + else + ret = NET_XMIT_DROP; + + if (likely(ret == NET_XMIT_SUCCESS)) { + atomic_long_add(len, &priv->tx_bytes); + atomic_long_inc(&priv->tx_packets); + } else { + atomic_long_inc(&priv->tx_dropped); + } + return NETDEV_TX_OK; +} - dev->stats.tx_bytes += skb->len; - dev->stats.tx_packets++; +static struct rtnl_link_stats64 *l2tp_eth_get_stats64(struct net_device *dev, + struct rtnl_link_stats64 *stats) +{ + struct l2tp_eth *priv = netdev_priv(dev); - return 0; + stats->tx_bytes = atomic_long_read(&priv->tx_bytes); + stats->tx_packets = atomic_long_read(&priv->tx_packets); + stats->tx_dropped = atomic_long_read(&priv->tx_dropped); + stats->rx_bytes = atomic_long_read(&priv->rx_bytes); + stats->rx_packets = atomic_long_read(&priv->rx_packets); + stats->rx_errors = atomic_long_read(&priv->rx_errors); + stats->rx_dropped = atomic_long_read(&priv->rx_dropped); + return stats; } + static struct net_device_ops l2tp_eth_netdev_ops = { .ndo_init = l2tp_eth_dev_init, .ndo_uninit = l2tp_eth_dev_uninit, .ndo_start_xmit = l2tp_eth_dev_xmit, + .ndo_get_stats64 = l2tp_eth_get_stats64, }; static void l2tp_eth_dev_setup(struct net_device *dev) { ether_setup(dev); - + dev->features |= NETIF_F_LLTX; dev->netdev_ops = &l2tp_eth_netdev_ops; dev->destructor = free_netdev; } @@ -112,27 +272,23 @@ { struct l2tp_eth_sess *spriv = l2tp_session_priv(session); struct net_device *dev = spriv->dev; + struct l2tp_eth *priv = netdev_priv(dev); if (session->debug & L2TP_MSG_DATA) { unsigned int length; - int offset; - u8 *ptr = skb->data; length = min(32u, skb->len); if (!pskb_may_pull(skb, length)) goto error; - printk(KERN_DEBUG "%s: eth recv: ", session->name); - - offset = 0; - do { - printk(" %02X", ptr[offset]); - } while (++offset < length); - - printk("\n"); + pr_debug("%s: eth recv\n", session->name); + print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, skb->data, length); } - if (!pskb_may_pull(skb, sizeof(ETH_HLEN))) + if ((dev->flags & IFF_UP) == 0) + goto drop; + + if (!pskb_may_pull(skb, ETH_HLEN)) goto error; secpath_reset(skb); @@ -143,16 +299,30 @@ skb_dst_drop(skb); nf_reset(skb); - if (dev_forward_skb(dev, skb) == NET_RX_SUCCESS) { - dev->stats.rx_packets++; - dev->stats.rx_bytes += data_len; - } else - dev->stats.rx_errors++; + /* + * overwrite input device, 2013-07-30 calle@avm.de + */ + skb->skb_iif = dev->ifindex; + if (unlikely((skb = patchmss(session, skb)) == 0)) { + atomic_long_inc(&priv->rx_errors); + return; + } + if (dev_forward_skb(dev, skb) == NET_RX_SUCCESS) { + atomic_long_inc(&priv->rx_packets); + atomic_long_add(data_len, &priv->rx_bytes); + } else { + atomic_long_inc(&priv->rx_errors); + } return; error: - dev->stats.rx_errors++; + atomic_long_inc(&priv->rx_errors); + kfree_skb(skb); + return; + +drop: + atomic_long_inc(&priv->rx_dropped); kfree_skb(skb); } @@ -167,6 +337,7 @@ if (dev) { unregister_netdev(dev); spriv->dev = NULL; + module_put(THIS_MODULE); } } } @@ -182,6 +353,7 @@ } #endif + static int l2tp_eth_create(struct net *net, u32 tunnel_id, u32 session_id, u32 peer_session_id, struct l2tp_session_cfg *cfg) { struct net_device *dev; @@ -249,15 +421,16 @@ spriv = l2tp_session_priv(session); spriv->dev = dev; + l2tp_eth_calc_mss(session); rc = register_netdev(dev); if (rc < 0) goto out_del_dev; + __module_get(THIS_MODULE); /* Must be done after register_netdev() */ strlcpy(session->ifname, dev->name, IFNAMSIZ); - dev_hold(dev); pn = l2tp_eth_pernet(dev_net(dev)); spin_lock(&pn->l2tp_eth_lock); list_add(&priv->list, &pn->l2tp_eth_dev_list); @@ -267,6 +440,7 @@ out_del_dev: free_netdev(dev); + spriv->dev = NULL; out_del_session: l2tp_session_delete(session); out: @@ -308,7 +482,7 @@ if (err) goto out_unreg; - printk(KERN_INFO "L2TP ethernet pseudowire support (L2TPv3)\n"); + pr_info("L2TP ethernet pseudowire support (L2TPv3)\n"); return 0;