Line data    Source code

       1             : // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
       2             : #include <net/netlink.h>
       3             : #include <net/sch_generic.h>
       4             : #include <net/dst.h>
       5             : #include <net/ip.h>
       6             : #include <net/ip6_fib.h>
       7             : 
       8             : struct sch_frag_data {
       9             :         unsigned long dst;
      10             :         struct qdisc_skb_cb cb;
      11             :         __be16 inner_protocol;
      12             :         u16 vlan_tci;
      13             :         __be16 vlan_proto;
      14             :         unsigned int l2_len;
      15             :         u8 l2_data[VLAN_ETH_HLEN];
      16             :         int (*xmit)(struct sk_buff *skb);
      17             : };
      18             : 
      19             : static DEFINE_PER_CPU(struct sch_frag_data, sch_frag_data_storage);
      20             : 
      21           0 : static int sch_frag_xmit(struct net *net, struct sock *sk, struct sk_buff *skb)
      22             : {
      23           0 :         struct sch_frag_data *data = this_cpu_ptr(&sch_frag_data_storage);
      24             : 
      25           0 :         if (skb_cow_head(skb, data->l2_len) < 0) {
      26           0 :                 kfree_skb(skb);
      27           0 :                 return -ENOMEM;
      28             :         }
      29             : 
      30           0 :         __skb_dst_copy(skb, data->dst);
      31           0 :         *qdisc_skb_cb(skb) = data->cb;
      32           0 :         skb->inner_protocol = data->inner_protocol;
      33           0 :         if (data->vlan_tci & VLAN_CFI_MASK)
      34           0 :                 __vlan_hwaccel_put_tag(skb, data->vlan_proto,
      35             :                                        data->vlan_tci & ~VLAN_CFI_MASK);
      36             :         else
      37           0 :                 __vlan_hwaccel_clear_tag(skb);
      38             : 
      39             :         /* Reconstruct the MAC header.  */
      40           0 :         skb_push(skb, data->l2_len);
      41           0 :         memcpy(skb->data, &data->l2_data, data->l2_len);
      42           0 :         skb_postpush_rcsum(skb, skb->data, data->l2_len);
      43           0 :         skb_reset_mac_header(skb);
      44             : 
      45           0 :         return data->xmit(skb);
      46             : }
      47             : 
      48           0 : static void sch_frag_prepare_frag(struct sk_buff *skb,
      49             :                                   int (*xmit)(struct sk_buff *skb))
      50             : {
      51           0 :         unsigned int hlen = skb_network_offset(skb);
      52           0 :         struct sch_frag_data *data;
      53             : 
      54           0 :         data = this_cpu_ptr(&sch_frag_data_storage);
      55           0 :         data->dst = skb->_skb_refdst;
      56           0 :         data->cb = *qdisc_skb_cb(skb);
      57           0 :         data->xmit = xmit;
      58           0 :         data->inner_protocol = skb->inner_protocol;
      59           0 :         if (skb_vlan_tag_present(skb))
      60           0 :                 data->vlan_tci = skb_vlan_tag_get(skb) | VLAN_CFI_MASK;
      61             :         else
      62           0 :                 data->vlan_tci = 0;
      63           0 :         data->vlan_proto = skb->vlan_proto;
      64           0 :         data->l2_len = hlen;
      65           0 :         memcpy(&data->l2_data, skb->data, hlen);
      66             : 
      67           0 :         memset(IPCB(skb), 0, sizeof(struct inet_skb_parm));
      68           0 :         skb_pull(skb, hlen);
      69           0 : }
      70             : 
      71             : static unsigned int
      72           0 : sch_frag_dst_get_mtu(const struct dst_entry *dst)
      73             : {
      74           0 :         return dst->dev->mtu;
      75             : }
      76             : 
      77             : static struct dst_ops sch_frag_dst_ops = {
      78             :         .family = AF_UNSPEC,
      79             :         .mtu = sch_frag_dst_get_mtu,
      80             : };
      81             : 
      82           0 : static int sch_fragment(struct net *net, struct sk_buff *skb,
      83             :                         u16 mru, int (*xmit)(struct sk_buff *skb))
      84             : {
      85           0 :         int ret = -1;
      86             : 
      87           0 :         if (skb_network_offset(skb) > VLAN_ETH_HLEN) {
      88           0 :                 net_warn_ratelimited("L2 header too long to fragment\n");
      89           0 :                 goto err;
      90             :         }
      91             : 
      92           0 :         if (skb_protocol(skb, true) == htons(ETH_P_IP)) {
      93           0 :                 struct dst_entry sch_frag_dst;
      94           0 :                 unsigned long orig_dst;
      95             : 
      96           0 :                 sch_frag_prepare_frag(skb, xmit);
      97           0 :                 dst_init(&sch_frag_dst, &sch_frag_dst_ops, NULL, 1,
      98             :                          DST_OBSOLETE_NONE, DST_NOCOUNT);
      99           0 :                 sch_frag_dst.dev = skb->dev;
     100             : 
     101           0 :                 orig_dst = skb->_skb_refdst;
     102           0 :                 skb_dst_set_noref(skb, &sch_frag_dst);
     103           0 :                 IPCB(skb)->frag_max_size = mru;
     104             : 
     105           0 :                 ret = ip_do_fragment(net, skb->sk, skb, sch_frag_xmit);
     106           0 :                 refdst_drop(orig_dst);
     107           0 :         } else if (skb_protocol(skb, true) == htons(ETH_P_IPV6)) {
     108           0 :                 unsigned long orig_dst;
     109           0 :                 struct rt6_info sch_frag_rt;
     110             : 
     111           0 :                 sch_frag_prepare_frag(skb, xmit);
     112           0 :                 memset(&sch_frag_rt, 0, sizeof(sch_frag_rt));
     113           0 :                 dst_init(&sch_frag_rt.dst, &sch_frag_dst_ops, NULL, 1,
     114             :                          DST_OBSOLETE_NONE, DST_NOCOUNT);
     115           0 :                 sch_frag_rt.dst.dev = skb->dev;
     116             : 
     117           0 :                 orig_dst = skb->_skb_refdst;
     118           0 :                 skb_dst_set_noref(skb, &sch_frag_rt.dst);
     119           0 :                 IP6CB(skb)->frag_max_size = mru;
     120             : 
     121           0 :                 ret = ipv6_stub->ipv6_fragment(net, skb->sk, skb,
     122             :                                                sch_frag_xmit);
     123           0 :                 refdst_drop(orig_dst);
     124             :         } else {
     125           0 :                 net_warn_ratelimited("Fail frag %s: eth=%x, MRU=%d, MTU=%d\n",
     126             :                                      netdev_name(skb->dev),
     127             :                                      ntohs(skb_protocol(skb, true)), mru,
     128             :                                      skb->dev->mtu);
     129           0 :                 goto err;
     130             :         }
     131             : 
     132             :         return ret;
     133           0 : err:
     134           0 :         kfree_skb(skb);
     135           0 :         return ret;
     136             : }
     137             : 
     138           0 : int sch_frag_xmit_hook(struct sk_buff *skb, int (*xmit)(struct sk_buff *skb))
     139             : {
     140           0 :         u16 mru = qdisc_skb_cb(skb)->mru;
     141           0 :         int err;
     142             : 
     143           0 :         if (mru && skb->len > mru + skb->dev->hard_header_len)
     144           0 :                 err = sch_fragment(dev_net(skb->dev), skb, mru, xmit);
     145             :         else
     146           0 :                 err = xmit(skb);
     147             : 
     148           0 :         return err;
     149             : }
     150             : EXPORT_SYMBOL_GPL(sch_frag_xmit_hook);