/* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 and * only version 2 as published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * RMNET Data ingress/egress handler * */ #include #include #include #include #include #include #include #include "rmnet_private.h" #include "rmnet_config.h" #include "rmnet_vnd.h" #include "rmnet_map.h" #include "rmnet_stats.h" #include "rmnet_handlers.h" RMNET_LOG_MODULE(RMNET_LOGMASK_HANDLER); #ifdef CONFIG_RMNET_DEBUG unsigned int dump_pkt_rx; module_param(dump_pkt_rx, uint, 0644); MODULE_PARM_DESC(dump_pkt_rx, "Dump packets entering ingress handler"); unsigned int dump_pkt_tx; module_param(dump_pkt_tx, uint, 0644); MODULE_PARM_DESC(dump_pkt_tx, "Dump packets exiting egress handler"); #endif /* CONFIG_RMNET_DEBUG */ #define RMNET_IP_VERSION_4 0x40 #define RMNET_IP_VERSION_6 0x60 /* Helper Functions */ /* __rmnet_set_skb_proto() - Set skb->protocol field * @skb: packet being modified * * Peek at the first byte of the packet and set the protocol. There is not * good way to determine if a packet has a MAP header. As of writing this, * the reserved bit in the MAP frame will prevent it from overlapping with * IPv4/IPv6 frames. This could change in the future! */ static inline void __rmnet_set_skb_proto(struct sk_buff *skb) { switch (skb->data[0] & 0xF0) { case RMNET_IP_VERSION_4: skb->protocol = htons(ETH_P_IP); break; case RMNET_IP_VERSION_6: skb->protocol = htons(ETH_P_IPV6); break; default: skb->protocol = htons(ETH_P_MAP); break; } } #ifdef CONFIG_RMNET_DEBUG /* rmnet_print_packet() - Print packet / diagnostics * @skb: Packet to print * @printlen: Number of bytes to print * @dev: Name of interface * @dir: Character representing direction (e.g.. 'r' for receive) * * This function prints out raw bytes in an SKB. Use of this will have major * performance impacts and may even trigger watchdog resets if too much is being * printed. Hence, this should always be compiled out unless absolutely needed. */ void rmnet_print_packet(const struct sk_buff *skb, const char *dev, char dir) { char buffer[200]; unsigned int len, printlen; int i, buffloc = 0; switch (dir) { case 'r': printlen = dump_pkt_rx; break; case 't': printlen = dump_pkt_tx; break; default: printlen = 0; break; } if (!printlen) return; pr_err("[%s][%c] - PKT skb->len=%d skb->head=%pK skb->data=%pK\n", dev, dir, skb->len, (void *)skb->head, (void *)skb->data); pr_err("[%s][%c] - PKT skb->tail=%pK skb->end=%pK\n", dev, dir, skb_tail_pointer(skb), skb_end_pointer(skb)); if (skb->len > 0) len = skb->len; else len = ((unsigned int)(uintptr_t)skb->end) - ((unsigned int)(uintptr_t)skb->data); pr_err("[%s][%c] - PKT len: %d, printing first %d bytes\n", dev, dir, len, printlen); memset(buffer, 0, sizeof(buffer)); for (i = 0; (i < printlen) && (i < len); i++) { if ((i % 16) == 0) { pr_err("[%s][%c] - PKT%s\n", dev, dir, buffer); memset(buffer, 0, sizeof(buffer)); buffloc = 0; buffloc += snprintf(&buffer[buffloc], sizeof(buffer) - buffloc, "%04X:", i); } buffloc += snprintf(&buffer[buffloc], sizeof(buffer) - buffloc, " %02x", skb->data[i]); } pr_err("[%s][%c] - PKT%s\n", dev, dir, buffer); } #else void rmnet_print_packet(const struct sk_buff *skb, const char *dev, char dir) { } #endif /* CONFIG_RMNET_DEBUG */ /* Generic handler */ /* rmnet_bridge_handler() - Bridge related functionality * * Return: * - RX_HANDLER_CONSUMED in all cases */ static rx_handler_result_t rmnet_bridge_handler (struct sk_buff *skb, struct rmnet_logical_ep_conf_s *ep) { if (!ep->egress_dev) { LOGD("Missing egress device for packet arriving on %s", skb->dev->name); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_BRDG_NO_EGRESS); } else { rmnet_egress_handler(skb, ep); } return RX_HANDLER_CONSUMED; } #ifdef NET_SKBUFF_DATA_USES_OFFSET static void rmnet_reset_mac_header(struct sk_buff *skb) { skb->mac_header = 0; skb->mac_len = 0; } #else static void rmnet_reset_mac_header(struct sk_buff *skb) { skb->mac_header = skb->network_header; skb->mac_len = 0; } #endif /*NET_SKBUFF_DATA_USES_OFFSET*/ /* __rmnet_deliver_skb() - Deliver skb * * Determines where to deliver skb. Options are: consume by network stack, * pass to bridge handler, or pass to virtual network device * * Return: * - RX_HANDLER_CONSUMED if packet forwarded or dropped * - RX_HANDLER_PASS if packet is to be consumed by network stack as-is */ static rx_handler_result_t __rmnet_deliver_skb (struct sk_buff *skb, struct rmnet_logical_ep_conf_s *ep) { switch (ep->rmnet_mode) { case RMNET_EPMODE_NONE: return RX_HANDLER_PASS; case RMNET_EPMODE_BRIDGE: return rmnet_bridge_handler(skb, ep); case RMNET_EPMODE_VND: skb_reset_transport_header(skb); skb_reset_network_header(skb); switch (rmnet_vnd_rx_fixup(skb, skb->dev)) { case RX_HANDLER_CONSUMED: return RX_HANDLER_CONSUMED; case RX_HANDLER_PASS: skb->pkt_type = PACKET_HOST; rmnet_reset_mac_header(skb); netif_receive_skb(skb); return RX_HANDLER_CONSUMED; } return RX_HANDLER_PASS; default: LOGD("Unknown ep mode %d", ep->rmnet_mode); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_DELIVER_NO_EP); return RX_HANDLER_CONSUMED; } } /* rmnet_ingress_deliver_packet() - Ingress handler for raw IP and bridged * MAP packets. * @skb: Packet needing a destination. * @config: Physical end point configuration that the packet arrived on. * * Return: * - RX_HANDLER_CONSUMED if packet forwarded/dropped * - RX_HANDLER_PASS if packet should be passed up the stack by caller */ static rx_handler_result_t rmnet_ingress_deliver_packet (struct sk_buff *skb, struct rmnet_phys_ep_conf_s *config) { if (!config) { LOGD("%s", "NULL physical EP provided"); kfree_skb(skb); return RX_HANDLER_CONSUMED; } if (!(config->local_ep.refcount)) { LOGD("Packet on %s has no local endpoint configuration", skb->dev->name); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_IPINGRESS_NO_EP); return RX_HANDLER_CONSUMED; } skb->dev = config->local_ep.egress_dev; return __rmnet_deliver_skb(skb, &config->local_ep); } /* MAP handler */ /* _rmnet_map_ingress_handler() - Actual MAP ingress handler * @skb: Packet being received * @config: Physical endpoint configuration for the ingress device * * Most MAP ingress functions are processed here. Packets are processed * individually; aggregated packets should use rmnet_map_ingress_handler() * * Return: * - RX_HANDLER_CONSUMED if packet is dropped * - result of __rmnet_deliver_skb() for all other cases */ static rx_handler_result_t _rmnet_map_ingress_handler (struct sk_buff *skb, struct rmnet_phys_ep_conf_s *config) { struct rmnet_logical_ep_conf_s *ep; u8 mux_id; u16 len; if (RMNET_MAP_GET_CD_BIT(skb)) { if (config->ingress_data_format & RMNET_INGRESS_FORMAT_MAP_COMMANDS) return rmnet_map_command(skb, config); LOGM("MAP command packet on %s; %s", skb->dev->name, "Not configured for MAP commands"); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_INGRESS_NOT_EXPECT_MAPC); return RX_HANDLER_CONSUMED; } mux_id = RMNET_MAP_GET_MUX_ID(skb); len = RMNET_MAP_GET_LENGTH(skb) - RMNET_MAP_GET_PAD(skb); if (mux_id >= RMNET_MAX_LOGICAL_EP) { LOGD("Got packet on %s with bad mux id %d", skb->dev->name, mux_id); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_MAPINGRESS_BAD_MUX); return RX_HANDLER_CONSUMED; } ep = &config->muxed_ep[mux_id]; if (!ep->refcount) { LOGD("Packet on %s:%d; has no logical endpoint config", skb->dev->name, mux_id); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_MAPINGRESS_MUX_NO_EP); return RX_HANDLER_CONSUMED; } if (config->ingress_data_format & RMNET_INGRESS_FORMAT_DEMUXING) skb->dev = ep->egress_dev; /* Subtract MAP header */ skb_pull(skb, sizeof(struct rmnet_map_header_s)); skb_trim(skb, len); __rmnet_set_skb_proto(skb); return __rmnet_deliver_skb(skb, ep); } /* rmnet_map_ingress_handler() - MAP ingress handler * @skb: Packet being received * @config: Physical endpoint configuration for the ingress device * * Called if and only if MAP is configured in the ingress device's ingress data * format. Deaggregation is done here, actual MAP processing is done in * _rmnet_map_ingress_handler(). * * Return: * - RX_HANDLER_CONSUMED for aggregated packets * - RX_HANDLER_CONSUMED for dropped packets * - result of _rmnet_map_ingress_handler() for all other cases */ static rx_handler_result_t rmnet_map_ingress_handler (struct sk_buff *skb, struct rmnet_phys_ep_conf_s *config) { struct sk_buff *skbn; int rc, co = 0; if (config->ingress_data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) { while ((skbn = rmnet_map_deaggregate(skb, config)) != NULL) { _rmnet_map_ingress_handler(skbn, config); co++; } LOGD("De-aggregated %d packets", co); rmnet_stats_deagg_pkts(co); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_MAPINGRESS_AGGBUF); rc = RX_HANDLER_CONSUMED; } else { rc = _rmnet_map_ingress_handler(skb, config); } return rc; } /* rmnet_map_egress_handler() - MAP egress handler * @skb: Packet being sent * @config: Physical endpoint configuration for the egress device * @ep: logical endpoint configuration of the packet originator * (e.g.. RmNet virtual network device) * @orig_dev: The originator vnd device * * Called if and only if MAP is configured in the egress device's egress data * format. Will expand skb if there is insufficient headroom for MAP protocol. * Note: headroomexpansion will incur a performance penalty. * * Return: * - 0 on success * - 1 on failure */ static int rmnet_map_egress_handler(struct sk_buff *skb, struct rmnet_phys_ep_conf_s *config, struct rmnet_logical_ep_conf_s *ep, struct net_device *orig_dev) { int required_headroom, additional_header_length; struct rmnet_map_header_s *map_header; additional_header_length = 0; required_headroom = sizeof(struct rmnet_map_header_s); LOGD("headroom of %d bytes", required_headroom); if (skb_headroom(skb) < required_headroom) { if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL)) { LOGD("Failed to add headroom of %d bytes", required_headroom); return RMNET_MAP_CONSUMED; } } map_header = rmnet_map_add_map_header (skb, additional_header_length, RMNET_MAP_NO_PAD_BYTES); if (!map_header) { LOGD("%s", "Failed to add MAP header to egress packet"); return RMNET_MAP_CONSUMED; } if (config->egress_data_format & RMNET_EGRESS_FORMAT_MUXING) { if (ep->mux_id == 0xff) map_header->mux_id = 0; else map_header->mux_id = ep->mux_id; } skb->protocol = htons(ETH_P_MAP); return RMNET_MAP_SUCCESS; } /* Ingress / Egress Entry Points */ /* rmnet_ingress_handler() - Ingress handler entry point * @skb: Packet being received * * Processes packet as per ingress data format for receiving device. Logical * endpoint is determined from packet inspection. Packet is then sent to the * egress device listed in the logical endpoint configuration. * * Return: * - RX_HANDLER_PASS if packet is not processed by handler (caller must * deal with the packet) * - RX_HANDLER_CONSUMED if packet is forwarded or processed by MAP */ rx_handler_result_t rmnet_ingress_handler(struct sk_buff *skb) { struct rmnet_phys_ep_conf_s *config; struct net_device *dev; int rc; if (!skb) return RX_HANDLER_CONSUMED; dev = skb->dev; rmnet_print_packet(skb, dev->name, 'r'); config = (struct rmnet_phys_ep_conf_s *) rcu_dereference(skb->dev->rx_handler_data); if (!config) { LOGD("%s is not associated with rmnet", skb->dev->name); kfree_skb(skb); return RX_HANDLER_CONSUMED; } /* Sometimes devices operate in ethernet mode even thouth there is no * ethernet header. This causes the skb->protocol to contain a bogus * value and the skb->data pointer to be off by 14 bytes. Fix it if * configured to do so */ if (config->ingress_data_format & RMNET_INGRESS_FIX_ETHERNET) { skb_push(skb, RMNET_ETHERNET_HEADER_LENGTH); __rmnet_set_skb_proto(skb); } if (config->ingress_data_format & RMNET_INGRESS_FORMAT_MAP) { rc = rmnet_map_ingress_handler(skb, config); } else { switch (ntohs(skb->protocol)) { case ETH_P_MAP: if (config->local_ep.rmnet_mode == RMNET_EPMODE_BRIDGE) { rc = rmnet_ingress_deliver_packet(skb, config); } else { LOGD("MAP packet on %s; MAP not set", dev->name); rmnet_kfree_skb (skb, RMNET_STATS_SKBFREE_INGRESS_NOT_EXPECT_MAPD); rc = RX_HANDLER_CONSUMED; } break; case ETH_P_ARP: case ETH_P_IP: case ETH_P_IPV6: rc = rmnet_ingress_deliver_packet(skb, config); break; default: LOGD("Unknown skb->proto 0x%04X", ntohs(skb->protocol) & 0xFFFF); rc = RX_HANDLER_PASS; } } return rc; } /* rmnet_rx_handler() - Rx handler callback registered with kernel * @pskb: Packet to be processed by rx handler * * Standard kernel-expected footprint for rx handlers. Calls * rmnet_ingress_handler with correctly formatted arguments * * Return: * - Whatever rmnet_ingress_handler() returns */ rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb) { return rmnet_ingress_handler(*pskb); } /* rmnet_egress_handler() - Egress handler entry point * @skb: packet to transmit * @ep: logical endpoint configuration of the packet originator * (e.g.. RmNet virtual network device) * * Modifies packet as per logical endpoint configuration and egress data format * for egress device configured in logical endpoint. Packet is then transmitted * on the egress device. */ void rmnet_egress_handler(struct sk_buff *skb, struct rmnet_logical_ep_conf_s *ep) { struct rmnet_phys_ep_conf_s *config; struct net_device *orig_dev; int rc; orig_dev = skb->dev; skb->dev = ep->egress_dev; config = (struct rmnet_phys_ep_conf_s *) rcu_dereference(skb->dev->rx_handler_data); if (!config) { LOGD("%s is not associated with rmnet", skb->dev->name); kfree_skb(skb); return; } LOGD("Packet going out on %s with egress format 0x%08X", skb->dev->name, config->egress_data_format); if (config->egress_data_format & RMNET_EGRESS_FORMAT_MAP) { switch (rmnet_map_egress_handler(skb, config, ep, orig_dev)) { case RMNET_MAP_CONSUMED: LOGD("%s", "MAP process consumed packet"); return; case RMNET_MAP_SUCCESS: break; default: LOGD("MAP egress failed on packet on %s", skb->dev->name); rmnet_kfree_skb(skb, RMNET_STATS_SKBFREE_EGR_MAPFAIL); return; } } if (ep->rmnet_mode == RMNET_EPMODE_VND) rmnet_vnd_tx_fixup(skb, orig_dev); rmnet_print_packet(skb, skb->dev->name, 't'); rc = dev_queue_xmit(skb); if (rc != 0) { LOGD("Failed to queue packet for transmission on [%s]", skb->dev->name); } rmnet_stats_queue_xmit(rc, RMNET_STATS_QUEUE_XMIT_EGRESS); }