1// SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
2/*
3 * Copyright (c) 2020 Mellanox Technologies. All rights reserved.
4 */
5
6#include <rdma/ib_verbs.h>
7#include <rdma/ib_cache.h>
8#include <rdma/lag.h>
9
10static struct sk_buff *rdma_build_skb(struct net_device *netdev,
11				      struct rdma_ah_attr *ah_attr,
12				      gfp_t flags)
13{
14	struct ipv6hdr *ip6h;
15	struct sk_buff *skb;
16	struct ethhdr *eth;
17	struct iphdr *iph;
18	struct udphdr *uh;
19	u8 smac[ETH_ALEN];
20	bool is_ipv4;
21	int hdr_len;
22
23	is_ipv4 = ipv6_addr_v4mapped((struct in6_addr *)ah_attr->grh.dgid.raw);
24	hdr_len = ETH_HLEN + sizeof(struct udphdr) + LL_RESERVED_SPACE(netdev);
25	hdr_len += is_ipv4 ? sizeof(struct iphdr) : sizeof(struct ipv6hdr);
26
27	skb = alloc_skb(hdr_len, flags);
28	if (!skb)
29		return NULL;
30
31	skb->dev = netdev;
32	skb_reserve(skb, hdr_len);
33	skb_push(skb, sizeof(struct udphdr));
34	skb_reset_transport_header(skb);
35	uh = udp_hdr(skb);
36	uh->source =
37		htons(rdma_flow_label_to_udp_sport(ah_attr->grh.flow_label));
38	uh->dest = htons(ROCE_V2_UDP_DPORT);
39	uh->len = htons(sizeof(struct udphdr));
40
41	if (is_ipv4) {
42		skb_push(skb, sizeof(struct iphdr));
43		skb_reset_network_header(skb);
44		iph = ip_hdr(skb);
45		iph->frag_off = 0;
46		iph->version = 4;
47		iph->protocol = IPPROTO_UDP;
48		iph->ihl = 0x5;
49		iph->tot_len = htons(sizeof(struct udphdr) + sizeof(struct
50								    iphdr));
51		memcpy(&iph->saddr, ah_attr->grh.sgid_attr->gid.raw + 12,
52		       sizeof(struct in_addr));
53		memcpy(&iph->daddr, ah_attr->grh.dgid.raw + 12,
54		       sizeof(struct in_addr));
55	} else {
56		skb_push(skb, sizeof(struct ipv6hdr));
57		skb_reset_network_header(skb);
58		ip6h = ipv6_hdr(skb);
59		ip6h->version = 6;
60		ip6h->nexthdr = IPPROTO_UDP;
61		memcpy(&ip6h->flow_lbl, &ah_attr->grh.flow_label,
62		       sizeof(*ip6h->flow_lbl));
63		memcpy(&ip6h->saddr, ah_attr->grh.sgid_attr->gid.raw,
64		       sizeof(struct in6_addr));
65		memcpy(&ip6h->daddr, ah_attr->grh.dgid.raw,
66		       sizeof(struct in6_addr));
67	}
68
69	skb_push(skb, sizeof(struct ethhdr));
70	skb_reset_mac_header(skb);
71	eth = eth_hdr(skb);
72	skb->protocol = eth->h_proto = htons(is_ipv4 ? ETH_P_IP : ETH_P_IPV6);
73	rdma_read_gid_l2_fields(ah_attr->grh.sgid_attr, NULL, smac);
74	memcpy(eth->h_source, smac, ETH_ALEN);
75	memcpy(eth->h_dest, ah_attr->roce.dmac, ETH_ALEN);
76
77	return skb;
78}
79
80static struct net_device *rdma_get_xmit_slave_udp(struct ib_device *device,
81						  struct net_device *master,
82						  struct rdma_ah_attr *ah_attr,
83						  gfp_t flags)
84{
85	struct net_device *slave;
86	struct sk_buff *skb;
87
88	skb = rdma_build_skb(master, ah_attr, flags);
89	if (!skb)
90		return ERR_PTR(-ENOMEM);
91
92	rcu_read_lock();
93	slave = netdev_get_xmit_slave(master, skb,
94				      !!(device->lag_flags &
95					 RDMA_LAG_FLAGS_HASH_ALL_SLAVES));
96	if (slave)
97		dev_hold(slave);
98	rcu_read_unlock();
99	kfree_skb(skb);
100	return slave;
101}
102
103void rdma_lag_put_ah_roce_slave(struct net_device *xmit_slave)
104{
105	dev_put(xmit_slave);
106}
107
108struct net_device *rdma_lag_get_ah_roce_slave(struct ib_device *device,
109					      struct rdma_ah_attr *ah_attr,
110					      gfp_t flags)
111{
112	struct net_device *slave = NULL;
113	struct net_device *master;
114
115	if (!(ah_attr->type == RDMA_AH_ATTR_TYPE_ROCE &&
116	      ah_attr->grh.sgid_attr->gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP &&
117	      ah_attr->grh.flow_label))
118		return NULL;
119
120	rcu_read_lock();
121	master = rdma_read_gid_attr_ndev_rcu(ah_attr->grh.sgid_attr);
122	if (IS_ERR(master)) {
123		rcu_read_unlock();
124		return master;
125	}
126	dev_hold(master);
127	rcu_read_unlock();
128
129	if (!netif_is_bond_master(master))
130		goto put;
131
132	slave = rdma_get_xmit_slave_udp(device, master, ah_attr, flags);
133put:
134	dev_put(master);
135	return slave;
136}
137