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