276 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved.3 *4 * RMNET Data ingress/egress handler5 */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 0x4018#define RMNET_IP_VERSION_6 0x6019 20/* Helper Functions */21 22static 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 39static void40rmnet_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 55static void56__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 104free_skb:105 kfree_skb(skb);106}107 108static void109rmnet_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 133static 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 182static void183rmnet_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. Logical197 * endpoint is determined from packet inspection. Packet is then sent to the198 * egress device listed in the logical endpoint configuration.199 */200rx_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 234done:235 return RX_HANDLER_CONSUMED;236}237 238/* Modifies packet as per logical endpoint configuration and egress data format239 * for egress device configured in logical endpoint. Packet is then transmitted240 * on the egress device.241 */242void 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 272drop:273 this_cpu_inc(priv->pcpu_stats->stats.tx_drops);274 kfree_skb(skb);275}276