drivers/net/ethernet/qualcomm/rmnet/rmnet_map_data.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Copyright (c) 2013-2018, 2021, The Linux Foundation. All rights reserved.
   3  *
   4  * RMNET Data MAP protocol
   5  */
   6
   7 #include <linux/netdevice.h>
   8 #include <linux/ip.h>
   9 #include <linux/ipv6.h>
  10 #include <net/ip6_checksum.h>
  11 #include <linux/bitfield.h>
  12 #include "rmnet_config.h"
  13 #include "rmnet_map.h"
  14 #include "rmnet_private.h"
  15
  16 #define RMNET_MAP_DEAGGR_SPACING  64
  17 #define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
  18
  19 static __sum16 *rmnet_map_get_csum_field(unsigned char protocol,
  20                                          const void *txporthdr)
  21 {
  22         if (protocol == IPPROTO_TCP)
  23                 return &((struct tcphdr *)txporthdr)->check;
  24
  25         if (protocol == IPPROTO_UDP)
  26                 return &((struct udphdr *)txporthdr)->check;
  27
  28         return NULL;
  29 }
  30
  31 static int
  32 rmnet_map_ipv4_dl_csum_trailer(struct sk_buff *skb,
  33                                struct rmnet_map_dl_csum_trailer *csum_trailer,
  34                                struct rmnet_priv *priv)
  35 {
  36         struct iphdr *ip4h = (struct iphdr *)skb->data;
  37         void *txporthdr = skb->data + ip4h->ihl * 4;
  38         __sum16 *csum_field, pseudo_csum;
  39         __sum16 ip_payload_csum;
  40         __sum16 csum_value_final;
  41
  42         /* Computing the checksum over just the IPv4 header--including its
  43          * checksum field--should yield 0.  If it doesn't, the IP header
  44          * is bad, so return an error and let the IP layer drop it.
  45          */
  46         if (ip_fast_csum(ip4h, ip4h->ihl)) {
  47                 priv->stats.csum_ip4_header_bad++;
  48                 return -EINVAL;
  49         }
  50
  51         /* We don't support checksum offload on IPv4 fragments */
  52         if (ip_is_fragment(ip4h)) {
  53                 priv->stats.csum_fragmented_pkt++;
  54                 return -EOPNOTSUPP;
  55         }
  56
  57         /* Checksum offload is only supported for UDP and TCP protocols */
  58         csum_field = rmnet_map_get_csum_field(ip4h->protocol, txporthdr);
  59         if (!csum_field) {
  60                 priv->stats.csum_err_invalid_transport++;
  61                 return -EPROTONOSUPPORT;
  62         }
  63
  64         /* RFC 768: UDP checksum is optional for IPv4, and is 0 if unused */
  65         if (!*csum_field && ip4h->protocol == IPPROTO_UDP) {
  66                 priv->stats.csum_skipped++;
  67                 return 0;
  68         }
  69
  70         /* The checksum value in the trailer is computed over the entire
  71          * IP packet, including the IP header and payload.  To derive the
  72          * transport checksum from this, we first subract the contribution
  73          * of the IP header from the trailer checksum.  We then add the
  74          * checksum computed over the pseudo header.
  75          *
  76          * We verified above that the IP header contributes zero to the
  77          * trailer checksum.  Therefore the checksum in the trailer is
  78          * just the checksum computed over the IP payload.
  79
  80          * If the IP payload arrives intact, adding the pseudo header
  81          * checksum to the IP payload checksum will yield 0xffff (negative
  82          * zero).  This means the trailer checksum and the pseudo checksum
  83          * are additive inverses of each other.  Put another way, the
  84          * message passes the checksum test if the trailer checksum value
  85          * is the negated pseudo header checksum.
  86          *
  87          * Knowing this, we don't even need to examine the transport
  88          * header checksum value; it is already accounted for in the
  89          * checksum value found in the trailer.
  90          */
  91         ip_payload_csum = (__force __sum16)~csum_trailer->csum_value;
  92
  93         pseudo_csum = ~csum_tcpudp_magic(ip4h->saddr, ip4h->daddr,
  94                                          ntohs(ip4h->tot_len) - ip4h->ihl * 4,
  95                                          ip4h->protocol, 0);
  96         pseudo_csum = csum16_add(ip_payload_csum, (__force __be16)pseudo_csum);
  97
  98         /* The cast is required to ensure only the low 16 bits are examined */
  99         if ((__sum16)~pseudo_csum) {
 100                 priv->stats.csum_validation_failed++;
 101                 return -EINVAL;
 102         }
 103         csum_value_final = ~csum16_sub(pseudo_csum, (__force __be16)*csum_field);
 104
 105         if (unlikely(!csum_value_final)) {
 106                 switch (ip4h->protocol) {
 107                 case IPPROTO_UDP:
 108                         /* RFC 768 - DL4 1's complement rule for UDP csum 0 */
 109                         csum_value_final = ~csum_value_final;
 110                         break;
 111
 112                 case IPPROTO_TCP:
 113                         /* DL4 Non-RFC compliant TCP checksum found */
 114                         if (*csum_field == (__force __sum16)0xFFFF)
 115                                 csum_value_final = ~csum_value_final;
 116                         break;
 117                 }
 118         }
 119
 120         if (csum_value_final == *csum_field) {
 121                 priv->stats.csum_ok++;
 122                 return 0;
 123         } else {
 124                 priv->stats.csum_validation_failed++;
 125                 return -EINVAL;
 126         }
 127 }
 128
 129 #if IS_ENABLED(CONFIG_IPV6)
 130 static int
 131 rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb,
 132                                struct rmnet_map_dl_csum_trailer *csum_trailer,
 133                                struct rmnet_priv *priv)
 134 {
 135         struct ipv6hdr *ip6h = (struct ipv6hdr *)skb->data;
 136         void *txporthdr = skb->data + sizeof(*ip6h);
 137         __sum16 *csum_field, pseudo_csum;
 138         __sum16 ip6_payload_csum;
 139         __be16 ip_header_csum;
 140         __sum16 csum_value_final;
 141         u32 length;
 142
 143         /* Checksum offload is only supported for UDP and TCP protocols;
 144          * the packet cannot include any IPv6 extension headers
 145          */
 146         csum_field = rmnet_map_get_csum_field(ip6h->nexthdr, txporthdr);
 147         if (!csum_field) {
 148                 priv->stats.csum_err_invalid_transport++;
 149                 return -EPROTONOSUPPORT;
 150         }
 151
 152         /* The checksum value in the trailer is computed over the entire
 153          * IP packet, including the IP header and payload.  To derive the
 154          * transport checksum from this, we first subract the contribution
 155          * of the IP header from the trailer checksum.  We then add the
 156          * checksum computed over the pseudo header.
 157          *
 158          * It's sufficient to compare the IP payload checksum with the
 159          * negated pseudo checksum to determine whether the packet
 160          * checksum was good.  (See further explanation in comments
 161          * in rmnet_map_ipv4_dl_csum_trailer()).
 162          */
 163         ip_header_csum = (__force __be16)ip_fast_csum(ip6h, sizeof(*ip6h) / 4);
 164         ip6_payload_csum = ~csum16_sub((__force __sum16)csum_trailer->csum_value,
 165                                        ip_header_csum);
 166
 167         length = (ip6h->nexthdr == IPPROTO_UDP) ?
 168                  ntohs(((struct udphdr *)txporthdr)->len) :
 169                  ntohs(ip6h->payload_len);
 170         pseudo_csum = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
 171                                        length, ip6h->nexthdr, 0);
 172         pseudo_csum = csum16_add(ip6_payload_csum, (__force __be16)pseudo_csum);
 173
 174         /* The cast is required to ensure only the low 16 bits are examined */
 175         if ((__sum16)~pseudo_csum) {
 176                 priv->stats.csum_validation_failed++;
 177                 return -EINVAL;
 178         }
 179
 180         csum_value_final = ~csum16_sub(pseudo_csum, (__force __be16)*csum_field);
 181
 182         if (unlikely(csum_value_final == 0)) {
 183                 switch (ip6h->nexthdr) {
 184                 case IPPROTO_UDP:
 185                         /* RFC 2460 section 8.1
 186                          * DL6 One's complement rule for UDP checksum 0
 187                          */
 188                         csum_value_final = ~csum_value_final;
 189                         break;
 190
 191                 case IPPROTO_TCP:
 192                         /* DL6 Non-RFC compliant TCP checksum found */
 193                         if (*csum_field == (__force __sum16)0xFFFF)
 194                                 csum_value_final = ~csum_value_final;
 195                         break;
 196                 }
 197         }
 198
 199         if (csum_value_final == *csum_field) {
 200                 priv->stats.csum_ok++;
 201                 return 0;
 202         } else {
 203                 priv->stats.csum_validation_failed++;
 204                 return -EINVAL;
 205         }
 206 }
 207 #endif
 208
 209 static void rmnet_map_complement_ipv4_txporthdr_csum_field(void *iphdr)
 210 {
 211         struct iphdr *ip4h = (struct iphdr *)iphdr;
 212         void *txphdr;
 213         u16 *csum;
 214
 215         txphdr = iphdr + ip4h->ihl * 4;
 216
 217         if (ip4h->protocol == IPPROTO_TCP || ip4h->protocol == IPPROTO_UDP) {
 218                 csum = (u16 *)rmnet_map_get_csum_field(ip4h->protocol, txphdr);
 219                 *csum = ~(*csum);
 220         }
 221 }
 222
 223 static void
 224 rmnet_map_ipv4_ul_csum_header(struct iphdr *iphdr,
 225                               struct rmnet_map_ul_csum_header *ul_header,
 226                               struct sk_buff *skb)
 227 {
 228         u16 val;
 229
 230         val = MAP_CSUM_UL_ENABLED_FLAG;
 231         if (iphdr->protocol == IPPROTO_UDP)
 232                 val |= MAP_CSUM_UL_UDP_FLAG;
 233         val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
 234
 235         ul_header->csum_start_offset = htons(skb_network_header_len(skb));
 236         ul_header->csum_info = htons(val);
 237
 238         skb->ip_summed = CHECKSUM_NONE;
 239
 240         rmnet_map_complement_ipv4_txporthdr_csum_field(iphdr);
 241 }
 242
 243 #if IS_ENABLED(CONFIG_IPV6)
 244 static void rmnet_map_complement_ipv6_txporthdr_csum_field(void *ip6hdr)
 245 {
 246         struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr;
 247         void *txphdr;
 248         u16 *csum;
 249
 250         txphdr = ip6hdr + sizeof(struct ipv6hdr);
 251
 252         if (ip6h->nexthdr == IPPROTO_TCP || ip6h->nexthdr == IPPROTO_UDP) {
 253                 csum = (u16 *)rmnet_map_get_csum_field(ip6h->nexthdr, txphdr);
 254                 *csum = ~(*csum);
 255         }
 256 }
 257
 258 static void
 259 rmnet_map_ipv6_ul_csum_header(struct ipv6hdr *ipv6hdr,
 260                               struct rmnet_map_ul_csum_header *ul_header,
 261                               struct sk_buff *skb)
 262 {
 263         u16 val;
 264
 265         val = MAP_CSUM_UL_ENABLED_FLAG;
 266         if (ipv6hdr->nexthdr == IPPROTO_UDP)
 267                 val |= MAP_CSUM_UL_UDP_FLAG;
 268         val |= skb->csum_offset & MAP_CSUM_UL_OFFSET_MASK;
 269
 270         ul_header->csum_start_offset = htons(skb_network_header_len(skb));
 271         ul_header->csum_info = htons(val);
 272
 273         skb->ip_summed = CHECKSUM_NONE;
 274
 275         rmnet_map_complement_ipv6_txporthdr_csum_field(ipv6hdr);
 276 }
 277 #endif
 278
 279 static void rmnet_map_v5_checksum_uplink_packet(struct sk_buff *skb,
 280                                                 struct rmnet_port *port,
 281                                                 struct net_device *orig_dev)
 282 {
 283         struct rmnet_priv *priv = netdev_priv(orig_dev);
 284         struct rmnet_map_v5_csum_header *ul_header;
 285
 286         ul_header = skb_push(skb, sizeof(*ul_header));
 287         memset(ul_header, 0, sizeof(*ul_header));
 288         ul_header->header_info = u8_encode_bits(RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD,
 289                                                 MAPV5_HDRINFO_HDR_TYPE_FMASK);
 290
 291         if (skb->ip_summed == CHECKSUM_PARTIAL) {
 292                 void *iph = ip_hdr(skb);
 293                 __sum16 *check;
 294                 void *trans;
 295                 u8 proto;
 296
 297                 if (skb->protocol == htons(ETH_P_IP)) {
 298                         u16 ip_len = ((struct iphdr *)iph)->ihl * 4;
 299
 300                         proto = ((struct iphdr *)iph)->protocol;
 301                         trans = iph + ip_len;
 302                 } else if (IS_ENABLED(CONFIG_IPV6) &&
 303                            skb->protocol == htons(ETH_P_IPV6)) {
 304                         u16 ip_len = sizeof(struct ipv6hdr);
 305
 306                         proto = ((struct ipv6hdr *)iph)->nexthdr;
 307                         trans = iph + ip_len;
 308                 } else {
 309                         priv->stats.csum_err_invalid_ip_version++;
 310                         goto sw_csum;
 311                 }
 312
 313                 check = rmnet_map_get_csum_field(proto, trans);
 314                 if (check) {
 315                         skb->ip_summed = CHECKSUM_NONE;
 316                         /* Ask for checksum offloading */
 317                         ul_header->csum_info |= MAPV5_CSUMINFO_VALID_FLAG;
 318                         priv->stats.csum_hw++;
 319                         return;
 320                 }
 321         }
 322
 323 sw_csum:
 324         priv->stats.csum_sw++;
 325 }
 326
 327 /* Adds MAP header to front of skb->data
 328  * Padding is calculated and set appropriately in MAP header. Mux ID is
 329  * initialized to 0.
 330  */
 331 struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
 332                                                   int hdrlen,
 333                                                   struct rmnet_port *port,
 334                                                   int pad)
 335 {
 336         struct rmnet_map_header *map_header;
 337         u32 padding, map_datalen;
 338         u8 *padbytes;
 339
 340         map_datalen = skb->len - hdrlen;
 341         map_header = (struct rmnet_map_header *)
 342                         skb_push(skb, sizeof(struct rmnet_map_header));
 343         memset(map_header, 0, sizeof(struct rmnet_map_header));
 344
 345         /* Set next_hdr bit for csum offload packets */
 346         if (port->data_format & RMNET_FLAGS_EGRESS_MAP_CKSUMV5)
 347                 map_header->flags |= MAP_NEXT_HEADER_FLAG;
 348
 349         if (pad == RMNET_MAP_NO_PAD_BYTES) {
 350                 map_header->pkt_len = htons(map_datalen);
 351                 return map_header;
 352         }
 353
 354         BUILD_BUG_ON(MAP_PAD_LEN_MASK < 3);
 355         padding = ALIGN(map_datalen, 4) - map_datalen;
 356
 357         if (padding == 0)
 358                 goto done;
 359
 360         if (skb_tailroom(skb) < padding)
 361                 return NULL;
 362
 363         padbytes = (u8 *)skb_put(skb, padding);
 364         memset(padbytes, 0, padding);
 365
 366 done:
 367         map_header->pkt_len = htons(map_datalen + padding);
 368         /* This is a data packet, so the CMD bit is 0 */
 369         map_header->flags = padding & MAP_PAD_LEN_MASK;
 370
 371         return map_header;
 372 }
 373
 374 /* Deaggregates a single packet
 375  * A whole new buffer is allocated for each portion of an aggregated frame.
 376  * Caller should keep calling deaggregate() on the source skb until 0 is
 377  * returned, indicating that there are no more packets to deaggregate. Caller
 378  * is responsible for freeing the original skb.
 379  */
 380 struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb,
 381                                       struct rmnet_port *port)
 382 {
 383         struct rmnet_map_v5_csum_header *next_hdr = NULL;
 384         struct rmnet_map_header *maph;
 385         void *data = skb->data;
 386         struct sk_buff *skbn;
 387         u8 nexthdr_type;
 388         u32 packet_len;
 389
 390         if (skb->len == 0)
 391                 return NULL;
 392
 393         maph = (struct rmnet_map_header *)skb->data;
 394         packet_len = ntohs(maph->pkt_len) + sizeof(*maph);
 395
 396         if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) {
 397                 packet_len += sizeof(struct rmnet_map_dl_csum_trailer);
 398         } else if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV5) {
 399                 if (!(maph->flags & MAP_CMD_FLAG)) {
 400                         packet_len += sizeof(*next_hdr);
 401                         if (maph->flags & MAP_NEXT_HEADER_FLAG)
 402                                 next_hdr = data + sizeof(*maph);
 403                         else
 404                                 /* Mapv5 data pkt without csum hdr is invalid */
 405                                 return NULL;
 406                 }
 407         }
 408
 409         if (((int)skb->len - (int)packet_len) < 0)
 410                 return NULL;
 411
 412         /* Some hardware can send us empty frames. Catch them */
 413         if (!maph->pkt_len)
 414                 return NULL;
 415
 416         if (next_hdr) {
 417                 nexthdr_type = u8_get_bits(next_hdr->header_info,
 418                                            MAPV5_HDRINFO_HDR_TYPE_FMASK);
 419                 if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
 420                         return NULL;
 421         }
 422
 423         skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
 424         if (!skbn)
 425                 return NULL;
 426
 427         skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
 428         skb_put(skbn, packet_len);
 429         memcpy(skbn->data, skb->data, packet_len);
 430         skb_pull(skb, packet_len);
 431
 432         return skbn;
 433 }
 434
 435 /* Validates packet checksums. Function takes a pointer to
 436  * the beginning of a buffer which contains the IP payload +
 437  * padding + checksum trailer.
 438  * Only IPv4 and IPv6 are supported along with TCP & UDP.
 439  * Fragmented or tunneled packets are not supported.
 440  */
 441 int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len)
 442 {
 443         struct rmnet_priv *priv = netdev_priv(skb->dev);
 444         struct rmnet_map_dl_csum_trailer *csum_trailer;
 445
 446         if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
 447                 priv->stats.csum_sw++;
 448                 return -EOPNOTSUPP;
 449         }
 450
 451         csum_trailer = (struct rmnet_map_dl_csum_trailer *)(skb->data + len);
 452
 453         if (!(csum_trailer->flags & MAP_CSUM_DL_VALID_FLAG)) {
 454                 priv->stats.csum_valid_unset++;
 455                 return -EINVAL;
 456         }
 457
 458         if (skb->protocol == htons(ETH_P_IP))
 459                 return rmnet_map_ipv4_dl_csum_trailer(skb, csum_trailer, priv);
 460
 461         if (IS_ENABLED(CONFIG_IPV6) && skb->protocol == htons(ETH_P_IPV6))
 462                 return rmnet_map_ipv6_dl_csum_trailer(skb, csum_trailer, priv);
 463
 464         priv->stats.csum_err_invalid_ip_version++;
 465
 466         return -EPROTONOSUPPORT;
 467 }
 468
 469 static void rmnet_map_v4_checksum_uplink_packet(struct sk_buff *skb,
 470                                                 struct net_device *orig_dev)
 471 {
 472         struct rmnet_priv *priv = netdev_priv(orig_dev);
 473         struct rmnet_map_ul_csum_header *ul_header;
 474         void *iphdr;
 475
 476         ul_header = (struct rmnet_map_ul_csum_header *)
 477                     skb_push(skb, sizeof(struct rmnet_map_ul_csum_header));
 478
 479         if (unlikely(!(orig_dev->features &
 480                      (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM))))
 481                 goto sw_csum;
 482
 483         if (skb->ip_summed != CHECKSUM_PARTIAL)
 484                 goto sw_csum;
 485
 486         iphdr = (char *)ul_header +
 487                 sizeof(struct rmnet_map_ul_csum_header);
 488
 489         if (skb->protocol == htons(ETH_P_IP)) {
 490                 rmnet_map_ipv4_ul_csum_header(iphdr, ul_header, skb);
 491                 priv->stats.csum_hw++;
 492                 return;
 493         }
 494
 495         if (IS_ENABLED(CONFIG_IPV6) && skb->protocol == htons(ETH_P_IPV6)) {
 496                 rmnet_map_ipv6_ul_csum_header(iphdr, ul_header, skb);
 497                 priv->stats.csum_hw++;
 498                 return;
 499         }
 500
 501         priv->stats.csum_err_invalid_ip_version++;
 502
 503 sw_csum:
 504         memset(ul_header, 0, sizeof(*ul_header));
 505
 506         priv->stats.csum_sw++;
 507 }
 508
 509 /* Generates UL checksum meta info header for IPv4 and IPv6 over TCP and UDP
 510  * packets that are supported for UL checksum offload.
 511  */
 512 void rmnet_map_checksum_uplink_packet(struct sk_buff *skb,
 513                                       struct rmnet_port *port,
 514                                       struct net_device *orig_dev,
 515                                       int csum_type)
 516 {
 517         switch (csum_type) {
 518         case RMNET_FLAGS_EGRESS_MAP_CKSUMV4:
 519                 rmnet_map_v4_checksum_uplink_packet(skb, orig_dev);
 520                 break;
 521         case RMNET_FLAGS_EGRESS_MAP_CKSUMV5:
 522                 rmnet_map_v5_checksum_uplink_packet(skb, port, orig_dev);
 523                 break;
 524         default:
 525                 break;
 526         }
 527 }
 528
 529 /* Process a MAPv5 packet header */
 530 int rmnet_map_process_next_hdr_packet(struct sk_buff *skb,
 531                                       u16 len)
 532 {
 533         struct rmnet_priv *priv = netdev_priv(skb->dev);
 534         struct rmnet_map_v5_csum_header *next_hdr;
 535         u8 nexthdr_type;
 536
 537         next_hdr = (struct rmnet_map_v5_csum_header *)(skb->data +
 538                         sizeof(struct rmnet_map_header));
 539
 540         nexthdr_type = u8_get_bits(next_hdr->header_info,
 541                                    MAPV5_HDRINFO_HDR_TYPE_FMASK);
 542
 543         if (nexthdr_type != RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD)
 544                 return -EINVAL;
 545
 546         if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
 547                 priv->stats.csum_sw++;
 548         } else if (next_hdr->csum_info & MAPV5_CSUMINFO_VALID_FLAG) {
 549                 priv->stats.csum_ok++;
 550                 skb->ip_summed = CHECKSUM_UNNECESSARY;
 551         } else {
 552                 priv->stats.csum_valid_unset++;
 553         }
 554
 555         /* Pull csum v5 header */
 556         skb_pull(skb, sizeof(*next_hdr));
 557
 558         return 0;
 559 }