1 // SPDX-License-Identifier: GPL-2.0-only
2 /* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved.
3  *
4  * RMNET Data MAP protocol
5  */
6 
7 #include <linux/netdevice.h>
8 #include <linux/ip.h>
9 #include <linux/ipv6.h>
10 #include <net/ip6_checksum.h>
11 #include <linux/bitfield.h>
12 #include "rmnet_config.h"
13 #include "rmnet_map.h"
14 #include "rmnet_private.h"
15 #include "rmnet_vnd.h"
16 
17 #define RMNET_MAP_DEAGGR_SPACING  64
18 #define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
19 
rmnet_map_get_csum_field(unsigned char protocol,const void * txporthdr)20 static __sum16 *rmnet_map_get_csum_field(unsigned char protocol,
21 					 const void *txporthdr)
22 {
23 	if (protocol == IPPROTO_TCP)
24 		return &((struct tcphdr *)txporthdr)->check;
25 
26 	if (protocol == IPPROTO_UDP)
27 		return &((struct udphdr *)txporthdr)->check;
28 
29 	return NULL;
30 }
31 
32 static int
rmnet_map_ipv4_dl_csum_trailer(struct sk_buff * skb,struct rmnet_map_dl_csum_trailer * csum_trailer,struct rmnet_priv * priv)33 rmnet_map_ipv4_dl_csum_trailer(struct sk_buff *skb,
34 			       struct rmnet_map_dl_csum_trailer *csum_trailer,
35 			       struct rmnet_priv *priv)
36 {
37 	struct iphdr *ip4h = (struct iphdr *)skb->data;
38 	void *txporthdr = skb->data + ip4h->ihl * 4;
39 	__sum16 *csum_field, pseudo_csum;
40 	__sum16 ip_payload_csum;
41 
42 	/* Computing the checksum over just the IPv4 header--including its
43 	 * checksum field--should yield 0.  If it doesn't, the IP header
44 	 * is bad, so return an error and let the IP layer drop it.
45 	 */
46 	if (ip_fast_csum(ip4h, ip4h->ihl)) {
47 		priv->stats.csum_ip4_header_bad++;
48 		return -EINVAL;
49 	}
50 
51 	/* We don't support checksum offload on IPv4 fragments */
52 	if (ip_is_fragment(ip4h)) {
53 		priv->stats.csum_fragmented_pkt++;
54 		return -EOPNOTSUPP;
55 	}
56 
57 	/* Checksum offload is only supported for UDP and TCP protocols */
58 	csum_field = rmnet_map_get_csum_field(ip4h->protocol, txporthdr);
59 	if (!csum_field) {
60 		priv->stats.csum_err_invalid_transport++;
61 		return -EPROTONOSUPPORT;
62 	}
63 
64 	/* RFC 768: UDP checksum is optional for IPv4, and is 0 if unused */
65 	if (!*csum_field && ip4h->protocol == IPPROTO_UDP) {
66 		priv->stats.csum_skipped++;
67 		return 0;
68 	}
69 
70 	/* The checksum value in the trailer is computed over the entire
71 	 * IP packet, including the IP header and payload.  To derive the
72 	 * transport checksum from this, we first subract the contribution
73 	 * of the IP header from the trailer checksum.  We then add the
74 	 * checksum computed over the pseudo header.
75 	 *
76 	 * We verified above that the IP header contributes zero to the
77 	 * trailer checksum.  Therefore the checksum in the trailer is
78 	 * just the checksum computed over the IP payload.
79 
80 	 * If the IP payload arrives intact, adding the pseudo header
81 	 * checksum to the IP payload checksum will yield 0xffff (negative
82 	 * zero).  This means the trailer checksum and the pseudo checksum
83 	 * are additive inverses of each other.  Put another way, the
84 	 * message passes the checksum test if the trailer checksum value
85 	 * is the negated pseudo header checksum.
86 	 *
87 	 * Knowing this, we don't even need to examine the transport
88 	 * header checksum value; it is already accounted for in the
89 	 * checksum value found in the trailer.
90 	 */
91 	ip_payload_csum = csum_trailer->csum_value;
92 
93 	pseudo_csum = csum_tcpudp_magic(ip4h->saddr, ip4h->daddr,
94 					ntohs(ip4h->tot_len) - ip4h->ihl * 4,
95 					ip4h->protocol, 0);
96 
97 	/* The cast is required to ensure only the low 16 bits are examined */
98 	if (ip_payload_csum != (__sum16)~pseudo_csum) {
99 		priv->stats.csum_validation_failed++;
100 		return -EINVAL;
101 	}
102 
103 	priv->stats.csum_ok++;
104 	return 0;
105 }
106 
107 #if IS_ENABLED(CONFIG_IPV6)
108 static int
rmnet_map_ipv6_dl_csum_trailer(struct sk_buff * skb,struct rmnet_map_dl_csum_trailer * csum_trailer,struct rmnet_priv * priv)109 rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb,
110 			       struct rmnet_map_dl_csum_trailer *csum_trailer,
111 			       struct rmnet_priv *priv)
112 {
113 	struct ipv6hdr *ip6h = (struct ipv6hdr *)skb->data;
114 	void *txporthdr = skb->data + sizeof(*ip6h);
115 	__sum16 *csum_field, pseudo_csum;
116 	__sum16 ip6_payload_csum;
117 	__be16 ip_header_csum;
118 
119 	/* Checksum offload is only supported for UDP and TCP protocols;
120 	 * the packet cannot include any IPv6 extension headers
121 	 */
122 	csum_field = rmnet_map_get_csum_field(ip6h->nexthdr, txporthdr);
123 	if (!csum_field) {
124 		priv->stats.csum_err_invalid_transport++;
125 		return -EPROTONOSUPPORT;
126 	}
127 
128 	/* The checksum value in the trailer is computed over the entire
129 	 * IP packet, including the IP header and payload.  To derive the
130 	 * transport checksum from this, we first subract the contribution
131 	 * of the IP header from the trailer checksum.  We then add the
132 	 * checksum computed over the pseudo header.
133 	 */
134 	ip_header_csum = (__force __be16)ip_fast_csum(ip6h, sizeof(*ip6h) / 4);
135 	ip6_payload_csum = csum16_sub(csum_trailer->csum_value, ip_header_csum);
136 
137 	pseudo_csum = csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
138 				      ntohs(ip6h->payload_len),
139 				      ip6h->nexthdr, 0);
140 
141 	/* It's sufficient to compare the IP payload checksum with the
142 	 * negated pseudo checksum to determine whether the packet
143 	 * checksum was good.  (See further explanation in comments
144 	 * in rmnet_map_ipv4_dl_csum_trailer()).
145 	 *
146 	 * The cast is required to ensure only the low 16 bits are
147 	 * examined.
148 	 */
149 	if (ip6_payload_csum != (__sum16)~pseudo_csum) {
150 		priv->stats.csum_validation_failed++;
151 		return -EINVAL;
152 	}
153 
154 	priv->stats.csum_ok++;
155 	return 0;
156 }
157 #else
158 static int
rmnet_map_ipv6_dl_csum_trailer(struct sk_buff * skb,struct rmnet_map_dl_csum_trailer * csum_trailer,struct rmnet_priv * priv)159 rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb,
160 			       struct rmnet_map_dl_csum_trailer *csum_trailer,
161 			       struct rmnet_priv *priv)
162 {
163 	return 0;
164 }
165 #endif
166 
rmnet_map_complement_ipv4_txporthdr_csum_field(struct iphdr * ip4h)167 static void rmnet_map_complement_ipv4_txporthdr_csum_field(struct iphdr *ip4h)
168 {
169 	void *txphdr;
170 	u16 *csum;
171 
172 	txphdr = (void *)ip4h + ip4h->ihl * 4;
173 
174 	if (ip4h->protocol == IPPROTO_TCP || ip4h->protocol == IPPROTO_UDP) {
175 		csum = (u16 *)rmnet_map_get_csum_field(ip4h->protocol, txphdr);
176 		*csum = ~(*csum);
177 	}
178 }
179 
180 static void
rmnet_map_ipv4_ul_csum_header(struct iphdr * iphdr,struct rmnet_map_ul_csum_header * ul_header,struct sk_buff * skb)181 rmnet_map_ipv4_ul_csum_header(struct iphdr *iphdr,
182 			      struct rmnet_map_ul_csum_header *ul_header,
183 			      struct sk_buff *skb)
184 {
185 	u16 val;
186 
187 	val = MAP_CSUM_UL_ENABLED_FLAG;
188 	if (iphdr->protocol == IPPROTO_UDP)
189 		val |= MAP_CSUM_UL_UDP_FLAG;
190 	val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
191 
192 	ul_header->csum_start_offset = htons(skb_network_header_len(skb));
193 	ul_header->csum_info = htons(val);
194 
195 	skb->ip_summed = CHECKSUM_NONE;
196 
197 	rmnet_map_complement_ipv4_txporthdr_csum_field(iphdr);
198 }
199 
200 #if IS_ENABLED(CONFIG_IPV6)
201 static void
rmnet_map_complement_ipv6_txporthdr_csum_field(struct ipv6hdr * ip6h)202 rmnet_map_complement_ipv6_txporthdr_csum_field(struct ipv6hdr *ip6h)
203 {
204 	void *txphdr;
205 	u16 *csum;
206 
207 	txphdr = ip6h + 1;
208 
209 	if (ip6h->nexthdr == IPPROTO_TCP || ip6h->nexthdr == IPPROTO_UDP) {
210 		csum = (u16 *)rmnet_map_get_csum_field(ip6h->nexthdr, txphdr);
211 		*csum = ~(*csum);
212 	}
213 }
214 
215 static void
rmnet_map_ipv6_ul_csum_header(struct ipv6hdr * ipv6hdr,struct rmnet_map_ul_csum_header * ul_header,struct sk_buff * skb)216 rmnet_map_ipv6_ul_csum_header(struct ipv6hdr *ipv6hdr,
217 			      struct rmnet_map_ul_csum_header *ul_header,
218 			      struct sk_buff *skb)
219 {
220 	u16 val;
221 
222 	val = MAP_CSUM_UL_ENABLED_FLAG;
223 	if (ipv6hdr->nexthdr == IPPROTO_UDP)
224 		val |= MAP_CSUM_UL_UDP_FLAG;
225 	val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
226 
227 	ul_header->csum_start_offset = htons(skb_network_header_len(skb));
228 	ul_header->csum_info = htons(val);
229 
230 	skb->ip_summed = CHECKSUM_NONE;
231 
232 	rmnet_map_complement_ipv6_txporthdr_csum_field(ipv6hdr);
233 }
234 #else
235 static void
rmnet_map_ipv6_ul_csum_header(void * ip6hdr,struct rmnet_map_ul_csum_header * ul_header,struct sk_buff * skb)236 rmnet_map_ipv6_ul_csum_header(void *ip6hdr,
237 			      struct rmnet_map_ul_csum_header *ul_header,
238 			      struct sk_buff *skb)
239 {
240 }
241 #endif
242 
rmnet_map_v5_checksum_uplink_packet(struct sk_buff * skb,struct rmnet_port * port,struct net_device * orig_dev)243 static void rmnet_map_v5_checksum_uplink_packet(struct sk_buff *skb,
244 						struct rmnet_port *port,
245 						struct net_device *orig_dev)
246 {
247 	struct rmnet_priv *priv = netdev_priv(orig_dev);
248 	struct rmnet_map_v5_csum_header *ul_header;
249 
250 	ul_header = skb_push(skb, sizeof(*ul_header));
251 	memset(ul_header, 0, sizeof(*ul_header));
252 	ul_header->header_info = u8_encode_bits(RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD,
253 						MAPV5_HDRINFO_HDR_TYPE_FMASK);
254 
255 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
256 		void *iph = ip_hdr(skb);
257 		__sum16 *check;
258 		void *trans;
259 		u8 proto;
260 
261 		if (skb->protocol == htons(ETH_P_IP)) {
262 			u16 ip_len = ((struct iphdr *)iph)->ihl * 4;
263 
264 			proto = ((struct iphdr *)iph)->protocol;
265 			trans = iph + ip_len;
266 		} else if (IS_ENABLED(CONFIG_IPV6) &&
267 			   skb->protocol == htons(ETH_P_IPV6)) {
268 			u16 ip_len = sizeof(struct ipv6hdr);
269 
270 			proto = ((struct ipv6hdr *)iph)->nexthdr;
271 			trans = iph + ip_len;
272 		} else {
273 			priv->stats.csum_err_invalid_ip_version++;
274 			goto sw_csum;
275 		}
276 
277 		check = rmnet_map_get_csum_field(proto, trans);
278 		if (check) {
279 			skb->ip_summed = CHECKSUM_NONE;
280 			/* Ask for checksum offloading */
281 			ul_header->csum_info |= MAPV5_CSUMINFO_VALID_FLAG;
282 			priv->stats.csum_hw++;
283 			return;
284 		}
285 	}
286 
287 sw_csum:
288 	priv->stats.csum_sw++;
289 }
290 
291 /* Adds MAP header to front of skb->data
292  * Padding is calculated and set appropriately in MAP header. Mux ID is
293  * initialized to 0.
294  */
rmnet_map_add_map_header(struct sk_buff * skb,int hdrlen,struct rmnet_port * port,int pad)295 struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
296 						  int hdrlen,
297 						  struct rmnet_port *port,
298 						  int pad)
299 {
300 	struct rmnet_map_header *map_header;
301 	u32 padding, map_datalen;
302 
303 	map_datalen = skb->len - hdrlen;
304 	map_header = (struct rmnet_map_header *)
305 			skb_push(skb, sizeof(struct rmnet_map_header));
306 	memset(map_header, 0, sizeof(struct rmnet_map_header));
307 
308 	/* Set next_hdr bit for csum offload packets */
309 	if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5)
310 		map_header->flags |= MAP_NEXT_HEADER_FLAG;
311 
312 	if (pad == RMNET_MAP_NO_PAD_BYTES) {
313 		map_header->pkt_len = htons(map_datalen);
314 		return map_header;
315 	}
316 
317 	BUILD_BUG_ON(MAP_PAD_LEN_MASK < 3);
318 	padding = ALIGN(map_datalen, 4) - map_datalen;
319 
320 	if (padding == 0)
321 		goto done;
322 
323 	if (skb_tailroom(skb) < padding)
324 		return NULL;
325 
326 	skb_put_zero(skb, padding);
327 
328 done:
329 	map_header->pkt_len = htons(map_datalen + padding);
330 	/* This is a data packet, so the CMD bit is 0 */
331 	map_header->flags = padding & MAP_PAD_LEN_MASK;
332 
333 	return map_header;
334 }
335 
336 /* Deaggregates a single packet
337  * A whole new buffer is allocated for each portion of an aggregated frame.
338  * Caller should keep calling deaggregate() on the source skb until 0 is
339  * returned, indicating that there are no more packets to deaggregate. Caller
340  * is responsible for freeing the original skb.
341  */
rmnet_map_deaggregate(struct sk_buff * skb,struct rmnet_port * port)342 struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb,
343 				      struct rmnet_port *port)
344 {
345 	struct rmnet_map_v5_csum_header *next_hdr = NULL;
346 	struct rmnet_map_header *maph;
347 	void *data = skb->data;
348 	struct sk_buff *skbn;
349 	u8 nexthdr_type;
350 	u32 packet_len;
351 
352 	if (skb->len == 0)
353 		return NULL;
354 
355 	maph = (struct rmnet_map_header *)skb->data;
356 	packet_len = ntohs(maph->pkt_len) + sizeof(*maph);
357 
358 	if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) {
359 		packet_len += sizeof(struct rmnet_map_dl_csum_trailer);
360 	} else if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) {
361 		if (!(maph->flags & MAP_CMD_FLAG)) {
362 			packet_len += sizeof(*next_hdr);
363 			if (maph->flags & MAP_NEXT_HEADER_FLAG)
364 				next_hdr = data + sizeof(*maph);
365 			else
366 				/* Mapv5 data pkt without csum hdr is invalid */
367 				return NULL;
368 		}
369 	}
370 
371 	if (((int)skb->len - (int)packet_len) < 0)
372 		return NULL;
373 
374 	/* Some hardware can send us empty frames. Catch them */
375 	if (!maph->pkt_len)
376 		return NULL;
377 
378 	if (next_hdr) {
379 		nexthdr_type = u8_get_bits(next_hdr->header_info,
380 					   MAPV5_HDRINFO_HDR_TYPE_FMASK);
381 		if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
382 			return NULL;
383 	}
384 
385 	skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
386 	if (!skbn)
387 		return NULL;
388 
389 	skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
390 	skb_put(skbn, packet_len);
391 	memcpy(skbn->data, skb->data, packet_len);
392 	skb_pull(skb, packet_len);
393 
394 	return skbn;
395 }
396 
397 /* Validates packet checksums. Function takes a pointer to
398  * the beginning of a buffer which contains the IP payload +
399  * padding + checksum trailer.
400  * Only IPv4 and IPv6 are supported along with TCP & UDP.
401  * Fragmented or tunneled packets are not supported.
402  */
rmnet_map_checksum_downlink_packet(struct sk_buff * skb,u16 len)403 int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len)
404 {
405 	struct rmnet_priv *priv = netdev_priv(skb->dev);
406 	struct rmnet_map_dl_csum_trailer *csum_trailer;
407 
408 	if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
409 		priv->stats.csum_sw++;
410 		return -EOPNOTSUPP;
411 	}
412 
413 	csum_trailer = (struct rmnet_map_dl_csum_trailer *)(skb->data + len);
414 
415 	if (!(csum_trailer->flags & MAP_CSUM_DL_VALID_FLAG)) {
416 		priv->stats.csum_valid_unset++;
417 		return -EINVAL;
418 	}
419 
420 	if (skb->protocol == htons(ETH_P_IP))
421 		return rmnet_map_ipv4_dl_csum_trailer(skb, csum_trailer, priv);
422 
423 	if (IS_ENABLED(CONFIG_IPV6) && skb->protocol == htons(ETH_P_IPV6))
424 		return rmnet_map_ipv6_dl_csum_trailer(skb, csum_trailer, priv);
425 
426 	priv->stats.csum_err_invalid_ip_version++;
427 
428 	return -EPROTONOSUPPORT;
429 }
430 
rmnet_map_v4_checksum_uplink_packet(struct sk_buff * skb,struct net_device * orig_dev)431 static void rmnet_map_v4_checksum_uplink_packet(struct sk_buff *skb,
432 						struct net_device *orig_dev)
433 {
434 	struct rmnet_priv *priv = netdev_priv(orig_dev);
435 	struct rmnet_map_ul_csum_header *ul_header;
436 	void *iphdr;
437 
438 	ul_header = (struct rmnet_map_ul_csum_header *)
439 		    skb_push(skb, sizeof(struct rmnet_map_ul_csum_header));
440 
441 	if (unlikely(!(orig_dev->features &
442 		     (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM))))
443 		goto sw_csum;
444 
445 	if (skb->ip_summed != CHECKSUM_PARTIAL)
446 		goto sw_csum;
447 
448 	iphdr = (char *)ul_header +
449 		sizeof(struct rmnet_map_ul_csum_header);
450 
451 	if (skb->protocol == htons(ETH_P_IP)) {
452 		rmnet_map_ipv4_ul_csum_header(iphdr, ul_header, skb);
453 		priv->stats.csum_hw++;
454 		return;
455 	}
456 
457 	if (IS_ENABLED(CONFIG_IPV6) && skb->protocol == htons(ETH_P_IPV6)) {
458 		rmnet_map_ipv6_ul_csum_header(iphdr, ul_header, skb);
459 		priv->stats.csum_hw++;
460 		return;
461 	}
462 
463 	priv->stats.csum_err_invalid_ip_version++;
464 
465 sw_csum:
466 	memset(ul_header, 0, sizeof(*ul_header));
467 
468 	priv->stats.csum_sw++;
469 }
470 
471 /* Generates UL checksum meta info header for IPv4 and IPv6 over TCP and UDP
472  * packets that are supported for UL checksum offload.
473  */
rmnet_map_checksum_uplink_packet(struct sk_buff * skb,struct rmnet_port * port,struct net_device * orig_dev,int csum_type)474 void rmnet_map_checksum_uplink_packet(struct sk_buff *skb,
475 				      struct rmnet_port *port,
476 				      struct net_device *orig_dev,
477 				      int csum_type)
478 {
479 	switch (csum_type) {
480 	case RMNET_FLAGS_EGRESS_MAP_CKSUMV4:
481 		rmnet_map_v4_checksum_uplink_packet(skb, orig_dev);
482 		break;
483 	case RMNET_FLAGS_EGRESS_MAP_CKSUMV5:
484 		rmnet_map_v5_checksum_uplink_packet(skb, port, orig_dev);
485 		break;
486 	default:
487 		break;
488 	}
489 }
490 
491 /* Process a MAPv5 packet header */
rmnet_map_process_next_hdr_packet(struct sk_buff * skb,u16 len)492 int rmnet_map_process_next_hdr_packet(struct sk_buff *skb,
493 				      u16 len)
494 {
495 	struct rmnet_priv *priv = netdev_priv(skb->dev);
496 	struct rmnet_map_v5_csum_header *next_hdr;
497 	u8 nexthdr_type;
498 
499 	next_hdr = (struct rmnet_map_v5_csum_header *)(skb->data +
500 			sizeof(struct rmnet_map_header));
501 
502 	nexthdr_type = u8_get_bits(next_hdr->header_info,
503 				   MAPV5_HDRINFO_HDR_TYPE_FMASK);
504 
505 	if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
506 		return -EINVAL;
507 
508 	if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
509 		priv->stats.csum_sw++;
510 	} else if (next_hdr->csum_info & MAPV5_CSUMINFO_VALID_FLAG) {
511 		priv->stats.csum_ok++;
512 		skb->ip_summed = CHECKSUM_UNNECESSARY;
513 	} else {
514 		priv->stats.csum_valid_unset++;
515 	}
516 
517 	/* Pull csum v5 header */
518 	skb_pull(skb, sizeof(*next_hdr));
519 
520 	return 0;
521 }
522 
523 #define RMNET_AGG_BYPASS_TIME_NSEC 10000000L
524 
reset_aggr_params(struct rmnet_port * port)525 static void reset_aggr_params(struct rmnet_port *port)
526 {
527 	port->skbagg_head = NULL;
528 	port->agg_count = 0;
529 	port->agg_state = 0;
530 	memset(&port->agg_time, 0, sizeof(struct timespec64));
531 }
532 
rmnet_send_skb(struct rmnet_port * port,struct sk_buff * skb)533 static void rmnet_send_skb(struct rmnet_port *port, struct sk_buff *skb)
534 {
535 	if (skb_needs_linearize(skb, port->dev->features)) {
536 		if (unlikely(__skb_linearize(skb))) {
537 			struct rmnet_priv *priv;
538 
539 			priv = netdev_priv(port->rmnet_dev);
540 			this_cpu_inc(priv->pcpu_stats->stats.tx_drops);
541 			dev_kfree_skb_any(skb);
542 			return;
543 		}
544 	}
545 
546 	dev_queue_xmit(skb);
547 }
548 
rmnet_map_flush_tx_packet_work(struct work_struct * work)549 static void rmnet_map_flush_tx_packet_work(struct work_struct *work)
550 {
551 	struct sk_buff *skb = NULL;
552 	struct rmnet_port *port;
553 
554 	port = container_of(work, struct rmnet_port, agg_wq);
555 
556 	spin_lock_bh(&port->agg_lock);
557 	if (likely(port->agg_state == -EINPROGRESS)) {
558 		/* Buffer may have already been shipped out */
559 		if (likely(port->skbagg_head)) {
560 			skb = port->skbagg_head;
561 			reset_aggr_params(port);
562 		}
563 		port->agg_state = 0;
564 	}
565 
566 	spin_unlock_bh(&port->agg_lock);
567 	if (skb)
568 		rmnet_send_skb(port, skb);
569 }
570 
rmnet_map_flush_tx_packet_queue(struct hrtimer * t)571 static enum hrtimer_restart rmnet_map_flush_tx_packet_queue(struct hrtimer *t)
572 {
573 	struct rmnet_port *port;
574 
575 	port = container_of(t, struct rmnet_port, hrtimer);
576 
577 	schedule_work(&port->agg_wq);
578 
579 	return HRTIMER_NORESTART;
580 }
581 
rmnet_map_tx_aggregate(struct sk_buff * skb,struct rmnet_port * port,struct net_device * orig_dev)582 unsigned int rmnet_map_tx_aggregate(struct sk_buff *skb, struct rmnet_port *port,
583 				    struct net_device *orig_dev)
584 {
585 	struct timespec64 diff, last;
586 	unsigned int len = skb->len;
587 	struct sk_buff *agg_skb;
588 	int size;
589 
590 	spin_lock_bh(&port->agg_lock);
591 	memcpy(&last, &port->agg_last, sizeof(struct timespec64));
592 	ktime_get_real_ts64(&port->agg_last);
593 
594 	if (!port->skbagg_head) {
595 		/* Check to see if we should agg first. If the traffic is very
596 		 * sparse, don't aggregate.
597 		 */
598 new_packet:
599 		diff = timespec64_sub(port->agg_last, last);
600 		size = port->egress_agg_params.bytes - skb->len;
601 
602 		if (size < 0) {
603 			/* dropped */
604 			spin_unlock_bh(&port->agg_lock);
605 			return 0;
606 		}
607 
608 		if (diff.tv_sec > 0 || diff.tv_nsec > RMNET_AGG_BYPASS_TIME_NSEC ||
609 		    size == 0)
610 			goto no_aggr;
611 
612 		port->skbagg_head = skb_copy_expand(skb, 0, size, GFP_ATOMIC);
613 		if (!port->skbagg_head)
614 			goto no_aggr;
615 
616 		dev_kfree_skb_any(skb);
617 		port->skbagg_head->protocol = htons(ETH_P_MAP);
618 		port->agg_count = 1;
619 		ktime_get_real_ts64(&port->agg_time);
620 		skb_frag_list_init(port->skbagg_head);
621 		goto schedule;
622 	}
623 	diff = timespec64_sub(port->agg_last, port->agg_time);
624 	size = port->egress_agg_params.bytes - port->skbagg_head->len;
625 
626 	if (skb->len > size) {
627 		agg_skb = port->skbagg_head;
628 		reset_aggr_params(port);
629 		spin_unlock_bh(&port->agg_lock);
630 		hrtimer_cancel(&port->hrtimer);
631 		rmnet_send_skb(port, agg_skb);
632 		spin_lock_bh(&port->agg_lock);
633 		goto new_packet;
634 	}
635 
636 	if (skb_has_frag_list(port->skbagg_head))
637 		port->skbagg_tail->next = skb;
638 	else
639 		skb_shinfo(port->skbagg_head)->frag_list = skb;
640 
641 	port->skbagg_head->len += skb->len;
642 	port->skbagg_head->data_len += skb->len;
643 	port->skbagg_head->truesize += skb->truesize;
644 	port->skbagg_tail = skb;
645 	port->agg_count++;
646 
647 	if (diff.tv_sec > 0 || diff.tv_nsec > port->egress_agg_params.time_nsec ||
648 	    port->agg_count >= port->egress_agg_params.count ||
649 	    port->skbagg_head->len == port->egress_agg_params.bytes) {
650 		agg_skb = port->skbagg_head;
651 		reset_aggr_params(port);
652 		spin_unlock_bh(&port->agg_lock);
653 		hrtimer_cancel(&port->hrtimer);
654 		rmnet_send_skb(port, agg_skb);
655 		return len;
656 	}
657 
658 schedule:
659 	if (!hrtimer_active(&port->hrtimer) && port->agg_state != -EINPROGRESS) {
660 		port->agg_state = -EINPROGRESS;
661 		hrtimer_start(&port->hrtimer,
662 			      ns_to_ktime(port->egress_agg_params.time_nsec),
663 			      HRTIMER_MODE_REL);
664 	}
665 	spin_unlock_bh(&port->agg_lock);
666 
667 	return len;
668 
669 no_aggr:
670 	spin_unlock_bh(&port->agg_lock);
671 	skb->protocol = htons(ETH_P_MAP);
672 	dev_queue_xmit(skb);
673 
674 	return len;
675 }
676 
rmnet_map_update_ul_agg_config(struct rmnet_port * port,u32 size,u32 count,u32 time)677 void rmnet_map_update_ul_agg_config(struct rmnet_port *port, u32 size,
678 				    u32 count, u32 time)
679 {
680 	spin_lock_bh(&port->agg_lock);
681 	port->egress_agg_params.bytes = size;
682 	WRITE_ONCE(port->egress_agg_params.count, count);
683 	port->egress_agg_params.time_nsec = time * NSEC_PER_USEC;
684 	spin_unlock_bh(&port->agg_lock);
685 }
686 
rmnet_map_tx_aggregate_init(struct rmnet_port * port)687 void rmnet_map_tx_aggregate_init(struct rmnet_port *port)
688 {
689 	hrtimer_init(&port->hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
690 	port->hrtimer.function = rmnet_map_flush_tx_packet_queue;
691 	spin_lock_init(&port->agg_lock);
692 	rmnet_map_update_ul_agg_config(port, 4096, 1, 800);
693 	INIT_WORK(&port->agg_wq, rmnet_map_flush_tx_packet_work);
694 }
695 
rmnet_map_tx_aggregate_exit(struct rmnet_port * port)696 void rmnet_map_tx_aggregate_exit(struct rmnet_port *port)
697 {
698 	hrtimer_cancel(&port->hrtimer);
699 	cancel_work_sync(&port->agg_wq);
700 
701 	spin_lock_bh(&port->agg_lock);
702 	if (port->agg_state == -EINPROGRESS) {
703 		if (port->skbagg_head) {
704 			dev_kfree_skb_any(port->skbagg_head);
705 			reset_aggr_params(port);
706 		}
707 
708 		port->agg_state = 0;
709 	}
710 	spin_unlock_bh(&port->agg_lock);
711 }
712