drivers/net/ethernet/qualcomm/rmnet/rmnet_handlers.c

   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         skb->protocol = htons(ETH_P_MAP);
 168
 169         return 0;
 170 }
 171
 172 static void
 173 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
 174 {
 175         if (skb_mac_header_was_set(skb))
 176                 skb_push(skb, skb->mac_len);
 177
 178         if (bridge_dev) {
 179                 skb->dev = bridge_dev;
 180                 dev_queue_xmit(skb);
 181         }
 182 }
 183
 184 /* Ingress / Egress Entry Points */
 185
 186 /* Processes packet as per ingress data format for receiving device. Logical
 187  * endpoint is determined from packet inspection. Packet is then sent to the
 188  * egress device listed in the logical endpoint configuration.
 189  */
 190 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
 191 {
 192         struct sk_buff *skb = *pskb;
 193         struct rmnet_port *port;
 194         struct net_device *dev;
 195
 196         if (!skb)
 197                 goto done;
 198
 199         if (skb_linearize(skb)) {
 200                 kfree_skb(skb);
 201                 goto done;
 202         }
 203
 204         if (skb->pkt_type == PACKET_LOOPBACK)
 205                 return RX_HANDLER_PASS;
 206
 207         dev = skb->dev;
 208         port = rmnet_get_port_rcu(dev);
 209         if (unlikely(!port)) {
 210                 dev_core_stats_rx_nohandler_inc(skb->dev);
 211                 kfree_skb(skb);
 212                 goto done;
 213         }
 214
 215         switch (port->rmnet_mode) {
 216         case RMNET_EPMODE_VND:
 217                 rmnet_map_ingress_handler(skb, port);
 218                 break;
 219         case RMNET_EPMODE_BRIDGE:
 220                 rmnet_bridge_handler(skb, port->bridge_ep);
 221                 break;
 222         }
 223
 224 done:
 225         return RX_HANDLER_CONSUMED;
 226 }
 227
 228 /* Modifies packet as per logical endpoint configuration and egress data format
 229  * for egress device configured in logical endpoint. Packet is then transmitted
 230  * on the egress device.
 231  */
 232 void rmnet_egress_handler(struct sk_buff *skb)
 233 {
 234         struct net_device *orig_dev;
 235         struct rmnet_port *port;
 236         struct rmnet_priv *priv;
 237         u8 mux_id;
 238
 239         sk_pacing_shift_update(skb->sk, 8);
 240
 241         orig_dev = skb->dev;
 242         priv = netdev_priv(orig_dev);
 243         skb->dev = priv->real_dev;
 244         mux_id = priv->mux_id;
 245
 246         port = rmnet_get_port_rcu(skb->dev);
 247         if (!port)
 248                 goto drop;
 249
 250         if (rmnet_map_egress_handler(skb, port, mux_id, orig_dev))
 251                 goto drop;
 252
 253         rmnet_vnd_tx_fixup(skb, orig_dev);
 254
 255         dev_queue_xmit(skb);
 256         return;
 257
 258 drop:
 259         this_cpu_inc(priv->pcpu_stats->stats.tx_drops);
 260         kfree_skb(skb);
 261 }