rmnet_handlers.c 5.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
/* 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 <linux/netdevice.h>
#include <linux/netdev_features.h>
18
#include <linux/if_arp.h>
19
#include <net/sock.h>
20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
#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 */

48
static void
49
rmnet_deliver_skb(struct sk_buff *skb)
50
{
51 52
	struct rmnet_priv *priv = netdev_priv(skb->dev);

53 54 55
	skb_reset_transport_header(skb);
	skb_reset_network_header(skb);
	rmnet_vnd_rx_fixup(skb, skb->dev);
56

57 58
	skb->pkt_type = PACKET_HOST;
	skb_set_mac_header(skb, 0);
59
	gro_cells_receive(&priv->gro_cells, skb);
60 61 62 63
}

/* MAP handler */

64
static void
65
__rmnet_map_ingress_handler(struct sk_buff *skb,
66
			    struct rmnet_port *port)
67 68
{
	struct rmnet_endpoint *ep;
69
	u16 len, pad;
70 71 72
	u8 mux_id;

	if (RMNET_MAP_GET_CD_BIT(skb)) {
73
		if (port->data_format & 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 81
	pad = RMNET_MAP_GET_PAD(skb);
	len = RMNET_MAP_GET_LENGTH(skb) - pad;
82

83 84
	if (mux_id >= RMNET_MAX_LOGICAL_EP)
		goto free_skb;
85

86 87 88
	ep = rmnet_get_endpoint(port, mux_id);
	if (!ep)
		goto free_skb;
89

90
	skb->dev = ep->egress_dev;
91 92 93 94

	/* Subtract MAP header */
	skb_pull(skb, sizeof(struct rmnet_map_header));
	rmnet_set_skb_proto(skb);
95 96 97 98 99 100 101

	if (port->data_format & RMNET_INGRESS_FORMAT_MAP_CKSUMV4) {
		if (!rmnet_map_checksum_downlink_packet(skb, len + pad))
			skb->ip_summed = CHECKSUM_UNNECESSARY;
	}

	skb_trim(skb, len);
102 103
	rmnet_deliver_skb(skb);
	return;
104 105 106

free_skb:
	kfree_skb(skb);
107 108
}

109
static void
110
rmnet_map_ingress_handler(struct sk_buff *skb,
111
			  struct rmnet_port *port)
112 113 114
{
	struct sk_buff *skbn;

115 116 117 118 119 120 121 122 123
	if (skb->dev->type == ARPHRD_ETHER) {
		if (pskb_expand_head(skb, ETH_HLEN, 0, GFP_KERNEL)) {
			kfree_skb(skb);
			return;
		}

		skb_push(skb, ETH_HLEN);
	}

124
	if (port->data_format & RMNET_INGRESS_FORMAT_DEAGGREGATION) {
125
		while ((skbn = rmnet_map_deaggregate(skb, port)) != NULL)
126
			__rmnet_map_ingress_handler(skbn, port);
127 128 129

		consume_skb(skb);
	} else {
130
		__rmnet_map_ingress_handler(skb, port);
131 132 133 134
	}
}

static int rmnet_map_egress_handler(struct sk_buff *skb,
135
				    struct rmnet_port *port, u8 mux_id,
136 137 138 139 140 141 142 143 144 145
				    struct net_device *orig_dev)
{
	int required_headroom, additional_header_len;
	struct rmnet_map_header *map_header;

	additional_header_len = 0;
	required_headroom = sizeof(struct rmnet_map_header);

	if (skb_headroom(skb) < required_headroom) {
		if (pskb_expand_head(skb, required_headroom, 0, GFP_KERNEL))
146
			goto fail;
147 148 149 150
	}

	map_header = rmnet_map_add_map_header(skb, additional_header_len, 0);
	if (!map_header)
151
		goto fail;
152

153
	map_header->mux_id = mux_id;
154 155 156

	skb->protocol = htons(ETH_P_MAP);

157
	return 0;
158 159 160

fail:
	kfree_skb(skb);
161
	return -ENOMEM;
162 163
}

164
static void
165 166 167 168 169 170 171 172
rmnet_bridge_handler(struct sk_buff *skb, struct net_device *bridge_dev)
{
	if (bridge_dev) {
		skb->dev = bridge_dev;
		dev_queue_xmit(skb);
	}
}

173 174 175 176 177 178 179 180 181
/* 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;
182
	struct rmnet_port *port;
183 184 185
	struct net_device *dev;

	if (!skb)
186
		goto done;
187 188

	dev = skb->dev;
189
	port = rmnet_get_port(dev);
190

191 192
	switch (port->rmnet_mode) {
	case RMNET_EPMODE_VND:
193
		rmnet_map_ingress_handler(skb, port);
194 195
		break;
	case RMNET_EPMODE_BRIDGE:
196
		rmnet_bridge_handler(skb, port->bridge_ep);
197 198
		break;
	}
199

200 201
done:
	return RX_HANDLER_CONSUMED;
202 203 204 205 206 207
}

/* 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.
 */
208
void rmnet_egress_handler(struct sk_buff *skb)
209 210
{
	struct net_device *orig_dev;
211
	struct rmnet_port *port;
212 213
	struct rmnet_priv *priv;
	u8 mux_id;
214

215 216
	sk_pacing_shift_update(skb->sk, 8);

217
	orig_dev = skb->dev;
218 219 220
	priv = netdev_priv(orig_dev);
	skb->dev = priv->real_dev;
	mux_id = priv->mux_id;
221

222 223
	port = rmnet_get_port(skb->dev);
	if (!port) {
224 225 226 227
		kfree_skb(skb);
		return;
	}

228 229
	if (rmnet_map_egress_handler(skb, port, mux_id, orig_dev))
		return;
230

231
	rmnet_vnd_tx_fixup(skb, orig_dev);
232 233 234

	dev_queue_xmit(skb);
}