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_deliver_skb(struct sk_buff *skb) 48 { 49 skb_reset_transport_header(skb); 50 skb_reset_network_header(skb); 51 rmnet_vnd_rx_fixup(skb, skb->dev); 52 53 skb->pkt_type = PACKET_HOST; 54 skb_set_mac_header(skb, 0); 55 netif_receive_skb(skb); 56 return RX_HANDLER_CONSUMED; 57 } 58 59 /* MAP handler */ 60 61 static rx_handler_result_t 62 __rmnet_map_ingress_handler(struct sk_buff *skb, 63 struct rmnet_port *port) 64 { 65 struct rmnet_endpoint *ep; 66 u8 mux_id; 67 u16 len; 68 69 if (RMNET_MAP_GET_CD_BIT(skb)) { 70 if (port->ingress_data_format 71 & RMNET_INGRESS_FORMAT_MAP_COMMANDS) 72 return rmnet_map_command(skb, port); 73 74 goto free_skb; 75 } 76 77 mux_id = RMNET_MAP_GET_MUX_ID(skb); 78 len = RMNET_MAP_GET_LENGTH(skb) - RMNET_MAP_GET_PAD(skb); 79 80 if (mux_id >= RMNET_MAX_LOGICAL_EP) 81 goto free_skb; 82 83 ep = rmnet_get_endpoint(port, mux_id); 84 if (!ep) 85 goto free_skb; 86 87 if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEMUXING) 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 return rmnet_deliver_skb(skb); 95 96 free_skb: 97 kfree_skb(skb); 98 return RX_HANDLER_CONSUMED; 99 } 100 101 static rx_handler_result_t 102 rmnet_map_ingress_handler(struct sk_buff *skb, 103 struct rmnet_port *port) 104 { 105 struct sk_buff *skbn; 106 int rc; 107 108 if (port->ingress_data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) { 109 while ((skbn = rmnet_map_deaggregate(skb)) != NULL) 110 __rmnet_map_ingress_handler(skbn, port); 111 112 consume_skb(skb); 113 rc = RX_HANDLER_CONSUMED; 114 } else { 115 rc = __rmnet_map_ingress_handler(skb, port); 116 } 117 118 return rc; 119 } 120 121 static int rmnet_map_egress_handler(struct sk_buff *skb, 122 struct rmnet_port *port, u8 mux_id, 123 struct net_device *orig_dev) 124 { 125 int required_headroom, additional_header_len; 126 struct rmnet_map_header *map_header; 127 128 additional_header_len = 0; 129 required_headroom = sizeof(struct rmnet_map_header); 130 131 if (skb_headroom(skb) < required_headroom) { 132 if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL)) 133 return RMNET_MAP_CONSUMED; 134 } 135 136 map_header = rmnet_map_add_map_header(skb, additional_header_len, 0); 137 if (!map_header) 138 return RMNET_MAP_CONSUMED; 139 140 if (port->egress_data_format & RMNET_EGRESS_FORMAT_MUXING) { 141 if (mux_id == 0xff) 142 map_header->mux_id = 0; 143 else 144 map_header->mux_id = mux_id; 145 } 146 147 skb->protocol = htons(ETH_P_MAP); 148 149 return RMNET_MAP_SUCCESS; 150 } 151 152 static rx_handler_result_t 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 return RX_HANDLER_CONSUMED; 161 } 162 163 /* Ingress / Egress Entry Points */ 164 165 /* Processes packet as per ingress data format for receiving device. Logical 166 * endpoint is determined from packet inspection. Packet is then sent to the 167 * egress device listed in the logical endpoint configuration. 168 */ 169 rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb) 170 { 171 int rc = RX_HANDLER_CONSUMED; 172 struct sk_buff *skb = *pskb; 173 struct rmnet_port *port; 174 struct net_device *dev; 175 176 if (!skb) 177 return RX_HANDLER_CONSUMED; 178 179 dev = skb->dev; 180 port = rmnet_get_port(dev); 181 182 switch (port->rmnet_mode) { 183 case RMNET_EPMODE_VND: 184 if (port->ingress_data_format & RMNET_INGRESS_FORMAT_MAP) 185 rc = rmnet_map_ingress_handler(skb, port); 186 break; 187 case RMNET_EPMODE_BRIDGE: 188 rc = rmnet_bridge_handler(skb, port->bridge_ep); 189 break; 190 } 191 192 return rc; 193 } 194 195 /* Modifies packet as per logical endpoint configuration and egress data format 196 * for egress device configured in logical endpoint. Packet is then transmitted 197 * on the egress device. 198 */ 199 void rmnet_egress_handler(struct sk_buff *skb) 200 { 201 struct net_device *orig_dev; 202 struct rmnet_port *port; 203 struct rmnet_priv *priv; 204 u8 mux_id; 205 206 orig_dev = skb->dev; 207 priv = netdev_priv(orig_dev); 208 skb->dev = priv->real_dev; 209 mux_id = priv->mux_id; 210 211 port = rmnet_get_port(skb->dev); 212 if (!port) { 213 kfree_skb(skb); 214 return; 215 } 216 217 if (port->egress_data_format & RMNET_EGRESS_FORMAT_MAP) { 218 switch (rmnet_map_egress_handler(skb, port, mux_id, orig_dev)) { 219 case RMNET_MAP_CONSUMED: 220 return; 221 222 case RMNET_MAP_SUCCESS: 223 break; 224 225 default: 226 kfree_skb(skb); 227 return; 228 } 229 } 230 231 rmnet_vnd_tx_fixup(skb, orig_dev); 232 233 dev_queue_xmit(skb); 234 } 235