1 // SPDX-License-Identifier: GPL-2.0-only
2 /* Copyright (c) 2013-2018, 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 
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
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
56 __rmnet_map_ingress_handler(struct sk_buff *skb,
57 			    struct rmnet_port *port)
58 {
59 	struct rmnet_endpoint *ep;
60 	u16 len, pad;
61 	u8 mux_id;
62 
63 	if (RMNET_MAP_GET_CD_BIT(skb)) {
64 		if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS)
65 			return rmnet_map_command(skb, port);
66 
67 		goto free_skb;
68 	}
69 
70 	mux_id = RMNET_MAP_GET_MUX_ID(skb);
71 	pad = RMNET_MAP_GET_PAD(skb);
72 	len = RMNET_MAP_GET_LENGTH(skb) - pad;
73 
74 	if (mux_id >= RMNET_MAX_LOGICAL_EP)
75 		goto free_skb;
76 
77 	ep = rmnet_get_endpoint(port, mux_id);
78 	if (!ep)
79 		goto free_skb;
80 
81 	skb->dev = ep->egress_dev;
82 
83 	/* Subtract MAP header */
84 	skb_pull(skb, sizeof(struct rmnet_map_header));
85 	rmnet_set_skb_proto(skb);
86 
87 	if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) {
88 		if (!rmnet_map_checksum_downlink_packet(skb, len + pad))
89 			skb->ip_summed = CHECKSUM_UNNECESSARY;
90 	}
91 
92 	skb_trim(skb, len);
93 	rmnet_deliver_skb(skb);
94 	return;
95 
96 free_skb:
97 	kfree_skb(skb);
98 }
99 
100 static void
101 rmnet_map_ingress_handler(struct sk_buff *skb,
102 			  struct rmnet_port *port)
103 {
104 	struct sk_buff *skbn;
105 
106 	if (skb->dev->type == ARPHRD_ETHER) {
107 		if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_ATOMIC)) {
108 			kfree_skb(skb);
109 			return;
110 		}
111 
112 		skb_push(skb, ETH_HLEN);
113 	}
114 
115 	if (port->data_format & RMNET_FLAGS_INGRESS_DEAGGREGATION) {
116 		while ((skbn = rmnet_map_deaggregate(skb, port)) != NULL)
117 			__rmnet_map_ingress_handler(skbn, port);
118 
119 		consume_skb(skb);
120 	} else {
121 		__rmnet_map_ingress_handler(skb, port);
122 	}
123 }
124 
125 static int rmnet_map_egress_handler(struct sk_buff *skb,
126 				    struct rmnet_port *port, u8 mux_id,
127 				    struct net_device *orig_dev)
128 {
129 	int required_headroom, additional_header_len;
130 	struct rmnet_map_header *map_header;
131 
132 	additional_header_len = 0;
133 	required_headroom = sizeof(struct rmnet_map_header);
134 
135 	if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4) {
136 		additional_header_len = sizeof(struct rmnet_map_ul_csum_header);
137 		required_headroom += additional_header_len;
138 	}
139 
140 	if (skb_headroom(skb) < required_headroom) {
141 		if (pskb_expand_head(skb, required_headroom, 0, GFP_ATOMIC))
142 			return -ENOMEM;
143 	}
144 
145 	if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4)
146 		rmnet_map_checksum_uplink_packet(skb, orig_dev);
147 
148 	map_header = rmnet_map_add_map_header(skb, additional_header_len, 0);
149 	if (!map_header)
150 		return -ENOMEM;
151 
152 	map_header->mux_id = mux_id;
153 
154 	skb->protocol = htons(ETH_P_MAP);
155 
156 	return 0;
157 }
158 
159 static void
160 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
161 {
162 	if (skb_mac_header_was_set(skb))
163 		skb_push(skb, skb->mac_len);
164 
165 	if (bridge_dev) {
166 		skb->dev = bridge_dev;
167 		dev_queue_xmit(skb);
168 	}
169 }
170 
171 /* Ingress / Egress Entry Points */
172 
173 /* Processes packet as per ingress data format for receiving device. Logical
174  * endpoint is determined from packet inspection. Packet is then sent to the
175  * egress device listed in the logical endpoint configuration.
176  */
177 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
178 {
179 	struct sk_buff *skb = *pskb;
180 	struct rmnet_port *port;
181 	struct net_device *dev;
182 
183 	if (!skb)
184 		goto done;
185 
186 	if (skb->pkt_type == PACKET_LOOPBACK)
187 		return RX_HANDLER_PASS;
188 
189 	dev = skb->dev;
190 	port = rmnet_get_port_rcu(dev);
191 
192 	switch (port->rmnet_mode) {
193 	case RMNET_EPMODE_VND:
194 		rmnet_map_ingress_handler(skb, port);
195 		break;
196 	case RMNET_EPMODE_BRIDGE:
197 		rmnet_bridge_handler(skb, port->bridge_ep);
198 		break;
199 	}
200 
201 done:
202 	return RX_HANDLER_CONSUMED;
203 }
204 
205 /* Modifies packet as per logical endpoint configuration and egress data format
206  * for egress device configured in logical endpoint. Packet is then transmitted
207  * on the egress device.
208  */
209 void rmnet_egress_handler(struct sk_buff *skb)
210 {
211 	struct net_device *orig_dev;
212 	struct rmnet_port *port;
213 	struct rmnet_priv *priv;
214 	u8 mux_id;
215 
216 	sk_pacing_shift_update(skb->sk, 8);
217 
218 	orig_dev = skb->dev;
219 	priv = netdev_priv(orig_dev);
220 	skb->dev = priv->real_dev;
221 	mux_id = priv->mux_id;
222 
223 	port = rmnet_get_port_rcu(skb->dev);
224 	if (!port)
225 		goto drop;
226 
227 	if (rmnet_map_egress_handler(skb, port, mux_id, orig_dev))
228 		goto drop;
229 
230 	rmnet_vnd_tx_fixup(skb, orig_dev);
231 
232 	dev_queue_xmit(skb);
233 	return;
234 
235 drop:
236 	this_cpu_inc(priv->pcpu_stats->stats.tx_drops);
237 	kfree_skb(skb);
238 }
239