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

   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 }