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