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