blob: cecf72be5102952eec37a43b88545671ea5f6d32 [file] [log] [blame]
// SPDX-License-Identifier: GPL-2.0-only
/* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved.
*
* RMNET Data MAP protocol
*/
#include <linux/netdevice.h>
#include <linux/ip.h>
#include <linux/ipv6.h>
#include <net/ip6_checksum.h>
#include <linux/bitfield.h>
#include "rmnet_config.h"
#include "rmnet_map.h"
#include "rmnet_private.h"
#define RMNET_MAP_DEAGGR_SPACING 64
#define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
static __sum16 *rmnet_map_get_csum_field(unsigned char protocol,
const void *txporthdr)
{
__sum16 *check = NULL;
switch (protocol) {
case IPPROTO_TCP:
check = &(((struct tcphdr *)txporthdr)->check);
break;
case IPPROTO_UDP:
check = &(((struct udphdr *)txporthdr)->check);
break;
default:
check = NULL;
break;
}
return check;
}
static int
rmnet_map_ipv4_dl_csum_trailer(struct sk_buff *skb,
struct rmnet_map_dl_csum_trailer *csum_trailer,
struct rmnet_priv *priv)
{
__sum16 *csum_field, csum_temp, pseudo_csum, hdr_csum, ip_payload_csum;
u16 csum_value, csum_value_final;
struct iphdr *ip4h;
void *txporthdr;
__be16 addend;
ip4h = (struct iphdr *)(skb->data);
if ((ntohs(ip4h->frag_off) & IP_MF) ||
((ntohs(ip4h->frag_off) & IP_OFFSET) > 0)) {
priv->stats.csum_fragmented_pkt++;
return -EOPNOTSUPP;
}
txporthdr = skb->data + ip4h->ihl * 4;
csum_field = rmnet_map_get_csum_field(ip4h->protocol, txporthdr);
if (!csum_field) {
priv->stats.csum_err_invalid_transport++;
return -EPROTONOSUPPORT;
}
/* RFC 768 - Skip IPv4 UDP packets where sender checksum field is 0 */
if (*csum_field == 0 && ip4h->protocol == IPPROTO_UDP) {
priv->stats.csum_skipped++;
return 0;
}
csum_value = ~ntohs(csum_trailer->csum_value);
hdr_csum = ~ip_fast_csum(ip4h, (int)ip4h->ihl);
ip_payload_csum = csum16_sub((__force __sum16)csum_value,
(__force __be16)hdr_csum);
pseudo_csum = ~csum_tcpudp_magic(ip4h->saddr, ip4h->daddr,
ntohs(ip4h->tot_len) - ip4h->ihl * 4,
ip4h->protocol, 0);
addend = (__force __be16)ntohs((__force __be16)pseudo_csum);
pseudo_csum = csum16_add(ip_payload_csum, addend);
addend = (__force __be16)ntohs((__force __be16)*csum_field);
csum_temp = ~csum16_sub(pseudo_csum, addend);
csum_value_final = (__force u16)csum_temp;
if (unlikely(csum_value_final == 0)) {
switch (ip4h->protocol) {
case IPPROTO_UDP:
/* RFC 768 - DL4 1's complement rule for UDP csum 0 */
csum_value_final = ~csum_value_final;
break;
case IPPROTO_TCP:
/* DL4 Non-RFC compliant TCP checksum found */
if (*csum_field == (__force __sum16)0xFFFF)
csum_value_final = ~csum_value_final;
break;
}
}
if (csum_value_final == ntohs((__force __be16)*csum_field)) {
priv->stats.csum_ok++;
return 0;
} else {
priv->stats.csum_validation_failed++;
return -EINVAL;
}
}
#if IS_ENABLED(CONFIG_IPV6)
static int
rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb,
struct rmnet_map_dl_csum_trailer *csum_trailer,
struct rmnet_priv *priv)
{
__sum16 *csum_field, ip6_payload_csum, pseudo_csum, csum_temp;
u16 csum_value, csum_value_final;
__be16 ip6_hdr_csum, addend;
struct ipv6hdr *ip6h;
void *txporthdr;
u32 length;
ip6h = (struct ipv6hdr *)(skb->data);
txporthdr = skb->data + sizeof(struct ipv6hdr);
csum_field = rmnet_map_get_csum_field(ip6h->nexthdr, txporthdr);
if (!csum_field) {
priv->stats.csum_err_invalid_transport++;
return -EPROTONOSUPPORT;
}
csum_value = ~ntohs(csum_trailer->csum_value);
ip6_hdr_csum = (__force __be16)
~ntohs((__force __be16)ip_compute_csum(ip6h,
(int)(txporthdr - (void *)(skb->data))));
ip6_payload_csum = csum16_sub((__force __sum16)csum_value,
ip6_hdr_csum);
length = (ip6h->nexthdr == IPPROTO_UDP) ?
ntohs(((struct udphdr *)txporthdr)->len) :
ntohs(ip6h->payload_len);
pseudo_csum = ~(csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
length, ip6h->nexthdr, 0));
addend = (__force __be16)ntohs((__force __be16)pseudo_csum);
pseudo_csum = csum16_add(ip6_payload_csum, addend);
addend = (__force __be16)ntohs((__force __be16)*csum_field);
csum_temp = ~csum16_sub(pseudo_csum, addend);
csum_value_final = (__force u16)csum_temp;
if (unlikely(csum_value_final == 0)) {
switch (ip6h->nexthdr) {
case IPPROTO_UDP:
/* RFC 2460 section 8.1
* DL6 One's complement rule for UDP checksum 0
*/
csum_value_final = ~csum_value_final;
break;
case IPPROTO_TCP:
/* DL6 Non-RFC compliant TCP checksum found */
if (*csum_field == (__force __sum16)0xFFFF)
csum_value_final = ~csum_value_final;
break;
}
}
if (csum_value_final == ntohs((__force __be16)*csum_field)) {
priv->stats.csum_ok++;
return 0;
} else {
priv->stats.csum_validation_failed++;
return -EINVAL;
}
}
#endif
static void rmnet_map_complement_ipv4_txporthdr_csum_field(void *iphdr)
{
struct iphdr *ip4h = (struct iphdr *)iphdr;
void *txphdr;
u16 *csum;
txphdr = iphdr + ip4h->ihl * 4;
if (ip4h->protocol == IPPROTO_TCP || ip4h->protocol == IPPROTO_UDP) {
csum = (u16 *)rmnet_map_get_csum_field(ip4h->protocol, txphdr);
*csum = ~(*csum);
}
}
static void
rmnet_map_ipv4_ul_csum_header(void *iphdr,
struct rmnet_map_ul_csum_header *ul_header,
struct sk_buff *skb)
{
struct iphdr *ip4h = iphdr;
u16 val;
val = MAP_CSUM_UL_ENABLED_FLAG;
if (ip4h->protocol == IPPROTO_UDP)
val |= MAP_CSUM_UL_UDP_FLAG;
val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
ul_header->csum_start_offset = htons(skb_network_header_len(skb));
ul_header->csum_info = htons(val);
skb->ip_summed = CHECKSUM_NONE;
rmnet_map_complement_ipv4_txporthdr_csum_field(iphdr);
}
#if IS_ENABLED(CONFIG_IPV6)
static void rmnet_map_complement_ipv6_txporthdr_csum_field(void *ip6hdr)
{
struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr;
void *txphdr;
u16 *csum;
txphdr = ip6hdr + sizeof(struct ipv6hdr);
if (ip6h->nexthdr == IPPROTO_TCP || ip6h->nexthdr == IPPROTO_UDP) {
csum = (u16 *)rmnet_map_get_csum_field(ip6h->nexthdr, txphdr);
*csum = ~(*csum);
}
}
static void
rmnet_map_ipv6_ul_csum_header(void *ip6hdr,
struct rmnet_map_ul_csum_header *ul_header,
struct sk_buff *skb)
{
struct ipv6hdr *ip6h = ip6hdr;
u16 val;
val = MAP_CSUM_UL_ENABLED_FLAG;
if (ip6h->nexthdr == IPPROTO_UDP)
val |= MAP_CSUM_UL_UDP_FLAG;
val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
ul_header->csum_start_offset = htons(skb_network_header_len(skb));
ul_header->csum_info = htons(val);
skb->ip_summed = CHECKSUM_NONE;
rmnet_map_complement_ipv6_txporthdr_csum_field(ip6hdr);
}
#endif
static void rmnet_map_v5_checksum_uplink_packet(struct sk_buff *skb,
struct rmnet_port *port,
struct net_device *orig_dev)
{
struct rmnet_priv *priv = netdev_priv(orig_dev);
struct rmnet_map_v5_csum_header *ul_header;
ul_header = skb_push(skb, sizeof(*ul_header));
memset(ul_header, 0, sizeof(*ul_header));
ul_header->header_info = u8_encode_bits(RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD,
MAPV5_HDRINFO_HDR_TYPE_FMASK);
if (skb->ip_summed == CHECKSUM_PARTIAL) {
void *iph = ip_hdr(skb);
__sum16 *check;
void *trans;
u8 proto;
if (skb->protocol == htons(ETH_P_IP)) {
u16 ip_len = ((struct iphdr *)iph)->ihl * 4;
proto = ((struct iphdr *)iph)->protocol;
trans = iph + ip_len;
} else if (IS_ENABLED(CONFIG_IPV6) &&
skb->protocol == htons(ETH_P_IPV6)) {
u16 ip_len = sizeof(struct ipv6hdr);
proto = ((struct ipv6hdr *)iph)->nexthdr;
trans = iph + ip_len;
} else {
priv->stats.csum_err_invalid_ip_version++;
goto sw_csum;
}
check = rmnet_map_get_csum_field(proto, trans);
if (check) {
skb->ip_summed = CHECKSUM_NONE;
/* Ask for checksum offloading */
ul_header->csum_info |= MAPV5_CSUMINFO_VALID_FLAG;
priv->stats.csum_hw++;
return;
}
}
sw_csum:
priv->stats.csum_sw++;
}
/* Adds MAP header to front of skb->data
* Padding is calculated and set appropriately in MAP header. Mux ID is
* initialized to 0.
*/
struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
int hdrlen,
struct rmnet_port *port,
int pad)
{
struct rmnet_map_header *map_header;
u32 padding, map_datalen;
u8 *padbytes;
map_datalen = skb->len - hdrlen;
map_header = (struct rmnet_map_header *)
skb_push(skb, sizeof(struct rmnet_map_header));
memset(map_header, 0, sizeof(struct rmnet_map_header));
/* Set next_hdr bit for csum offload packets */
if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5)
map_header->flags |= MAP_NEXT_HEADER_FLAG;
if (pad == RMNET_MAP_NO_PAD_BYTES) {
map_header->pkt_len = htons(map_datalen);
return map_header;
}
BUILD_BUG_ON(MAP_PAD_LEN_MASK < 3);
padding = ALIGN(map_datalen, 4) - map_datalen;
if (padding == 0)
goto done;
if (skb_tailroom(skb) < padding)
return NULL;
padbytes = (u8 *)skb_put(skb, padding);
memset(padbytes, 0, padding);
done:
map_header->pkt_len = htons(map_datalen + padding);
/* This is a data packet, so the CMD bit is 0 */
map_header->flags = padding & MAP_PAD_LEN_MASK;
return map_header;
}
/* Deaggregates a single packet
* A whole new buffer is allocated for each portion of an aggregated frame.
* Caller should keep calling deaggregate() on the source skb until 0 is
* returned, indicating that there are no more packets to deaggregate. Caller
* is responsible for freeing the original skb.
*/
struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb,
struct rmnet_port *port)
{
struct rmnet_map_v5_csum_header *next_hdr = NULL;
struct rmnet_map_header *maph;
void *data = skb->data;
struct sk_buff *skbn;
u8 nexthdr_type;
u32 packet_len;
if (skb->len == 0)
return NULL;
maph = (struct rmnet_map_header *)skb->data;
packet_len = ntohs(maph->pkt_len) + sizeof(*maph);
if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) {
packet_len += sizeof(struct rmnet_map_dl_csum_trailer);
} else if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) {
if (!(maph->flags & MAP_CMD_FLAG)) {
packet_len += sizeof(*next_hdr);
if (maph->flags & MAP_NEXT_HEADER_FLAG)
next_hdr = data + sizeof(*maph);
else
/* Mapv5 data pkt without csum hdr is invalid */
return NULL;
}
}
if (((int)skb->len - (int)packet_len) < 0)
return NULL;
/* Some hardware can send us empty frames. Catch them */
if (!maph->pkt_len)
return NULL;
if (next_hdr) {
nexthdr_type = u8_get_bits(next_hdr->header_info,
MAPV5_HDRINFO_HDR_TYPE_FMASK);
if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
return NULL;
}
skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
if (!skbn)
return NULL;
skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
skb_put(skbn, packet_len);
memcpy(skbn->data, skb->data, packet_len);
skb_pull(skb, packet_len);
return skbn;
}
/* Validates packet checksums. Function takes a pointer to
* the beginning of a buffer which contains the IP payload +
* padding + checksum trailer.
* Only IPv4 and IPv6 are supported along with TCP & UDP.
* Fragmented or tunneled packets are not supported.
*/
int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len)
{
struct rmnet_priv *priv = netdev_priv(skb->dev);
struct rmnet_map_dl_csum_trailer *csum_trailer;
if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
priv->stats.csum_sw++;
return -EOPNOTSUPP;
}
csum_trailer = (struct rmnet_map_dl_csum_trailer *)(skb->data + len);
if (!(csum_trailer->flags & MAP_CSUM_DL_VALID_FLAG)) {
priv->stats.csum_valid_unset++;
return -EINVAL;
}
if (skb->protocol == htons(ETH_P_IP)) {
return rmnet_map_ipv4_dl_csum_trailer(skb, csum_trailer, priv);
} else if (skb->protocol == htons(ETH_P_IPV6)) {
#if IS_ENABLED(CONFIG_IPV6)
return rmnet_map_ipv6_dl_csum_trailer(skb, csum_trailer, priv);
#else
priv->stats.csum_err_invalid_ip_version++;
return -EPROTONOSUPPORT;
#endif
} else {
priv->stats.csum_err_invalid_ip_version++;
return -EPROTONOSUPPORT;
}
return 0;
}
static void rmnet_map_v4_checksum_uplink_packet(struct sk_buff *skb,
struct net_device *orig_dev)
{
struct rmnet_priv *priv = netdev_priv(orig_dev);
struct rmnet_map_ul_csum_header *ul_header;
void *iphdr;
ul_header = (struct rmnet_map_ul_csum_header *)
skb_push(skb, sizeof(struct rmnet_map_ul_csum_header));
if (unlikely(!(orig_dev->features &
(NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM))))
goto sw_csum;
if (skb->ip_summed == CHECKSUM_PARTIAL) {
iphdr = (char *)ul_header +
sizeof(struct rmnet_map_ul_csum_header);
if (skb->protocol == htons(ETH_P_IP)) {
rmnet_map_ipv4_ul_csum_header(iphdr, ul_header, skb);
priv->stats.csum_hw++;
return;
} else if (skb->protocol == htons(ETH_P_IPV6)) {
#if IS_ENABLED(CONFIG_IPV6)
rmnet_map_ipv6_ul_csum_header(iphdr, ul_header, skb);
priv->stats.csum_hw++;
return;
#else
priv->stats.csum_err_invalid_ip_version++;
goto sw_csum;
#endif
} else {
priv->stats.csum_err_invalid_ip_version++;
}
}
sw_csum:
memset(ul_header, 0, sizeof(*ul_header));
priv->stats.csum_sw++;
}
/* Generates UL checksum meta info header for IPv4 and IPv6 over TCP and UDP
* packets that are supported for UL checksum offload.
*/
void rmnet_map_checksum_uplink_packet(struct sk_buff *skb,
struct rmnet_port *port,
struct net_device *orig_dev,
int csum_type)
{
switch (csum_type) {
case RMNET_FLAGS_EGRESS_MAP_CKSUMV4:
rmnet_map_v4_checksum_uplink_packet(skb, orig_dev);
break;
case RMNET_FLAGS_EGRESS_MAP_CKSUMV5:
rmnet_map_v5_checksum_uplink_packet(skb, port, orig_dev);
break;
default:
break;
}
}
/* Process a MAPv5 packet header */
int rmnet_map_process_next_hdr_packet(struct sk_buff *skb,
u16 len)
{
struct rmnet_priv *priv = netdev_priv(skb->dev);
struct rmnet_map_v5_csum_header *next_hdr;
u8 nexthdr_type;
next_hdr = (struct rmnet_map_v5_csum_header *)(skb->data +
sizeof(struct rmnet_map_header));
nexthdr_type = u8_get_bits(next_hdr->header_info,
MAPV5_HDRINFO_HDR_TYPE_FMASK);
if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
return -EINVAL;
if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
priv->stats.csum_sw++;
} else if (next_hdr->csum_info & MAPV5_CSUMINFO_VALID_FLAG) {
priv->stats.csum_ok++;
skb->ip_summed = CHECKSUM_UNNECESSARY;
} else {
priv->stats.csum_valid_unset++;
}
/* Pull csum v5 header */
skb_pull(skb, sizeof(*next_hdr));
return 0;
}