1 /* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
2  *
3  * This program is free software; you can redistribute it and/or modify
4  * it under the terms of the GNU General Public License version 2 and
5  * only version 2 as published by the Free Software Foundation.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * RMNET Data MAP protocol
13  *
14  */
15 
16 #include <linux/netdevice.h>
17 #include "rmnet_config.h"
18 #include "rmnet_map.h"
19 #include "rmnet_private.h"
20 
21 #define RMNET_MAP_DEAGGR_SPACING  64
22 #define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
23 
24 /* Adds MAP header to front of skb->data
25  * Padding is calculated and set appropriately in MAP header. Mux ID is
26  * initialized to 0.
27  */
28 struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
29 						  int hdrlen, int pad)
30 {
31 	struct rmnet_map_header *map_header;
32 	u32 padding, map_datalen;
33 	u8 *padbytes;
34 
35 	if (skb_headroom(skb) < sizeof(struct rmnet_map_header))
36 		return NULL;
37 
38 	map_datalen = skb->len - hdrlen;
39 	map_header = (struct rmnet_map_header *)
40 			skb_push(skb, sizeof(struct rmnet_map_header));
41 	memset(map_header, 0, sizeof(struct rmnet_map_header));
42 
43 	if (pad == RMNET_MAP_NO_PAD_BYTES) {
44 		map_header->pkt_len = htons(map_datalen);
45 		return map_header;
46 	}
47 
48 	padding = ALIGN(map_datalen, 4) - map_datalen;
49 
50 	if (padding == 0)
51 		goto done;
52 
53 	if (skb_tailroom(skb) < padding)
54 		return NULL;
55 
56 	padbytes = (u8 *)skb_put(skb, padding);
57 	memset(padbytes, 0, padding);
58 
59 done:
60 	map_header->pkt_len = htons(map_datalen + padding);
61 	map_header->pad_len = padding & 0x3F;
62 
63 	return map_header;
64 }
65 
66 /* Deaggregates a single packet
67  * A whole new buffer is allocated for each portion of an aggregated frame.
68  * Caller should keep calling deaggregate() on the source skb until 0 is
69  * returned, indicating that there are no more packets to deaggregate. Caller
70  * is responsible for freeing the original skb.
71  */
72 struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb)
73 {
74 	struct rmnet_map_header *maph;
75 	struct sk_buff *skbn;
76 	u32 packet_len;
77 
78 	if (skb->len == 0)
79 		return NULL;
80 
81 	maph = (struct rmnet_map_header *)skb->data;
82 	packet_len = ntohs(maph->pkt_len) + sizeof(struct rmnet_map_header);
83 
84 	if (((int)skb->len - (int)packet_len) < 0)
85 		return NULL;
86 
87 	/* Some hardware can send us empty frames. Catch them */
88 	if (ntohs(maph->pkt_len) == 0)
89 		return NULL;
90 
91 	skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
92 	if (!skbn)
93 		return NULL;
94 
95 	skbn->dev = skb->dev;
96 	skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
97 	skb_put(skbn, packet_len);
98 	memcpy(skbn->data, skb->data, packet_len);
99 	skb_pull(skb, packet_len);
100 
101 	return skbn;
102 }
103