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 <linux/if_arp.h> 19 #include "rmnet_private.h" 20 #include "rmnet_config.h" 21 #include "rmnet_vnd.h" 22 #include "rmnet_map.h" 23 #include "rmnet_handlers.h" 24 25 #define RMNET_IP_VERSION_4 0x40 26 #define RMNET_IP_VERSION_6 0x60 27 28 /* Helper Functions */ 29 30 static void rmnet_set_skb_proto(struct sk_buff *skb) 31 { 32 switch (skb->data[0] & 0xF0) { 33 case RMNET_IP_VERSION_4: 34 skb->protocol = htons(ETH_P_IP); 35 break; 36 case RMNET_IP_VERSION_6: 37 skb->protocol = htons(ETH_P_IPV6); 38 break; 39 default: 40 skb->protocol = htons(ETH_P_MAP); 41 break; 42 } 43 } 44 45 /* Generic handler */ 46 47 static void 48 rmnet_deliver_skb(struct sk_buff *skb) 49 { 50 struct rmnet_priv *priv = netdev_priv(skb->dev); 51 52 skb_reset_transport_header(skb); 53 skb_reset_network_header(skb); 54 rmnet_vnd_rx_fixup(skb, skb->dev); 55 56 skb->pkt_type = PACKET_HOST; 57 skb_set_mac_header(skb, 0); 58 gro_cells_receive(&priv->gro_cells, skb); 59 } 60 61 /* MAP handler */ 62 63 static void 64 __rmnet_map_ingress_handler(struct sk_buff *skb, 65 struct rmnet_port *port) 66 { 67 struct rmnet_endpoint *ep; 68 u8 mux_id; 69 u16 len; 70 71 if (RMNET_MAP_GET_CD_BIT(skb)) { 72 if (port->ingress_data_format 73 & RMNET_INGRESS_FORMAT_MAP_COMMANDS) 74 return rmnet_map_command(skb, port); 75 76 goto free_skb; 77 } 78 79 mux_id = RMNET_MAP_GET_MUX_ID(skb); 80 len = RMNET_MAP_GET_LENGTH(skb) - RMNET_MAP_GET_PAD(skb); 81 82 if (mux_id >= RMNET_MAX_LOGICAL_EP) 83 goto free_skb; 84 85 ep = rmnet_get_endpoint(port, mux_id); 86 if (!ep) 87 goto free_skb; 88 89 skb->dev = ep->egress_dev; 90 91 /* Subtract MAP header */ 92 skb_pull(skb, sizeof(struct rmnet_map_header)); 93 skb_trim(skb, len); 94 rmnet_set_skb_proto(skb); 95 rmnet_deliver_skb(skb); 96 return; 97 98 free_skb: 99 kfree_skb(skb); 100 } 101 102 static void 103 rmnet_map_ingress_handler(struct sk_buff *skb, 104 struct rmnet_port *port) 105 { 106 struct sk_buff *skbn; 107 108 if (skb->dev->type == ARPHRD_ETHER) { 109 if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_KERNEL)) { 110 kfree_skb(skb); 111 return; 112 } 113 114 skb_push(skb, ETH_HLEN); 115 } 116 117 if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) { 118 while ((skbn = rmnet_map_deaggregate(skb)) != NULL) 119 __rmnet_map_ingress_handler(skbn, port); 120 121 consume_skb(skb); 122 } else { 123 __rmnet_map_ingress_handler(skb, port); 124 } 125 } 126 127 static int rmnet_map_egress_handler(struct sk_buff *skb, 128 struct rmnet_port *port, u8 mux_id, 129 struct net_device *orig_dev) 130 { 131 int required_headroom, additional_header_len; 132 struct rmnet_map_header *map_header; 133 134 additional_header_len = 0; 135 required_headroom = sizeof(struct rmnet_map_header); 136 137 if (skb_headroom(skb) < required_headroom) { 138 if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL)) 139 goto fail; 140 } 141 142 map_header = rmnet_map_add_map_header(skb, additional_header_len, 0); 143 if (!map_header) 144 goto fail; 145 146 if (mux_id == 0xff) 147 map_header->mux_id = 0; 148 else 149 map_header->mux_id = mux_id; 150 151 skb->protocol = htons(ETH_P_MAP); 152 153 return 0; 154 155 fail: 156 kfree_skb(skb); 157 return -ENOMEM; 158 } 159 160 static void 161 rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev) 162 { 163 if (bridge_dev) { 164 skb->dev = bridge_dev; 165 dev_queue_xmit(skb); 166 } 167 } 168 169 /* Ingress / Egress Entry Points */ 170 171 /* Processes packet as per ingress data format for receiving device. Logical 172 * endpoint is determined from packet inspection. Packet is then sent to the 173 * egress device listed in the logical endpoint configuration. 174 */ 175 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb) 176 { 177 struct sk_buff *skb = *pskb; 178 struct rmnet_port *port; 179 struct net_device *dev; 180 181 if (!skb) 182 goto done; 183 184 dev = skb->dev; 185 port = rmnet_get_port(dev); 186 187 switch (port->rmnet_mode) { 188 case RMNET_EPMODE_VND: 189 rmnet_map_ingress_handler(skb, port); 190 break; 191 case RMNET_EPMODE_BRIDGE: 192 rmnet_bridge_handler(skb, port->bridge_ep); 193 break; 194 } 195 196 done: 197 return RX_HANDLER_CONSUMED; 198 } 199 200 /* Modifies packet as per logical endpoint configuration and egress data format 201 * for egress device configured in logical endpoint. Packet is then transmitted 202 * on the egress device. 203 */ 204 void rmnet_egress_handler(struct sk_buff *skb) 205 { 206 struct net_device *orig_dev; 207 struct rmnet_port *port; 208 struct rmnet_priv *priv; 209 u8 mux_id; 210 211 orig_dev = skb->dev; 212 priv = netdev_priv(orig_dev); 213 skb->dev = priv->real_dev; 214 mux_id = priv->mux_id; 215 216 port = rmnet_get_port(skb->dev); 217 if (!port) { 218 kfree_skb(skb); 219 return; 220 } 221 222 if (rmnet_map_egress_handler(skb, port, mux_id, orig_dev)) 223 return; 224 225 rmnet_vnd_tx_fixup(skb, orig_dev); 226 227 dev_queue_xmit(skb); 228 } 229