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