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 rx_handler_result_t
  47 rmnet_bridge_handler(struct sk_buff *skb, struct rmnet_endpoint *ep)
  48 {
  49         if (!ep->egress_dev)
  50                 kfree_skb(skb);
  51         else
  52                 rmnet_egress_handler(skb, ep);
  53
  54         return RX_HANDLER_CONSUMED;
  55 }
  56
  57 static rx_handler_result_t
  58 rmnet_deliver_skb(struct sk_buff *skb, struct rmnet_endpoint *ep)
  59 {
  60         switch (ep->rmnet_mode) {
  61         case RMNET_EPMODE_NONE:
  62                 return RX_HANDLER_PASS;
  63
  64         case RMNET_EPMODE_BRIDGE:
  65                 return rmnet_bridge_handler(skb, ep);
  66
  67         case RMNET_EPMODE_VND:
  68                 skb_reset_transport_header(skb);
  69                 skb_reset_network_header(skb);
  70                 rmnet_vnd_rx_fixup(skb, skb->dev);
  71
  72                 skb->pkt_type = PACKET_HOST;
  73                 skb_set_mac_header(skb, 0);
  74                 netif_receive_skb(skb);
  75                 return RX_HANDLER_CONSUMED;
  76
  77         default:
  78                 kfree_skb(skb);
  79                 return RX_HANDLER_CONSUMED;
  80         }
  81 }
  82
  83 static rx_handler_result_t
  84 rmnet_ingress_deliver_packet(struct sk_buff *skb,
  85                              struct rmnet_port *port)
  86 {
  87         if (!port) {
  88                 kfree_skb(skb);
  89                 return RX_HANDLER_CONSUMED;
  90         }
  91
  92         skb->dev = port->local_ep.egress_dev;
  93
  94         return rmnet_deliver_skb(skb, &port->local_ep);
  95 }
  96
  97 /* MAP handler */
  98
  99 static rx_handler_result_t
 100 __rmnet_map_ingress_handler(struct sk_buff *skb,
 101                             struct rmnet_port *port)
 102 {
 103         struct rmnet_endpoint *ep;
 104         u8 mux_id;
 105         u16 len;
 106
 107         if (RMNET_MAP_GET_CD_BIT(skb)) {
 108                 if (port->ingress_data_format
 109                     & RMNET_INGRESS_FORMAT_MAP_COMMANDS)
 110                         return rmnet_map_command(skb, port);
 111
 112                 kfree_skb(skb);
 113                 return RX_HANDLER_CONSUMED;
 114         }
 115
 116         mux_id = RMNET_MAP_GET_MUX_ID(skb);
 117         len = RMNET_MAP_GET_LENGTH(skb) - RMNET_MAP_GET_PAD(skb);
 118
 119         if (mux_id >= RMNET_MAX_LOGICAL_EP) {
 120                 kfree_skb(skb);
 121                 return RX_HANDLER_CONSUMED;
 122         }
 123
 124         ep = &port->muxed_ep[mux_id];
 125
 126         if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEMUXING)
 127                 skb->dev = ep->egress_dev;
 128
 129         /* Subtract MAP header */
 130         skb_pull(skb, sizeof(struct rmnet_map_header));
 131         skb_trim(skb, len);
 132         rmnet_set_skb_proto(skb);
 133         return rmnet_deliver_skb(skb, ep);
 134 }
 135
 136 static rx_handler_result_t
 137 rmnet_map_ingress_handler(struct sk_buff *skb,
 138                           struct rmnet_port *port)
 139 {
 140         struct sk_buff *skbn;
 141         int rc;
 142
 143         if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) {
 144                 while ((skbn = rmnet_map_deaggregate(skb)) != NULL)
 145                         __rmnet_map_ingress_handler(skbn, port);
 146
 147                 consume_skb(skb);
 148                 rc = RX_HANDLER_CONSUMED;
 149         } else {
 150                 rc = __rmnet_map_ingress_handler(skb, port);
 151         }
 152
 153         return rc;
 154 }
 155
 156 static int rmnet_map_egress_handler(struct sk_buff *skb,
 157                                     struct rmnet_port *port,
 158                                     struct rmnet_endpoint *ep,
 159                                     struct net_device *orig_dev)
 160 {
 161         int required_headroom, additional_header_len;
 162         struct rmnet_map_header *map_header;
 163
 164         additional_header_len = 0;
 165         required_headroom = sizeof(struct rmnet_map_header);
 166
 167         if (skb_headroom(skb) < required_headroom) {
 168                 if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL))
 169                         goto fail;
 170         }
 171
 172         map_header = rmnet_map_add_map_header(skb, additional_header_len, 0);
 173         if (!map_header)
 174                 goto fail;
 175
 176         if (port->egress_data_format & RMNET_EGRESS_FORMAT_MUXING) {
 177                 if (ep->mux_id == 0xff)
 178                         map_header->mux_id = 0;
 179                 else
 180                         map_header->mux_id = ep->mux_id;
 181         }
 182
 183         skb->protocol = htons(ETH_P_MAP);
 184
 185         return RMNET_MAP_SUCCESS;
 186
 187 fail:
 188         kfree_skb(skb);
 189         return RMNET_MAP_CONSUMED;
 190 }
 191
 192 /* Ingress / Egress Entry Points */
 193
 194 /* Processes packet as per ingress data format for receiving device. Logical
 195  * endpoint is determined from packet inspection. Packet is then sent to the
 196  * egress device listed in the logical endpoint configuration.
 197  */
 198 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb)
 199 {
 200         struct rmnet_port *port;
 201         struct sk_buff *skb = *pskb;
 202         struct net_device *dev;
 203         int rc;
 204
 205         if (!skb)
 206                 return RX_HANDLER_CONSUMED;
 207
 208         if (skb->pkt_type == PACKET_LOOPBACK)
 209                 return RX_HANDLER_PASS;
 210
 211         dev = skb->dev;
 212         port = rmnet_get_port(dev);
 213
 214         if (port->ingress_data_format & RMNET_INGRESS_FORMAT_MAP) {
 215                 rc = rmnet_map_ingress_handler(skb, port);
 216         } else {
 217                 switch (ntohs(skb->protocol)) {
 218                 case ETH_P_MAP:
 219                         if (port->local_ep.rmnet_mode ==
 220                                 RMNET_EPMODE_BRIDGE) {
 221                                 rc = rmnet_ingress_deliver_packet(skb, port);
 222                         } else {
 223                                 kfree_skb(skb);
 224                                 rc = RX_HANDLER_CONSUMED;
 225                         }
 226                         break;
 227
 228                 case ETH_P_IP:
 229                 case ETH_P_IPV6:
 230                         rc = rmnet_ingress_deliver_packet(skb, port);
 231                         break;
 232
 233                 default:
 234                         rc = RX_HANDLER_PASS;
 235                 }
 236         }
 237
 238         return rc;
 239 }
 240
 241 /* Modifies packet as per logical endpoint configuration and egress data format
 242  * for egress device configured in logical endpoint. Packet is then transmitted
 243  * on the egress device.
 244  */
 245 void rmnet_egress_handler(struct sk_buff *skb,
 246                           struct rmnet_endpoint *ep)
 247 {
 248         struct net_device *orig_dev;
 249         struct rmnet_port *port;
 250
 251         orig_dev = skb->dev;
 252         skb->dev = ep->egress_dev;
 253
 254         port = rmnet_get_port(skb->dev);
 255         if (!port) {
 256                 kfree_skb(skb);
 257                 return;
 258         }
 259
 260         if (port->egress_data_format & RMNET_EGRESS_FORMAT_MAP) {
 261                 switch (rmnet_map_egress_handler(skb, port, ep, orig_dev)) {
 262                 case RMNET_MAP_CONSUMED:
 263                         return;
 264
 265                 case RMNET_MAP_SUCCESS:
 266                         break;
 267
 268                 default:
 269                         kfree_skb(skb);
 270                         return;
 271                 }
 272         }
 273
 274         if (ep->rmnet_mode == RMNET_EPMODE_VND)
 275                 rmnet_vnd_tx_fixup(skb, orig_dev);
 276
 277         dev_queue_xmit(skb);
 278 }