mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
261 lines
5.8 KiB
261 lines
5.8 KiB
// SPDX-License-Identifier: GPL-2.0-only |
|
/* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved. |
|
* |
|
* RMNET Data ingress/egress handler |
|
*/ |
|
|
|
#include <linux/netdevice.h> |
|
#include <linux/netdev_features.h> |
|
#include <linux/if_arp.h> |
|
#include <net/sock.h> |
|
#include "rmnet_private.h" |
|
#include "rmnet_config.h" |
|
#include "rmnet_vnd.h" |
|
#include "rmnet_map.h" |
|
#include "rmnet_handlers.h" |
|
|
|
#define RMNET_IP_VERSION_4 0x40 |
|
#define RMNET_IP_VERSION_6 0x60 |
|
|
|
/* Helper Functions */ |
|
|
|
static 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; |
|
} |
|
} |
|
|
|
/* Generic handler */ |
|
|
|
static void |
|
rmnet_deliver_skb(struct sk_buff *skb) |
|
{ |
|
struct rmnet_priv *priv = netdev_priv(skb->dev); |
|
|
|
skb_reset_transport_header(skb); |
|
skb_reset_network_header(skb); |
|
rmnet_vnd_rx_fixup(skb, skb->dev); |
|
|
|
skb->pkt_type = PACKET_HOST; |
|
skb_set_mac_header(skb, 0); |
|
gro_cells_receive(&priv->gro_cells, skb); |
|
} |
|
|
|
/* MAP handler */ |
|
|
|
static void |
|
__rmnet_map_ingress_handler(struct sk_buff *skb, |
|
struct rmnet_port *port) |
|
{ |
|
struct rmnet_map_header *map_header = (void *)skb->data; |
|
struct rmnet_endpoint *ep; |
|
u16 len, pad; |
|
u8 mux_id; |
|
|
|
if (map_header->flags & MAP_CMD_FLAG) { |
|
/* Packet contains a MAP command (not data) */ |
|
if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS) |
|
return rmnet_map_command(skb, port); |
|
|
|
goto free_skb; |
|
} |
|
|
|
mux_id = map_header->mux_id; |
|
pad = map_header->flags & MAP_PAD_LEN_MASK; |
|
len = ntohs(map_header->pkt_len) - pad; |
|
|
|
if (mux_id >= RMNET_MAX_LOGICAL_EP) |
|
goto free_skb; |
|
|
|
ep = rmnet_get_endpoint(port, mux_id); |
|
if (!ep) |
|
goto free_skb; |
|
|
|
skb->dev = ep->egress_dev; |
|
|
|
if ((port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) && |
|
(map_header->flags & MAP_NEXT_HEADER_FLAG)) { |
|
if (rmnet_map_process_next_hdr_packet(skb, len)) |
|
goto free_skb; |
|
skb_pull(skb, sizeof(*map_header)); |
|
rmnet_set_skb_proto(skb); |
|
} else { |
|
/* Subtract MAP header */ |
|
skb_pull(skb, sizeof(*map_header)); |
|
rmnet_set_skb_proto(skb); |
|
if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4 && |
|
!rmnet_map_checksum_downlink_packet(skb, len + pad)) |
|
skb->ip_summed = CHECKSUM_UNNECESSARY; |
|
} |
|
|
|
skb_trim(skb, len); |
|
rmnet_deliver_skb(skb); |
|
return; |
|
|
|
free_skb: |
|
kfree_skb(skb); |
|
} |
|
|
|
static void |
|
rmnet_map_ingress_handler(struct sk_buff *skb, |
|
struct rmnet_port *port) |
|
{ |
|
struct sk_buff *skbn; |
|
|
|
if (skb->dev->type == ARPHRD_ETHER) { |
|
if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_ATOMIC)) { |
|
kfree_skb(skb); |
|
return; |
|
} |
|
|
|
skb_push(skb, ETH_HLEN); |
|
} |
|
|
|
if (port->data_format & RMNET_FLAGS_INGRESS_DEAGGREGATION) { |
|
while ((skbn = rmnet_map_deaggregate(skb, port)) != NULL) |
|
__rmnet_map_ingress_handler(skbn, port); |
|
|
|
consume_skb(skb); |
|
} else { |
|
__rmnet_map_ingress_handler(skb, port); |
|
} |
|
} |
|
|
|
static int rmnet_map_egress_handler(struct sk_buff *skb, |
|
struct rmnet_port *port, u8 mux_id, |
|
struct net_device *orig_dev) |
|
{ |
|
int required_headroom, additional_header_len, csum_type = 0; |
|
struct rmnet_map_header *map_header; |
|
|
|
additional_header_len = 0; |
|
required_headroom = sizeof(struct rmnet_map_header); |
|
|
|
if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV4) { |
|
additional_header_len = sizeof(struct rmnet_map_ul_csum_header); |
|
csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV4; |
|
} else if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5) { |
|
additional_header_len = sizeof(struct rmnet_map_v5_csum_header); |
|
csum_type = RMNET_FLAGS_EGRESS_MAP_CKSUMV5; |
|
} |
|
|
|
required_headroom += additional_header_len; |
|
|
|
if (skb_cow_head(skb, required_headroom) < 0) |
|
return -ENOMEM; |
|
|
|
if (csum_type) |
|
rmnet_map_checksum_uplink_packet(skb, port, orig_dev, |
|
csum_type); |
|
|
|
map_header = rmnet_map_add_map_header(skb, additional_header_len, |
|
port, 0); |
|
if (!map_header) |
|
return -ENOMEM; |
|
|
|
map_header->mux_id = mux_id; |
|
|
|
skb->protocol = htons(ETH_P_MAP); |
|
|
|
return 0; |
|
} |
|
|
|
static void |
|
rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev) |
|
{ |
|
if (skb_mac_header_was_set(skb)) |
|
skb_push(skb, skb->mac_len); |
|
|
|
if (bridge_dev) { |
|
skb->dev = bridge_dev; |
|
dev_queue_xmit(skb); |
|
} |
|
} |
|
|
|
/* Ingress / Egress Entry Points */ |
|
|
|
/* 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. |
|
*/ |
|
rx_handler_result_t rmnet_rx_handler(struct sk_buff **pskb) |
|
{ |
|
struct sk_buff *skb = *pskb; |
|
struct rmnet_port *port; |
|
struct net_device *dev; |
|
|
|
if (!skb) |
|
goto done; |
|
|
|
if (skb_linearize(skb)) { |
|
kfree_skb(skb); |
|
goto done; |
|
} |
|
|
|
if (skb->pkt_type == PACKET_LOOPBACK) |
|
return RX_HANDLER_PASS; |
|
|
|
dev = skb->dev; |
|
port = rmnet_get_port_rcu(dev); |
|
if (unlikely(!port)) { |
|
atomic_long_inc(&skb->dev->rx_nohandler); |
|
kfree_skb(skb); |
|
goto done; |
|
} |
|
|
|
switch (port->rmnet_mode) { |
|
case RMNET_EPMODE_VND: |
|
rmnet_map_ingress_handler(skb, port); |
|
break; |
|
case RMNET_EPMODE_BRIDGE: |
|
rmnet_bridge_handler(skb, port->bridge_ep); |
|
break; |
|
} |
|
|
|
done: |
|
return RX_HANDLER_CONSUMED; |
|
} |
|
|
|
/* 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 net_device *orig_dev; |
|
struct rmnet_port *port; |
|
struct rmnet_priv *priv; |
|
u8 mux_id; |
|
|
|
sk_pacing_shift_update(skb->sk, 8); |
|
|
|
orig_dev = skb->dev; |
|
priv = netdev_priv(orig_dev); |
|
skb->dev = priv->real_dev; |
|
mux_id = priv->mux_id; |
|
|
|
port = rmnet_get_port_rcu(skb->dev); |
|
if (!port) |
|
goto drop; |
|
|
|
if (rmnet_map_egress_handler(skb, port, mux_id, orig_dev)) |
|
goto drop; |
|
|
|
rmnet_vnd_tx_fixup(skb, orig_dev); |
|
|
|
dev_queue_xmit(skb); |
|
return; |
|
|
|
drop: |
|
this_cpu_inc(priv->pcpu_stats->stats.tx_drops); |
|
kfree_skb(skb); |
|
}
|
|
|