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

   1 /* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
   2  *
   3  * This program is free software; you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License version 2 and
   5  * only version 2 as published by the Free Software Foundation.
   6  *
   7  * This program is distributed in the hope that it will be useful,
   8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10  * GNU General Public License for more details.
  11  *
  12  * RMNET Data ingress/egress handler
  13  *
  14  */
  15
  16 #include <linux/netdevice.h>
  17 #include <linux/netdev_features.h>
  18 #include "rmnet_private.h"
  19 #include "rmnet_config.h"
  20 #include "rmnet_vnd.h"
  21 #include "rmnet_map.h"
  22 #include "rmnet_handlers.h"
  23
  24 #define RMNET_IP_VERSION_4 0x40
  25 #define RMNET_IP_VERSION_6 0x60
  26
  27 /* Helper Functions */
  28
  29 static void rmnet_set_skb_proto(struct sk_buff *skb)
  30 {
  31         switch (skb->data[0] & 0xF0) {
  32         case RMNET_IP_VERSION_4:
  33                 skb->protocol = htons(ETH_P_IP);
  34                 break;
  35         case RMNET_IP_VERSION_6:
  36                 skb->protocol = htons(ETH_P_IPV6);
  37                 break;
  38         default:
  39                 skb->protocol = htons(ETH_P_MAP);
  40                 break;
  41         }
  42 }
  43
  44 /* Generic handler */
  45
  46 static void
  47 rmnet_deliver_skb(struct sk_buff *skb)
  48 {
  49         struct rmnet_priv *priv = netdev_priv(skb->dev);
  50
  51         skb_reset_transport_header(skb);
  52         skb_reset_network_header(skb);
  53         rmnet_vnd_rx_fixup(skb, skb->dev);
  54
  55         skb->pkt_type = PACKET_HOST;
  56         skb_set_mac_header(skb, 0);
  57         gro_cells_receive(&priv->gro_cells, skb);
  58 }
  59
  60 /* MAP handler */
  61
  62 static void
  63 __rmnet_map_ingress_handler(struct sk_buff *skb,
  64                             struct rmnet_port *port)
  65 {
  66         struct rmnet_endpoint *ep;
  67         u8 mux_id;
  68         u16 len;
  69
  70         if (RMNET_MAP_GET_CD_BIT(skb)) {
  71                 if (port->ingress_data_format
  72                     & RMNET_INGRESS_FORMAT_MAP_COMMANDS)
  73                         return rmnet_map_command(skb, port);
  74
  75                 goto free_skb;
  76         }
  77
  78         mux_id = RMNET_MAP_GET_MUX_ID(skb);
  79         len = RMNET_MAP_GET_LENGTH(skb) - RMNET_MAP_GET_PAD(skb);
  80
  81         if (mux_id >= RMNET_MAX_LOGICAL_EP)
  82                 goto free_skb;
  83
  84         ep = rmnet_get_endpoint(port, mux_id);
  85         if (!ep)
  86                 goto free_skb;
  87
  88         skb->dev = ep->egress_dev;
  89
  90         /* Subtract MAP header */
  91         skb_pull(skb, sizeof(struct rmnet_map_header));
  92         skb_trim(skb, len);
  93         rmnet_set_skb_proto(skb);
  94         rmnet_deliver_skb(skb);
  95         return;
  96
  97 free_skb:
  98         kfree_skb(skb);
  99 }
 100
 101 static void
 102 rmnet_map_ingress_handler(struct sk_buff *skb,
 103                           struct rmnet_port *port)
 104 {
 105         struct sk_buff *skbn;
 106
 107         if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) {
 108                 while ((skbn = rmnet_map_deaggregate(skb)) != NULL)
 109                         __rmnet_map_ingress_handler(skbn, port);
 110
 111                 consume_skb(skb);
 112         } else {
 113                 __rmnet_map_ingress_handler(skb, port);
 114         }
 115 }
 116
 117 static int rmnet_map_egress_handler(struct sk_buff *skb,
 118                                     struct rmnet_port *port, u8 mux_id,
 119                                     struct net_device *orig_dev)
 120 {
 121         int required_headroom, additional_header_len;
 122         struct rmnet_map_header *map_header;
 123
 124         additional_header_len = 0;
 125         required_headroom = sizeof(struct rmnet_map_header);
 126
 127         if (skb_headroom(skb) < required_headroom) {
 128                 if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL))
 129                         goto fail;
 130         }
 131
 132         map_header = rmnet_map_add_map_header(skb, additional_header_len, 0);
 133         if (!map_header)
 134                 goto fail;
 135
 136         if (port->egress_data_format & RMNET_EGRESS_FORMAT_MUXING) {
 137                 if (mux_id == 0xff)
 138                         map_header->mux_id = 0;
 139                 else
 140                         map_header->mux_id = mux_id;
 141         }
 142
 143         skb->protocol = htons(ETH_P_MAP);
 144
 145         return RMNET_MAP_SUCCESS;
 146
 147 fail:
 148         kfree_skb(skb);
 149         return RMNET_MAP_CONSUMED;
 150 }
 151
 152 static void
 153 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
 154 {
 155         if (bridge_dev) {
 156                 skb->dev = bridge_dev;
 157                 dev_queue_xmit(skb);
 158         }
 159 }
 160
 161 /* Ingress / Egress Entry Points */
 162
 163 /* Processes packet as per ingress data format for receiving device. Logical
 164  * endpoint is determined from packet inspection. Packet is then sent to the
 165  * egress device listed in the logical endpoint configuration.
 166  */
 167 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
 168 {
 169         struct sk_buff *skb = *pskb;
 170         struct rmnet_port *port;
 171         struct net_device *dev;
 172
 173         if (!skb)
 174                 goto done;
 175
 176         dev = skb->dev;
 177         port = rmnet_get_port(dev);
 178
 179         switch (port->rmnet_mode) {
 180         case RMNET_EPMODE_VND:
 181                 if (port->ingress_data_format & RMNET_INGRESS_FORMAT_MAP)
 182                         rmnet_map_ingress_handler(skb, port);
 183                 break;
 184         case RMNET_EPMODE_BRIDGE:
 185                 rmnet_bridge_handler(skb, port->bridge_ep);
 186                 break;
 187         }
 188
 189 done:
 190         return RX_HANDLER_CONSUMED;
 191 }
 192
 193 /* Modifies packet as per logical endpoint configuration and egress data format
 194  * for egress device configured in logical endpoint. Packet is then transmitted
 195  * on the egress device.
 196  */
 197 void rmnet_egress_handler(struct sk_buff *skb)
 198 {
 199         struct net_device *orig_dev;
 200         struct rmnet_port *port;
 201         struct rmnet_priv *priv;
 202         u8 mux_id;
 203
 204         orig_dev = skb->dev;
 205         priv = netdev_priv(orig_dev);
 206         skb->dev = priv->real_dev;
 207         mux_id = priv->mux_id;
 208
 209         port = rmnet_get_port(skb->dev);
 210         if (!port) {
 211                 kfree_skb(skb);
 212                 return;
 213         }
 214
 215         if (port->egress_data_format & RMNET_EGRESS_FORMAT_MAP) {
 216                 switch (rmnet_map_egress_handler(skb, port, mux_id, orig_dev)) {
 217                 case RMNET_MAP_CONSUMED:
 218                         return;
 219
 220                 case RMNET_MAP_SUCCESS:
 221                         break;
 222
 223                 default:
 224                         kfree_skb(skb);
 225                         return;
 226                 }
 227         }
 228
 229         rmnet_vnd_tx_fixup(skb, orig_dev);
 230
 231         dev_queue_xmit(skb);
 232 }