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