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

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Copyright (c) 2013-2018, 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 "rmnet_config.h"
  12 #include "rmnet_map.h"
  13 #include "rmnet_private.h"
  14
  15 #define RMNET_MAP_DEAGGR_SPACING  64
  16 #define RMNET_MAP_DEAGGR_HEADROOM (RMNET_MAP_DEAGGR_SPACING / 2)
  17
  18 static __sum16 *rmnet_map_get_csum_field(unsigned char protocol,
  19                                          const void *txporthdr)
  20 {
  21         __sum16 *check = NULL;
  22
  23         switch (protocol) {
  24         case IPPROTO_TCP:
  25                 check = &(((struct tcphdr *)txporthdr)->check);
  26                 break;
  27
  28         case IPPROTO_UDP:
  29                 check = &(((struct udphdr *)txporthdr)->check);
  30                 break;
  31
  32         default:
  33                 check = NULL;
  34                 break;
  35         }
  36
  37         return check;
  38 }
  39
  40 static int
  41 rmnet_map_ipv4_dl_csum_trailer(struct sk_buff *skb,
  42                                struct rmnet_map_dl_csum_trailer *csum_trailer,
  43                                struct rmnet_priv *priv)
  44 {
  45         __sum16 *csum_field, csum_temp, pseudo_csum, hdr_csum, ip_payload_csum;
  46         u16 csum_value, csum_value_final;
  47         struct iphdr *ip4h;
  48         void *txporthdr;
  49         __be16 addend;
  50
  51         ip4h = (struct iphdr *)(skb->data);
  52         if ((ntohs(ip4h->frag_off) & IP_MF) ||
  53             ((ntohs(ip4h->frag_off) & IP_OFFSET) > 0)) {
  54                 priv->stats.csum_fragmented_pkt++;
  55                 return -EOPNOTSUPP;
  56         }
  57
  58         txporthdr = skb->data + ip4h->ihl * 4;
  59
  60         csum_field = rmnet_map_get_csum_field(ip4h->protocol, txporthdr);
  61
  62         if (!csum_field) {
  63                 priv->stats.csum_err_invalid_transport++;
  64                 return -EPROTONOSUPPORT;
  65         }
  66
  67         /* RFC 768 - Skip IPv4 UDP packets where sender checksum field is 0 */
  68         if (*csum_field == 0 && ip4h->protocol == IPPROTO_UDP) {
  69                 priv->stats.csum_skipped++;
  70                 return 0;
  71         }
  72
  73         csum_value = ~ntohs(csum_trailer->csum_value);
  74         hdr_csum = ~ip_fast_csum(ip4h, (int)ip4h->ihl);
  75         ip_payload_csum = csum16_sub((__force __sum16)csum_value,
  76                                      (__force __be16)hdr_csum);
  77
  78         pseudo_csum = ~csum_tcpudp_magic(ip4h->saddr, ip4h->daddr,
  79                                          ntohs(ip4h->tot_len) - ip4h->ihl * 4,
  80                                          ip4h->protocol, 0);
  81         addend = (__force __be16)ntohs((__force __be16)pseudo_csum);
  82         pseudo_csum = csum16_add(ip_payload_csum, addend);
  83
  84         addend = (__force __be16)ntohs((__force __be16)*csum_field);
  85         csum_temp = ~csum16_sub(pseudo_csum, addend);
  86         csum_value_final = (__force u16)csum_temp;
  87
  88         if (unlikely(csum_value_final == 0)) {
  89                 switch (ip4h->protocol) {
  90                 case IPPROTO_UDP:
  91                         /* RFC 768 - DL4 1's complement rule for UDP csum 0 */
  92                         csum_value_final = ~csum_value_final;
  93                         break;
  94
  95                 case IPPROTO_TCP:
  96                         /* DL4 Non-RFC compliant TCP checksum found */
  97                         if (*csum_field == (__force __sum16)0xFFFF)
  98                                 csum_value_final = ~csum_value_final;
  99                         break;
 100                 }
 101         }
 102
 103         if (csum_value_final == ntohs((__force __be16)*csum_field)) {
 104                 priv->stats.csum_ok++;
 105                 return 0;
 106         } else {
 107                 priv->stats.csum_validation_failed++;
 108                 return -EINVAL;
 109         }
 110 }
 111
 112 #if IS_ENABLED(CONFIG_IPV6)
 113 static int
 114 rmnet_map_ipv6_dl_csum_trailer(struct sk_buff *skb,
 115                                struct rmnet_map_dl_csum_trailer *csum_trailer,
 116                                struct rmnet_priv *priv)
 117 {
 118         __sum16 *csum_field, ip6_payload_csum, pseudo_csum, csum_temp;
 119         u16 csum_value, csum_value_final;
 120         __be16 ip6_hdr_csum, addend;
 121         struct ipv6hdr *ip6h;
 122         void *txporthdr;
 123         u32 length;
 124
 125         ip6h = (struct ipv6hdr *)(skb->data);
 126
 127         txporthdr = skb->data + sizeof(struct ipv6hdr);
 128         csum_field = rmnet_map_get_csum_field(ip6h->nexthdr, txporthdr);
 129
 130         if (!csum_field) {
 131                 priv->stats.csum_err_invalid_transport++;
 132                 return -EPROTONOSUPPORT;
 133         }
 134
 135         csum_value = ~ntohs(csum_trailer->csum_value);
 136         ip6_hdr_csum = (__force __be16)
 137                         ~ntohs((__force __be16)ip_compute_csum(ip6h,
 138                                (int)(txporthdr - (void *)(skb->data))));
 139         ip6_payload_csum = csum16_sub((__force __sum16)csum_value,
 140                                       ip6_hdr_csum);
 141
 142         length = (ip6h->nexthdr == IPPROTO_UDP) ?
 143                  ntohs(((struct udphdr *)txporthdr)->len) :
 144                  ntohs(ip6h->payload_len);
 145         pseudo_csum = ~(csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
 146                              length, ip6h->nexthdr, 0));
 147         addend = (__force __be16)ntohs((__force __be16)pseudo_csum);
 148         pseudo_csum = csum16_add(ip6_payload_csum, addend);
 149
 150         addend = (__force __be16)ntohs((__force __be16)*csum_field);
 151         csum_temp = ~csum16_sub(pseudo_csum, addend);
 152         csum_value_final = (__force u16)csum_temp;
 153
 154         if (unlikely(csum_value_final == 0)) {
 155                 switch (ip6h->nexthdr) {
 156                 case IPPROTO_UDP:
 157                         /* RFC 2460 section 8.1
 158                          * DL6 One's complement rule for UDP checksum 0
 159                          */
 160                         csum_value_final = ~csum_value_final;
 161                         break;
 162
 163                 case IPPROTO_TCP:
 164                         /* DL6 Non-RFC compliant TCP checksum found */
 165                         if (*csum_field == (__force __sum16)0xFFFF)
 166                                 csum_value_final = ~csum_value_final;
 167                         break;
 168                 }
 169         }
 170
 171         if (csum_value_final == ntohs((__force __be16)*csum_field)) {
 172                 priv->stats.csum_ok++;
 173                 return 0;
 174         } else {
 175                 priv->stats.csum_validation_failed++;
 176                 return -EINVAL;
 177         }
 178 }
 179 #endif
 180
 181 static void rmnet_map_complement_ipv4_txporthdr_csum_field(void *iphdr)
 182 {
 183         struct iphdr *ip4h = (struct iphdr *)iphdr;
 184         void *txphdr;
 185         u16 *csum;
 186
 187         txphdr = iphdr + ip4h->ihl * 4;
 188
 189         if (ip4h->protocol == IPPROTO_TCP || ip4h->protocol == IPPROTO_UDP) {
 190                 csum = (u16 *)rmnet_map_get_csum_field(ip4h->protocol, txphdr);
 191                 *csum = ~(*csum);
 192         }
 193 }
 194
 195 static void
 196 rmnet_map_ipv4_ul_csum_header(void *iphdr,
 197                               struct rmnet_map_ul_csum_header *ul_header,
 198                               struct sk_buff *skb)
 199 {
 200         struct iphdr *ip4h = (struct iphdr *)iphdr;
 201         __be16 *hdr = (__be16 *)ul_header, offset;
 202
 203         offset = htons((__force u16)(skb_transport_header(skb) -
 204                                      (unsigned char *)iphdr));
 205         ul_header->csum_start_offset = offset;
 206         ul_header->csum_insert_offset = skb->csum_offset;
 207         ul_header->csum_enabled = 1;
 208         if (ip4h->protocol == IPPROTO_UDP)
 209                 ul_header->udp_ind = 1;
 210         else
 211                 ul_header->udp_ind = 0;
 212
 213         /* Changing remaining fields to network order */
 214         hdr++;
 215         *hdr = htons((__force u16)*hdr);
 216
 217         skb->ip_summed = CHECKSUM_NONE;
 218
 219         rmnet_map_complement_ipv4_txporthdr_csum_field(iphdr);
 220 }
 221
 222 #if IS_ENABLED(CONFIG_IPV6)
 223 static void rmnet_map_complement_ipv6_txporthdr_csum_field(void *ip6hdr)
 224 {
 225         struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr;
 226         void *txphdr;
 227         u16 *csum;
 228
 229         txphdr = ip6hdr + sizeof(struct ipv6hdr);
 230
 231         if (ip6h->nexthdr == IPPROTO_TCP || ip6h->nexthdr == IPPROTO_UDP) {
 232                 csum = (u16 *)rmnet_map_get_csum_field(ip6h->nexthdr, txphdr);
 233                 *csum = ~(*csum);
 234         }
 235 }
 236
 237 static void
 238 rmnet_map_ipv6_ul_csum_header(void *ip6hdr,
 239                               struct rmnet_map_ul_csum_header *ul_header,
 240                               struct sk_buff *skb)
 241 {
 242         struct ipv6hdr *ip6h = (struct ipv6hdr *)ip6hdr;
 243         __be16 *hdr = (__be16 *)ul_header, offset;
 244
 245         offset = htons((__force u16)(skb_transport_header(skb) -
 246                                      (unsigned char *)ip6hdr));
 247         ul_header->csum_start_offset = offset;
 248         ul_header->csum_insert_offset = skb->csum_offset;
 249         ul_header->csum_enabled = 1;
 250
 251         if (ip6h->nexthdr == IPPROTO_UDP)
 252                 ul_header->udp_ind = 1;
 253         else
 254                 ul_header->udp_ind = 0;
 255
 256         /* Changing remaining fields to network order */
 257         hdr++;
 258         *hdr = htons((__force u16)*hdr);
 259
 260         skb->ip_summed = CHECKSUM_NONE;
 261
 262         rmnet_map_complement_ipv6_txporthdr_csum_field(ip6hdr);
 263 }
 264 #endif
 265
 266 /* Adds MAP header to front of skb->data
 267  * Padding is calculated and set appropriately in MAP header. Mux ID is
 268  * initialized to 0.
 269  */
 270 struct rmnet_map_header *rmnet_map_add_map_header(struct sk_buff *skb,
 271                                                   int hdrlen, int pad)
 272 {
 273         struct rmnet_map_header *map_header;
 274         u32 padding, map_datalen;
 275         u8 *padbytes;
 276
 277         map_datalen = skb->len - hdrlen;
 278         map_header = (struct rmnet_map_header *)
 279                         skb_push(skb, sizeof(struct rmnet_map_header));
 280         memset(map_header, 0, sizeof(struct rmnet_map_header));
 281
 282         if (pad == RMNET_MAP_NO_PAD_BYTES) {
 283                 map_header->pkt_len = htons(map_datalen);
 284                 return map_header;
 285         }
 286
 287         padding = ALIGN(map_datalen, 4) - map_datalen;
 288
 289         if (padding == 0)
 290                 goto done;
 291
 292         if (skb_tailroom(skb) < padding)
 293                 return NULL;
 294
 295         padbytes = (u8 *)skb_put(skb, padding);
 296         memset(padbytes, 0, padding);
 297
 298 done:
 299         map_header->pkt_len = htons(map_datalen + padding);
 300         map_header->pad_len = padding & 0x3F;
 301
 302         return map_header;
 303 }
 304
 305 /* Deaggregates a single packet
 306  * A whole new buffer is allocated for each portion of an aggregated frame.
 307  * Caller should keep calling deaggregate() on the source skb until 0 is
 308  * returned, indicating that there are no more packets to deaggregate. Caller
 309  * is responsible for freeing the original skb.
 310  */
 311 struct sk_buff *rmnet_map_deaggregate(struct sk_buff *skb,
 312                                       struct rmnet_port *port)
 313 {
 314         struct rmnet_map_header *maph;
 315         struct sk_buff *skbn;
 316         u32 packet_len;
 317
 318         if (skb->len == 0)
 319                 return NULL;
 320
 321         maph = (struct rmnet_map_header *)skb->data;
 322         packet_len = ntohs(maph->pkt_len) + sizeof(struct rmnet_map_header);
 323
 324         if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4)
 325                 packet_len += sizeof(struct rmnet_map_dl_csum_trailer);
 326
 327         if (((int)skb->len - (int)packet_len) < 0)
 328                 return NULL;
 329
 330         /* Some hardware can send us empty frames. Catch them */
 331         if (ntohs(maph->pkt_len) == 0)
 332                 return NULL;
 333
 334         skbn = alloc_skb(packet_len + RMNET_MAP_DEAGGR_SPACING, GFP_ATOMIC);
 335         if (!skbn)
 336                 return NULL;
 337
 338         skb_reserve(skbn, RMNET_MAP_DEAGGR_HEADROOM);
 339         skb_put(skbn, packet_len);
 340         memcpy(skbn->data, skb->data, packet_len);
 341         skb_pull(skb, packet_len);
 342
 343         return skbn;
 344 }
 345
 346 /* Validates packet checksums. Function takes a pointer to
 347  * the beginning of a buffer which contains the IP payload +
 348  * padding + checksum trailer.
 349  * Only IPv4 and IPv6 are supported along with TCP & UDP.
 350  * Fragmented or tunneled packets are not supported.
 351  */
 352 int rmnet_map_checksum_downlink_packet(struct sk_buff *skb, u16 len)
 353 {
 354         struct rmnet_priv *priv = netdev_priv(skb->dev);
 355         struct rmnet_map_dl_csum_trailer *csum_trailer;
 356
 357         if (unlikely(!(skb->dev->features & NETIF_F_RXCSUM))) {
 358                 priv->stats.csum_sw++;
 359                 return -EOPNOTSUPP;
 360         }
 361
 362         csum_trailer = (struct rmnet_map_dl_csum_trailer *)(skb->data + len);
 363
 364         if (!csum_trailer->valid) {
 365                 priv->stats.csum_valid_unset++;
 366                 return -EINVAL;
 367         }
 368
 369         if (skb->protocol == htons(ETH_P_IP)) {
 370                 return rmnet_map_ipv4_dl_csum_trailer(skb, csum_trailer, priv);
 371         } else if (skb->protocol == htons(ETH_P_IPV6)) {
 372 #if IS_ENABLED(CONFIG_IPV6)
 373                 return rmnet_map_ipv6_dl_csum_trailer(skb, csum_trailer, priv);
 374 #else
 375                 priv->stats.csum_err_invalid_ip_version++;
 376                 return -EPROTONOSUPPORT;
 377 #endif
 378         } else {
 379                 priv->stats.csum_err_invalid_ip_version++;
 380                 return -EPROTONOSUPPORT;
 381         }
 382
 383         return 0;
 384 }
 385
 386 /* Generates UL checksum meta info header for IPv4 and IPv6 over TCP and UDP
 387  * packets that are supported for UL checksum offload.
 388  */
 389 void rmnet_map_checksum_uplink_packet(struct sk_buff *skb,
 390                                       struct net_device *orig_dev)
 391 {
 392         struct rmnet_priv *priv = netdev_priv(orig_dev);
 393         struct rmnet_map_ul_csum_header *ul_header;
 394         void *iphdr;
 395
 396         ul_header = (struct rmnet_map_ul_csum_header *)
 397                     skb_push(skb, sizeof(struct rmnet_map_ul_csum_header));
 398
 399         if (unlikely(!(orig_dev->features &
 400                      (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM))))
 401                 goto sw_csum;
 402
 403         if (skb->ip_summed == CHECKSUM_PARTIAL) {
 404                 iphdr = (char *)ul_header +
 405                         sizeof(struct rmnet_map_ul_csum_header);
 406
 407                 if (skb->protocol == htons(ETH_P_IP)) {
 408                         rmnet_map_ipv4_ul_csum_header(iphdr, ul_header, skb);
 409                         return;
 410                 } else if (skb->protocol == htons(ETH_P_IPV6)) {
 411 #if IS_ENABLED(CONFIG_IPV6)
 412                         rmnet_map_ipv6_ul_csum_header(iphdr, ul_header, skb);
 413                         return;
 414 #else
 415                         priv->stats.csum_err_invalid_ip_version++;
 416                         goto sw_csum;
 417 #endif
 418                 } else {
 419                         priv->stats.csum_err_invalid_ip_version++;
 420                 }
 421         }
 422
 423 sw_csum:
 424         ul_header->csum_start_offset = 0;
 425         ul_header->csum_insert_offset = 0;
 426         ul_header->csum_enabled = 0;
 427         ul_header->udp_ind = 0;
 428
 429         priv->stats.csum_sw++;
 430 }