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 } 431