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 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_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 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 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 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 */ 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 */ 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