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 if (rmnet_map_validate_packet_len(skb, port))
130 __rmnet_map_ingress_handler(skb, port);
131 else
132 kfree_skb(skb);
133 }
134 }
135
rmnet_map_egress_handler(struct sk_buff * skb,struct rmnet_port * port,u8 mux_id,struct net_device * orig_dev)136 static int rmnet_map_egress_handler(struct sk_buff *skb,
137 struct rmnet_port *port, u8 mux_id,
138 struct net_device *orig_dev)
139 {
140 int required_headroom, additional_header_len, csum_type = 0;
141 struct rmnet_map_header *map_header;
142
143 additional_header_len = 0;
144 required_headroom = sizeof(struct rmnet_map_header);
145
146 if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4) {
147 additional_header_len = sizeof(struct rmnet_map_ul_csum_header);
148 csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV4;
149 } else if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5) {
150 additional_header_len = sizeof(struct rmnet_map_v5_csum_header);
151 csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV5;
152 }
153
154 required_headroom += additional_header_len;
155
156 if (skb_cow_head(skb, required_headroom) < 0)
157 return -ENOMEM;
158
159 if (csum_type)
160 rmnet_map_checksum_uplink_packet(skb, port, orig_dev,
161 csum_type);
162
163 map_header = rmnet_map_add_map_header(skb, additional_header_len,
164 port, 0);
165 if (!map_header)
166 return -ENOMEM;
167
168 map_header->mux_id = mux_id;
169
170 if (READ_ONCE(port->egress_agg_params.count) > 1) {
171 unsigned int len;
172
173 len = rmnet_map_tx_aggregate(skb, port, orig_dev);
174 if (likely(len)) {
175 rmnet_vnd_tx_fixup_len(len, orig_dev);
176 return -EINPROGRESS;
177 }
178 return -ENOMEM;
179 }
180
181 skb->protocol = htons(ETH_P_MAP);
182 return 0;
183 }
184
185 static void
rmnet_bridge_handler(struct sk_buff * skb,struct net_device * bridge_dev)186 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
187 {
188 if (skb_mac_header_was_set(skb))
189 skb_push(skb, skb->mac_len);
190
191 if (bridge_dev) {
192 skb->dev = bridge_dev;
193 dev_queue_xmit(skb);
194 }
195 }
196
197 /* Ingress / Egress Entry Points */
198
199 /* Processes packet as per ingress data format for receiving device. Logical
200 * endpoint is determined from packet inspection. Packet is then sent to the
201 * egress device listed in the logical endpoint configuration.
202 */
rmnet_rx_handler(struct sk_buff ** pskb)203 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
204 {
205 struct sk_buff *skb = *pskb;
206 struct rmnet_port *port;
207 struct net_device *dev;
208
209 if (!skb)
210 goto done;
211
212 if (skb_linearize(skb)) {
213 kfree_skb(skb);
214 goto done;
215 }
216
217 if (skb->pkt_type == PACKET_LOOPBACK)
218 return RX_HANDLER_PASS;
219
220 dev = skb->dev;
221 port = rmnet_get_port_rcu(dev);
222 if (unlikely(!port)) {
223 dev_core_stats_rx_nohandler_inc(skb->dev);
224 kfree_skb(skb);
225 goto done;
226 }
227
228 switch (port->rmnet_mode) {
229 case RMNET_EPMODE_VND:
230 rmnet_map_ingress_handler(skb, port);
231 break;
232 case RMNET_EPMODE_BRIDGE:
233 rmnet_bridge_handler(skb, port->bridge_ep);
234 break;
235 }
236
237 done:
238 return RX_HANDLER_CONSUMED;
239 }
240
241 /* Modifies packet as per logical endpoint configuration and egress data format
242 * for egress device configured in logical endpoint. Packet is then transmitted
243 * on the egress device.
244 */
rmnet_egress_handler(struct sk_buff * skb)245 void rmnet_egress_handler(struct sk_buff *skb)
246 {
247 struct net_device *orig_dev;
248 struct rmnet_port *port;
249 struct rmnet_priv *priv;
250 u8 mux_id;
251 int err;
252
253 sk_pacing_shift_update(skb->sk, 8);
254
255 orig_dev = skb->dev;
256 priv = netdev_priv(orig_dev);
257 skb->dev = priv->real_dev;
258 mux_id = priv->mux_id;
259
260 port = rmnet_get_port_rcu(skb->dev);
261 if (!port)
262 goto drop;
263
264 err = rmnet_map_egress_handler(skb, port, mux_id, orig_dev);
265 if (err == -ENOMEM)
266 goto drop;
267 else if (err == -EINPROGRESS)
268 return;
269
270 rmnet_vnd_tx_fixup(skb, orig_dev);
271
272 dev_queue_xmit(skb);
273 return;
274
275 drop:
276 this_cpu_inc(priv->pcpu_stats->stats.tx_drops);
277 kfree_skb(skb);
278 }
279