1 // SPDX-License-Identifier: GPL-2.0-only
2 /* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved.
3  *
4  * RMNET Data ingress/egress handler
5  */
6 
7 #include <linux/netdevice.h>
8 #include <linux/netdev_features.h>
9 #include <linux/if_arp.h>
10 #include <net/sock.h>
11 #include "rmnet_private.h"
12 #include "rmnet_config.h"
13 #include "rmnet_vnd.h"
14 #include "rmnet_map.h"
15 #include "rmnet_handlers.h"
16 
17 #define RMNET_IP_VERSION_4 0x40
18 #define RMNET_IP_VERSION_6 0x60
19 
20 /* Helper Functions */
21 
rmnet_set_skb_proto(struct sk_buff * skb)22 static void rmnet_set_skb_proto(struct sk_buff *skb)
23 {
24 	switch (skb->data[0] & 0xF0) {
25 	case RMNET_IP_VERSION_4:
26 		skb->protocol = htons(ETH_P_IP);
27 		break;
28 	case RMNET_IP_VERSION_6:
29 		skb->protocol = htons(ETH_P_IPV6);
30 		break;
31 	default:
32 		skb->protocol = htons(ETH_P_MAP);
33 		break;
34 	}
35 }
36 
37 /* Generic handler */
38 
39 static void
rmnet_deliver_skb(struct sk_buff * skb)40 rmnet_deliver_skb(struct sk_buff *skb)
41 {
42 	struct rmnet_priv *priv = netdev_priv(skb->dev);
43 
44 	skb_reset_transport_header(skb);
45 	skb_reset_network_header(skb);
46 	rmnet_vnd_rx_fixup(skb, skb->dev);
47 
48 	skb->pkt_type = PACKET_HOST;
49 	skb_set_mac_header(skb, 0);
50 	gro_cells_receive(&priv->gro_cells, skb);
51 }
52 
53 /* MAP handler */
54 
55 static void
__rmnet_map_ingress_handler(struct sk_buff * skb,struct rmnet_port * port)56 __rmnet_map_ingress_handler(struct sk_buff *skb,
57 			    struct rmnet_port *port)
58 {
59 	struct rmnet_map_header *map_header = (void *)skb->data;
60 	struct rmnet_endpoint *ep;
61 	u16 len, pad;
62 	u8 mux_id;
63 
64 	if (map_header->flags & MAP_CMD_FLAG) {
65 		/* Packet contains a MAP command (not data) */
66 		if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS)
67 			return rmnet_map_command(skb, port);
68 
69 		goto free_skb;
70 	}
71 
72 	mux_id = map_header->mux_id;
73 	pad = map_header->flags & MAP_PAD_LEN_MASK;
74 	len = ntohs(map_header->pkt_len) - pad;
75 
76 	if (mux_id >= RMNET_MAX_LOGICAL_EP)
77 		goto free_skb;
78 
79 	ep = rmnet_get_endpoint(port, mux_id);
80 	if (!ep)
81 		goto free_skb;
82 
83 	skb->dev = ep->egress_dev;
84 
85 	if ((port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) &&
86 	    (map_header->flags & MAP_NEXT_HEADER_FLAG)) {
87 		if (rmnet_map_process_next_hdr_packet(skb, len))
88 			goto free_skb;
89 		skb_pull(skb, sizeof(*map_header));
90 		rmnet_set_skb_proto(skb);
91 	} else {
92 		/* Subtract MAP header */
93 		skb_pull(skb, sizeof(*map_header));
94 		rmnet_set_skb_proto(skb);
95 		if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4 &&
96 		    !rmnet_map_checksum_downlink_packet(skb, len + pad))
97 			skb->ip_summed = CHECKSUM_UNNECESSARY;
98 	}
99 
100 	skb_trim(skb, len);
101 	rmnet_deliver_skb(skb);
102 	return;
103 
104 free_skb:
105 	kfree_skb(skb);
106 }
107 
108 static void
rmnet_map_ingress_handler(struct sk_buff * skb,struct rmnet_port * port)109 rmnet_map_ingress_handler(struct sk_buff *skb,
110 			  struct rmnet_port *port)
111 {
112 	struct sk_buff *skbn;
113 
114 	if (skb->dev->type == ARPHRD_ETHER) {
115 		if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_ATOMIC)) {
116 			kfree_skb(skb);
117 			return;
118 		}
119 
120 		skb_push(skb, ETH_HLEN);
121 	}
122 
123 	if (port->data_format & RMNET_FLAGS_INGRESS_DEAGGREGATION) {
124 		while ((skbn = rmnet_map_deaggregate(skb, port)) != NULL)
125 			__rmnet_map_ingress_handler(skbn, port);
126 
127 		consume_skb(skb);
128 	} else {
129 		__rmnet_map_ingress_handler(skb, port);
130 	}
131 }
132 
rmnet_map_egress_handler(struct sk_buff * skb,struct rmnet_port * port,u8 mux_id,struct net_device * orig_dev)133 static int rmnet_map_egress_handler(struct sk_buff *skb,
134 				    struct rmnet_port *port, u8 mux_id,
135 				    struct net_device *orig_dev)
136 {
137 	int required_headroom, additional_header_len, csum_type = 0;
138 	struct rmnet_map_header *map_header;
139 
140 	additional_header_len = 0;
141 	required_headroom = sizeof(struct rmnet_map_header);
142 
143 	if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4) {
144 		additional_header_len = sizeof(struct rmnet_map_ul_csum_header);
145 		csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV4;
146 	} else if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5) {
147 		additional_header_len = sizeof(struct rmnet_map_v5_csum_header);
148 		csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV5;
149 	}
150 
151 	required_headroom += additional_header_len;
152 
153 	if (skb_cow_head(skb, required_headroom) < 0)
154 		return -ENOMEM;
155 
156 	if (csum_type)
157 		rmnet_map_checksum_uplink_packet(skb, port, orig_dev,
158 						 csum_type);
159 
160 	map_header = rmnet_map_add_map_header(skb, additional_header_len,
161 					      port, 0);
162 	if (!map_header)
163 		return -ENOMEM;
164 
165 	map_header->mux_id = mux_id;
166 
167 	if (READ_ONCE(port->egress_agg_params.count) > 1) {
168 		unsigned int len;
169 
170 		len = rmnet_map_tx_aggregate(skb, port, orig_dev);
171 		if (likely(len)) {
172 			rmnet_vnd_tx_fixup_len(len, orig_dev);
173 			return -EINPROGRESS;
174 		}
175 		return -ENOMEM;
176 	}
177 
178 	skb->protocol = htons(ETH_P_MAP);
179 	return 0;
180 }
181 
182 static void
rmnet_bridge_handler(struct sk_buff * skb,struct net_device * bridge_dev)183 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
184 {
185 	if (skb_mac_header_was_set(skb))
186 		skb_push(skb, skb->mac_len);
187 
188 	if (bridge_dev) {
189 		skb->dev = bridge_dev;
190 		dev_queue_xmit(skb);
191 	}
192 }
193 
194 /* Ingress / Egress Entry Points */
195 
196 /* Processes packet as per ingress data format for receiving device. Logical
197  * endpoint is determined from packet inspection. Packet is then sent to the
198  * egress device listed in the logical endpoint configuration.
199  */
rmnet_rx_handler(struct sk_buff ** pskb)200 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
201 {
202 	struct sk_buff *skb = *pskb;
203 	struct rmnet_port *port;
204 	struct net_device *dev;
205 
206 	if (!skb)
207 		goto done;
208 
209 	if (skb_linearize(skb)) {
210 		kfree_skb(skb);
211 		goto done;
212 	}
213 
214 	if (skb->pkt_type == PACKET_LOOPBACK)
215 		return RX_HANDLER_PASS;
216 
217 	dev = skb->dev;
218 	port = rmnet_get_port_rcu(dev);
219 	if (unlikely(!port)) {
220 		dev_core_stats_rx_nohandler_inc(skb->dev);
221 		kfree_skb(skb);
222 		goto done;
223 	}
224 
225 	switch (port->rmnet_mode) {
226 	case RMNET_EPMODE_VND:
227 		rmnet_map_ingress_handler(skb, port);
228 		break;
229 	case RMNET_EPMODE_BRIDGE:
230 		rmnet_bridge_handler(skb, port->bridge_ep);
231 		break;
232 	}
233 
234 done:
235 	return RX_HANDLER_CONSUMED;
236 }
237 
238 /* Modifies packet as per logical endpoint configuration and egress data format
239  * for egress device configured in logical endpoint. Packet is then transmitted
240  * on the egress device.
241  */
rmnet_egress_handler(struct sk_buff * skb)242 void rmnet_egress_handler(struct sk_buff *skb)
243 {
244 	struct net_device *orig_dev;
245 	struct rmnet_port *port;
246 	struct rmnet_priv *priv;
247 	u8 mux_id;
248 	int err;
249 
250 	sk_pacing_shift_update(skb->sk, 8);
251 
252 	orig_dev = skb->dev;
253 	priv = netdev_priv(orig_dev);
254 	skb->dev = priv->real_dev;
255 	mux_id = priv->mux_id;
256 
257 	port = rmnet_get_port_rcu(skb->dev);
258 	if (!port)
259 		goto drop;
260 
261 	err = rmnet_map_egress_handler(skb, port, mux_id, orig_dev);
262 	if (err == -ENOMEM)
263 		goto drop;
264 	else if (err == -EINPROGRESS)
265 		return;
266 
267 	rmnet_vnd_tx_fixup(skb, orig_dev);
268 
269 	dev_queue_xmit(skb);
270 	return;
271 
272 drop:
273 	this_cpu_inc(priv->pcpu_stats->stats.tx_drops);
274 	kfree_skb(skb);
275 }
276