1 /* 2 * Copyright 2011, Siemens AG 3 * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com> 4 */ 5 6 /* Based on patches from Jon Smirl <jonsmirl@gmail.com> 7 * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com> 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License version 2 11 * as published by the Free Software Foundation. 12 * 13 * This program is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU General Public License for more details. 17 * 18 */ 19 20 /* Jon's code is based on 6lowpan implementation for Contiki which is: 21 * Copyright (c) 2008, Swedish Institute of Computer Science. 22 * All rights reserved. 23 * 24 * Redistribution and use in source and binary forms, with or without 25 * modification, are permitted provided that the following conditions 26 * are met: 27 * 1. Redistributions of source code must retain the above copyright 28 * notice, this list of conditions and the following disclaimer. 29 * 2. Redistributions in binary form must reproduce the above copyright 30 * notice, this list of conditions and the following disclaimer in the 31 * documentation and/or other materials provided with the distribution. 32 * 3. Neither the name of the Institute nor the names of its contributors 33 * may be used to endorse or promote products derived from this software 34 * without specific prior written permission. 35 * 36 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND 37 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 38 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 39 * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE 40 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 41 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 42 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 43 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 44 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 45 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 46 * SUCH DAMAGE. 47 */ 48 49 #include <linux/bitops.h> 50 #include <linux/if_arp.h> 51 #include <linux/netdevice.h> 52 53 #include <net/6lowpan.h> 54 #include <net/ipv6.h> 55 56 #include "6lowpan_i.h" 57 #include "nhc.h" 58 59 /* Values of fields within the IPHC encoding first byte */ 60 #define LOWPAN_IPHC_TF_MASK 0x18 61 #define LOWPAN_IPHC_TF_00 0x00 62 #define LOWPAN_IPHC_TF_01 0x08 63 #define LOWPAN_IPHC_TF_10 0x10 64 #define LOWPAN_IPHC_TF_11 0x18 65 66 #define LOWPAN_IPHC_NH 0x04 67 68 #define LOWPAN_IPHC_HLIM_MASK 0x03 69 #define LOWPAN_IPHC_HLIM_00 0x00 70 #define LOWPAN_IPHC_HLIM_01 0x01 71 #define LOWPAN_IPHC_HLIM_10 0x02 72 #define LOWPAN_IPHC_HLIM_11 0x03 73 74 /* Values of fields within the IPHC encoding second byte */ 75 #define LOWPAN_IPHC_CID 0x80 76 77 #define LOWPAN_IPHC_SAC 0x40 78 79 #define LOWPAN_IPHC_SAM_MASK 0x30 80 #define LOWPAN_IPHC_SAM_00 0x00 81 #define LOWPAN_IPHC_SAM_01 0x10 82 #define LOWPAN_IPHC_SAM_10 0x20 83 #define LOWPAN_IPHC_SAM_11 0x30 84 85 #define LOWPAN_IPHC_M 0x08 86 87 #define LOWPAN_IPHC_DAC 0x04 88 89 #define LOWPAN_IPHC_DAM_MASK 0x03 90 #define LOWPAN_IPHC_DAM_00 0x00 91 #define LOWPAN_IPHC_DAM_01 0x01 92 #define LOWPAN_IPHC_DAM_10 0x02 93 #define LOWPAN_IPHC_DAM_11 0x03 94 95 /* ipv6 address based on mac 96 * second bit-flip (Universe/Local) is done according RFC2464 97 */ 98 #define is_addr_mac_addr_based(a, m) \ 99 ((((a)->s6_addr[8]) == (((m)[0]) ^ 0x02)) && \ 100 (((a)->s6_addr[9]) == (m)[1]) && \ 101 (((a)->s6_addr[10]) == (m)[2]) && \ 102 (((a)->s6_addr[11]) == (m)[3]) && \ 103 (((a)->s6_addr[12]) == (m)[4]) && \ 104 (((a)->s6_addr[13]) == (m)[5]) && \ 105 (((a)->s6_addr[14]) == (m)[6]) && \ 106 (((a)->s6_addr[15]) == (m)[7])) 107 108 /* check whether we can compress the IID to 16 bits, 109 * it's possible for unicast addresses with first 49 bits are zero only. 110 */ 111 #define lowpan_is_iid_16_bit_compressable(a) \ 112 ((((a)->s6_addr16[4]) == 0) && \ 113 (((a)->s6_addr[10]) == 0) && \ 114 (((a)->s6_addr[11]) == 0xff) && \ 115 (((a)->s6_addr[12]) == 0xfe) && \ 116 (((a)->s6_addr[13]) == 0)) 117 118 /* check whether the 112-bit gid of the multicast address is mappable to: */ 119 120 /* 48 bits, FFXX::00XX:XXXX:XXXX */ 121 #define lowpan_is_mcast_addr_compressable48(a) \ 122 ((((a)->s6_addr16[1]) == 0) && \ 123 (((a)->s6_addr16[2]) == 0) && \ 124 (((a)->s6_addr16[3]) == 0) && \ 125 (((a)->s6_addr16[4]) == 0) && \ 126 (((a)->s6_addr[10]) == 0)) 127 128 /* 32 bits, FFXX::00XX:XXXX */ 129 #define lowpan_is_mcast_addr_compressable32(a) \ 130 ((((a)->s6_addr16[1]) == 0) && \ 131 (((a)->s6_addr16[2]) == 0) && \ 132 (((a)->s6_addr16[3]) == 0) && \ 133 (((a)->s6_addr16[4]) == 0) && \ 134 (((a)->s6_addr16[5]) == 0) && \ 135 (((a)->s6_addr[12]) == 0)) 136 137 /* 8 bits, FF02::00XX */ 138 #define lowpan_is_mcast_addr_compressable8(a) \ 139 ((((a)->s6_addr[1]) == 2) && \ 140 (((a)->s6_addr16[1]) == 0) && \ 141 (((a)->s6_addr16[2]) == 0) && \ 142 (((a)->s6_addr16[3]) == 0) && \ 143 (((a)->s6_addr16[4]) == 0) && \ 144 (((a)->s6_addr16[5]) == 0) && \ 145 (((a)->s6_addr16[6]) == 0) && \ 146 (((a)->s6_addr[14]) == 0)) 147 148 #define lowpan_is_linklocal_zero_padded(a) \ 149 (!(hdr->saddr.s6_addr[1] & 0x3f) && \ 150 !hdr->saddr.s6_addr16[1] && \ 151 !hdr->saddr.s6_addr32[1]) 152 153 #define LOWPAN_IPHC_CID_DCI(cid) (cid & 0x0f) 154 #define LOWPAN_IPHC_CID_SCI(cid) ((cid & 0xf0) >> 4) 155 156 static inline void lowpan_iphc_uncompress_802154_lladdr(struct in6_addr * ipaddr,const void * lladdr)157 lowpan_iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr, 158 const void *lladdr) 159 { 160 const struct ieee802154_addr *addr = lladdr; 161 u8 eui64[EUI64_ADDR_LEN]; 162 163 switch (addr->mode) { 164 case IEEE802154_ADDR_LONG: 165 ieee802154_le64_to_be64(eui64, &addr->extended_addr); 166 lowpan_iphc_uncompress_eui64_lladdr(ipaddr, eui64); 167 break; 168 case IEEE802154_ADDR_SHORT: 169 /* fe:80::ff:fe00:XXXX 170 * \__/ 171 * short_addr 172 * 173 * Universe/Local bit is zero. 174 */ 175 ipaddr->s6_addr[0] = 0xFE; 176 ipaddr->s6_addr[1] = 0x80; 177 ipaddr->s6_addr[11] = 0xFF; 178 ipaddr->s6_addr[12] = 0xFE; 179 ieee802154_le16_to_be16(&ipaddr->s6_addr16[7], 180 &addr->short_addr); 181 break; 182 default: 183 /* should never handled and filtered by 802154 6lowpan */ 184 WARN_ON_ONCE(1); 185 break; 186 } 187 } 188 189 static struct lowpan_iphc_ctx * lowpan_iphc_ctx_get_by_id(const struct net_device * dev,u8 id)190 lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id) 191 { 192 struct lowpan_iphc_ctx *ret = &lowpan_dev(dev)->ctx.table[id]; 193 194 if (!lowpan_iphc_ctx_is_active(ret)) 195 return NULL; 196 197 return ret; 198 } 199 200 static struct lowpan_iphc_ctx * lowpan_iphc_ctx_get_by_addr(const struct net_device * dev,const struct in6_addr * addr)201 lowpan_iphc_ctx_get_by_addr(const struct net_device *dev, 202 const struct in6_addr *addr) 203 { 204 struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; 205 struct lowpan_iphc_ctx *ret = NULL; 206 struct in6_addr addr_pfx; 207 u8 addr_plen; 208 int i; 209 210 for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { 211 /* Check if context is valid. A context that is not valid 212 * MUST NOT be used for compression. 213 */ 214 if (!lowpan_iphc_ctx_is_active(&table[i]) || 215 !lowpan_iphc_ctx_is_compression(&table[i])) 216 continue; 217 218 ipv6_addr_prefix(&addr_pfx, addr, table[i].plen); 219 220 /* if prefix len < 64, the remaining bits until 64th bit is 221 * zero. Otherwise we use table[i]->plen. 222 */ 223 if (table[i].plen < 64) 224 addr_plen = 64; 225 else 226 addr_plen = table[i].plen; 227 228 if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) { 229 /* remember first match */ 230 if (!ret) { 231 ret = &table[i]; 232 continue; 233 } 234 235 /* get the context with longest prefix len */ 236 if (table[i].plen > ret->plen) 237 ret = &table[i]; 238 } 239 } 240 241 return ret; 242 } 243 244 static struct lowpan_iphc_ctx * lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device * dev,const struct in6_addr * addr)245 lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev, 246 const struct in6_addr *addr) 247 { 248 struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table; 249 struct lowpan_iphc_ctx *ret = NULL; 250 struct in6_addr addr_mcast, network_pfx = {}; 251 int i; 252 253 /* init mcast address with */ 254 memcpy(&addr_mcast, addr, sizeof(*addr)); 255 256 for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) { 257 /* Check if context is valid. A context that is not valid 258 * MUST NOT be used for compression. 259 */ 260 if (!lowpan_iphc_ctx_is_active(&table[i]) || 261 !lowpan_iphc_ctx_is_compression(&table[i])) 262 continue; 263 264 /* setting plen */ 265 addr_mcast.s6_addr[3] = table[i].plen; 266 /* get network prefix to copy into multicast address */ 267 ipv6_addr_prefix(&network_pfx, &table[i].pfx, 268 table[i].plen); 269 /* setting network prefix */ 270 memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8); 271 272 if (ipv6_addr_equal(addr, &addr_mcast)) { 273 ret = &table[i]; 274 break; 275 } 276 } 277 278 return ret; 279 } 280 lowpan_iphc_uncompress_lladdr(const struct net_device * dev,struct in6_addr * ipaddr,const void * lladdr)281 static void lowpan_iphc_uncompress_lladdr(const struct net_device *dev, 282 struct in6_addr *ipaddr, 283 const void *lladdr) 284 { 285 switch (dev->addr_len) { 286 case ETH_ALEN: 287 lowpan_iphc_uncompress_eui48_lladdr(ipaddr, lladdr); 288 break; 289 case EUI64_ADDR_LEN: 290 lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 291 break; 292 default: 293 WARN_ON_ONCE(1); 294 break; 295 } 296 } 297 298 /* Uncompress address function for source and 299 * destination address(non-multicast). 300 * 301 * address_mode is the masked value for sam or dam value 302 */ lowpan_iphc_uncompress_addr(struct sk_buff * skb,const struct net_device * dev,struct in6_addr * ipaddr,u8 address_mode,const void * lladdr)303 static int lowpan_iphc_uncompress_addr(struct sk_buff *skb, 304 const struct net_device *dev, 305 struct in6_addr *ipaddr, 306 u8 address_mode, const void *lladdr) 307 { 308 bool fail; 309 310 switch (address_mode) { 311 /* SAM and DAM are the same here */ 312 case LOWPAN_IPHC_DAM_00: 313 /* for global link addresses */ 314 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 315 break; 316 case LOWPAN_IPHC_SAM_01: 317 case LOWPAN_IPHC_DAM_01: 318 /* fe:80::XXXX:XXXX:XXXX:XXXX */ 319 ipaddr->s6_addr[0] = 0xFE; 320 ipaddr->s6_addr[1] = 0x80; 321 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 322 break; 323 case LOWPAN_IPHC_SAM_10: 324 case LOWPAN_IPHC_DAM_10: 325 /* fe:80::ff:fe00:XXXX */ 326 ipaddr->s6_addr[0] = 0xFE; 327 ipaddr->s6_addr[1] = 0x80; 328 ipaddr->s6_addr[11] = 0xFF; 329 ipaddr->s6_addr[12] = 0xFE; 330 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 331 break; 332 case LOWPAN_IPHC_SAM_11: 333 case LOWPAN_IPHC_DAM_11: 334 fail = false; 335 switch (lowpan_dev(dev)->lltype) { 336 case LOWPAN_LLTYPE_IEEE802154: 337 lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 338 break; 339 default: 340 lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr); 341 break; 342 } 343 break; 344 default: 345 pr_debug("Invalid address mode value: 0x%x\n", address_mode); 346 return -EINVAL; 347 } 348 349 if (fail) { 350 pr_debug("Failed to fetch skb data\n"); 351 return -EIO; 352 } 353 354 raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 355 ipaddr->s6_addr, 16); 356 357 return 0; 358 } 359 360 /* Uncompress address function for source context 361 * based address(non-multicast). 362 */ lowpan_iphc_uncompress_ctx_addr(struct sk_buff * skb,const struct net_device * dev,const struct lowpan_iphc_ctx * ctx,struct in6_addr * ipaddr,u8 address_mode,const void * lladdr)363 static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb, 364 const struct net_device *dev, 365 const struct lowpan_iphc_ctx *ctx, 366 struct in6_addr *ipaddr, 367 u8 address_mode, const void *lladdr) 368 { 369 bool fail; 370 371 switch (address_mode) { 372 /* SAM and DAM are the same here */ 373 case LOWPAN_IPHC_DAM_00: 374 fail = false; 375 /* SAM_00 -> unspec address :: 376 * Do nothing, address is already :: 377 * 378 * DAM 00 -> reserved should never occur. 379 */ 380 break; 381 case LOWPAN_IPHC_SAM_01: 382 case LOWPAN_IPHC_DAM_01: 383 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 384 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 385 break; 386 case LOWPAN_IPHC_SAM_10: 387 case LOWPAN_IPHC_DAM_10: 388 ipaddr->s6_addr[11] = 0xFF; 389 ipaddr->s6_addr[12] = 0xFE; 390 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 391 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 392 break; 393 case LOWPAN_IPHC_SAM_11: 394 case LOWPAN_IPHC_DAM_11: 395 fail = false; 396 switch (lowpan_dev(dev)->lltype) { 397 case LOWPAN_LLTYPE_IEEE802154: 398 lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 399 break; 400 default: 401 lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr); 402 break; 403 } 404 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 405 break; 406 default: 407 pr_debug("Invalid sam value: 0x%x\n", address_mode); 408 return -EINVAL; 409 } 410 411 if (fail) { 412 pr_debug("Failed to fetch skb data\n"); 413 return -EIO; 414 } 415 416 raw_dump_inline(NULL, 417 "Reconstructed context based ipv6 src addr is", 418 ipaddr->s6_addr, 16); 419 420 return 0; 421 } 422 423 /* Uncompress function for multicast destination address, 424 * when M bit is set. 425 */ lowpan_uncompress_multicast_daddr(struct sk_buff * skb,struct in6_addr * ipaddr,u8 address_mode)426 static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 427 struct in6_addr *ipaddr, 428 u8 address_mode) 429 { 430 bool fail; 431 432 switch (address_mode) { 433 case LOWPAN_IPHC_DAM_00: 434 /* 00: 128 bits. The full address 435 * is carried in-line. 436 */ 437 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 438 break; 439 case LOWPAN_IPHC_DAM_01: 440 /* 01: 48 bits. The address takes 441 * the form ffXX::00XX:XXXX:XXXX. 442 */ 443 ipaddr->s6_addr[0] = 0xFF; 444 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 445 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 446 break; 447 case LOWPAN_IPHC_DAM_10: 448 /* 10: 32 bits. The address takes 449 * the form ffXX::00XX:XXXX. 450 */ 451 ipaddr->s6_addr[0] = 0xFF; 452 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 453 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 454 break; 455 case LOWPAN_IPHC_DAM_11: 456 /* 11: 8 bits. The address takes 457 * the form ff02::00XX. 458 */ 459 ipaddr->s6_addr[0] = 0xFF; 460 ipaddr->s6_addr[1] = 0x02; 461 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 462 break; 463 default: 464 pr_debug("DAM value has a wrong value: 0x%x\n", address_mode); 465 return -EINVAL; 466 } 467 468 if (fail) { 469 pr_debug("Failed to fetch skb data\n"); 470 return -EIO; 471 } 472 473 raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 474 ipaddr->s6_addr, 16); 475 476 return 0; 477 } 478 lowpan_uncompress_multicast_ctx_daddr(struct sk_buff * skb,struct lowpan_iphc_ctx * ctx,struct in6_addr * ipaddr,u8 address_mode)479 static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb, 480 struct lowpan_iphc_ctx *ctx, 481 struct in6_addr *ipaddr, 482 u8 address_mode) 483 { 484 struct in6_addr network_pfx = {}; 485 bool fail; 486 487 ipaddr->s6_addr[0] = 0xFF; 488 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2); 489 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4); 490 if (fail) 491 return -EIO; 492 493 /* take prefix_len and network prefix from the context */ 494 ipaddr->s6_addr[3] = ctx->plen; 495 /* get network prefix to copy into multicast address */ 496 ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen); 497 /* setting network prefix */ 498 memcpy(&ipaddr->s6_addr[4], &network_pfx, 8); 499 500 return 0; 501 } 502 503 /* get the ecn values from iphc tf format and set it to ipv6hdr */ lowpan_iphc_tf_set_ecn(struct ipv6hdr * hdr,const u8 * tf)504 static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf) 505 { 506 /* get the two higher bits which is ecn */ 507 u8 ecn = tf[0] & 0xc0; 508 509 /* ECN takes 0x30 in hdr->flow_lbl[0] */ 510 hdr->flow_lbl[0] |= (ecn >> 2); 511 } 512 513 /* get the dscp values from iphc tf format and set it to ipv6hdr */ lowpan_iphc_tf_set_dscp(struct ipv6hdr * hdr,const u8 * tf)514 static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf) 515 { 516 /* DSCP is at place after ECN */ 517 u8 dscp = tf[0] & 0x3f; 518 519 /* The four highest bits need to be set at hdr->priority */ 520 hdr->priority |= ((dscp & 0x3c) >> 2); 521 /* The two lower bits is part of hdr->flow_lbl[0] */ 522 hdr->flow_lbl[0] |= ((dscp & 0x03) << 6); 523 } 524 525 /* get the flow label values from iphc tf format and set it to ipv6hdr */ lowpan_iphc_tf_set_lbl(struct ipv6hdr * hdr,const u8 * lbl)526 static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl) 527 { 528 /* flow label is always some array started with lower nibble of 529 * flow_lbl[0] and followed with two bytes afterwards. Inside inline 530 * data the flow_lbl position can be different, which will be handled 531 * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit 532 * shifted, the different lbl pointer will handle that. 533 * 534 * The flow label will started at lower nibble of flow_lbl[0], the 535 * higher nibbles are part of DSCP + ECN. 536 */ 537 hdr->flow_lbl[0] |= lbl[0] & 0x0f; 538 memcpy(&hdr->flow_lbl[1], &lbl[1], 2); 539 } 540 541 /* lowpan_iphc_tf_decompress - decompress the traffic class. 542 * This function will return zero on success, a value lower than zero if 543 * failed. 544 */ lowpan_iphc_tf_decompress(struct sk_buff * skb,struct ipv6hdr * hdr,u8 val)545 static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr, 546 u8 val) 547 { 548 u8 tf[4]; 549 550 /* Traffic Class and Flow Label */ 551 switch (val) { 552 case LOWPAN_IPHC_TF_00: 553 /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */ 554 if (lowpan_fetch_skb(skb, tf, 4)) 555 return -EINVAL; 556 557 /* 1 2 3 558 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 559 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 560 * |ECN| DSCP | rsv | Flow Label | 561 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 562 */ 563 lowpan_iphc_tf_set_ecn(hdr, tf); 564 lowpan_iphc_tf_set_dscp(hdr, tf); 565 lowpan_iphc_tf_set_lbl(hdr, &tf[1]); 566 break; 567 case LOWPAN_IPHC_TF_01: 568 /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */ 569 if (lowpan_fetch_skb(skb, tf, 3)) 570 return -EINVAL; 571 572 /* 1 2 573 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 574 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 575 * |ECN|rsv| Flow Label | 576 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 577 */ 578 lowpan_iphc_tf_set_ecn(hdr, tf); 579 lowpan_iphc_tf_set_lbl(hdr, &tf[0]); 580 break; 581 case LOWPAN_IPHC_TF_10: 582 /* ECN + DSCP (1 byte), Flow Label is elided. */ 583 if (lowpan_fetch_skb(skb, tf, 1)) 584 return -EINVAL; 585 586 /* 0 1 2 3 4 5 6 7 587 * +-+-+-+-+-+-+-+-+ 588 * |ECN| DSCP | 589 * +-+-+-+-+-+-+-+-+ 590 */ 591 lowpan_iphc_tf_set_ecn(hdr, tf); 592 lowpan_iphc_tf_set_dscp(hdr, tf); 593 break; 594 case LOWPAN_IPHC_TF_11: 595 /* Traffic Class and Flow Label are elided */ 596 break; 597 default: 598 WARN_ON_ONCE(1); 599 return -EINVAL; 600 } 601 602 return 0; 603 } 604 605 /* TTL uncompression values */ 606 static const u8 lowpan_ttl_values[] = { 607 [LOWPAN_IPHC_HLIM_01] = 1, 608 [LOWPAN_IPHC_HLIM_10] = 64, 609 [LOWPAN_IPHC_HLIM_11] = 255, 610 }; 611 lowpan_header_decompress(struct sk_buff * skb,const struct net_device * dev,const void * daddr,const void * saddr)612 int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev, 613 const void *daddr, const void *saddr) 614 { 615 struct ipv6hdr hdr = {}; 616 struct lowpan_iphc_ctx *ci; 617 u8 iphc0, iphc1, cid = 0; 618 int err; 619 620 raw_dump_table(__func__, "raw skb data dump uncompressed", 621 skb->data, skb->len); 622 623 if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) || 624 lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1))) 625 return -EINVAL; 626 627 hdr.version = 6; 628 629 /* default CID = 0, another if the CID flag is set */ 630 if (iphc1 & LOWPAN_IPHC_CID) { 631 if (lowpan_fetch_skb(skb, &cid, sizeof(cid))) 632 return -EINVAL; 633 } 634 635 err = lowpan_iphc_tf_decompress(skb, &hdr, 636 iphc0 & LOWPAN_IPHC_TF_MASK); 637 if (err < 0) 638 return err; 639 640 /* Next Header */ 641 if (!(iphc0 & LOWPAN_IPHC_NH)) { 642 /* Next header is carried inline */ 643 if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 644 return -EINVAL; 645 646 pr_debug("NH flag is set, next header carried inline: %02x\n", 647 hdr.nexthdr); 648 } 649 650 /* Hop Limit */ 651 if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) { 652 hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK]; 653 } else { 654 if (lowpan_fetch_skb(skb, &hdr.hop_limit, 655 sizeof(hdr.hop_limit))) 656 return -EINVAL; 657 } 658 659 if (iphc1 & LOWPAN_IPHC_SAC) { 660 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 661 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid)); 662 if (!ci) { 663 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 664 return -EINVAL; 665 } 666 667 pr_debug("SAC bit is set. Handle context based source address.\n"); 668 err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr, 669 iphc1 & LOWPAN_IPHC_SAM_MASK, 670 saddr); 671 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 672 } else { 673 /* Source address uncompression */ 674 pr_debug("source address stateless compression\n"); 675 err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr, 676 iphc1 & LOWPAN_IPHC_SAM_MASK, 677 saddr); 678 } 679 680 /* Check on error of previous branch */ 681 if (err) 682 return -EINVAL; 683 684 switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) { 685 case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC: 686 skb->pkt_type = PACKET_BROADCAST; 687 688 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 689 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 690 if (!ci) { 691 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 692 return -EINVAL; 693 } 694 695 /* multicast with context */ 696 pr_debug("dest: context-based mcast compression\n"); 697 err = lowpan_uncompress_multicast_ctx_daddr(skb, ci, 698 &hdr.daddr, 699 iphc1 & LOWPAN_IPHC_DAM_MASK); 700 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 701 break; 702 case LOWPAN_IPHC_M: 703 skb->pkt_type = PACKET_BROADCAST; 704 705 /* multicast */ 706 err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, 707 iphc1 & LOWPAN_IPHC_DAM_MASK); 708 break; 709 case LOWPAN_IPHC_DAC: 710 skb->pkt_type = PACKET_HOST; 711 712 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 713 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 714 if (!ci) { 715 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 716 return -EINVAL; 717 } 718 719 /* Destination address context based uncompression */ 720 pr_debug("DAC bit is set. Handle context based destination address.\n"); 721 err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr, 722 iphc1 & LOWPAN_IPHC_DAM_MASK, 723 daddr); 724 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 725 break; 726 default: 727 skb->pkt_type = PACKET_HOST; 728 729 err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr, 730 iphc1 & LOWPAN_IPHC_DAM_MASK, 731 daddr); 732 pr_debug("dest: stateless compression mode %d dest %pI6c\n", 733 iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr); 734 break; 735 } 736 737 if (err) 738 return -EINVAL; 739 740 /* Next header data uncompression */ 741 if (iphc0 & LOWPAN_IPHC_NH) { 742 err = lowpan_nhc_do_uncompression(skb, dev, &hdr); 743 if (err < 0) 744 return err; 745 } else { 746 err = skb_cow(skb, sizeof(hdr)); 747 if (unlikely(err)) 748 return err; 749 } 750 751 switch (lowpan_dev(dev)->lltype) { 752 case LOWPAN_LLTYPE_IEEE802154: 753 if (lowpan_802154_cb(skb)->d_size) 754 hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size - 755 sizeof(struct ipv6hdr)); 756 else 757 hdr.payload_len = htons(skb->len); 758 break; 759 default: 760 hdr.payload_len = htons(skb->len); 761 break; 762 } 763 764 pr_debug("skb headroom size = %d, data length = %d\n", 765 skb_headroom(skb), skb->len); 766 767 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t" 768 "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 769 hdr.version, ntohs(hdr.payload_len), hdr.nexthdr, 770 hdr.hop_limit, &hdr.daddr); 771 772 skb_push(skb, sizeof(hdr)); 773 skb_reset_mac_header(skb); 774 skb_reset_network_header(skb); 775 skb_copy_to_linear_data(skb, &hdr, sizeof(hdr)); 776 777 raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr)); 778 779 return 0; 780 } 781 EXPORT_SYMBOL_GPL(lowpan_header_decompress); 782 783 static const u8 lowpan_iphc_dam_to_sam_value[] = { 784 [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00, 785 [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01, 786 [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10, 787 [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11, 788 }; 789 790 static inline bool lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr * ipaddr,const struct lowpan_iphc_ctx * ctx,const void * lladdr)791 lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr, 792 const struct lowpan_iphc_ctx *ctx, 793 const void *lladdr) 794 { 795 const struct ieee802154_addr *addr = lladdr; 796 unsigned char extended_addr[EUI64_ADDR_LEN]; 797 bool lladdr_compress = false; 798 struct in6_addr tmp = {}; 799 800 switch (addr->mode) { 801 case IEEE802154_ADDR_LONG: 802 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 803 /* check for SAM/DAM = 11 */ 804 memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN); 805 /* second bit-flip (Universe/Local) is done according RFC2464 */ 806 tmp.s6_addr[8] ^= 0x02; 807 /* context information are always used */ 808 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 809 if (ipv6_addr_equal(&tmp, ipaddr)) 810 lladdr_compress = true; 811 break; 812 case IEEE802154_ADDR_SHORT: 813 tmp.s6_addr[11] = 0xFF; 814 tmp.s6_addr[12] = 0xFE; 815 ieee802154_le16_to_be16(&tmp.s6_addr16[7], 816 &addr->short_addr); 817 /* context information are always used */ 818 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 819 if (ipv6_addr_equal(&tmp, ipaddr)) 820 lladdr_compress = true; 821 break; 822 default: 823 /* should never handled and filtered by 802154 6lowpan */ 824 WARN_ON_ONCE(1); 825 break; 826 } 827 828 return lladdr_compress; 829 } 830 lowpan_iphc_addr_equal(const struct net_device * dev,const struct lowpan_iphc_ctx * ctx,const struct in6_addr * ipaddr,const void * lladdr)831 static bool lowpan_iphc_addr_equal(const struct net_device *dev, 832 const struct lowpan_iphc_ctx *ctx, 833 const struct in6_addr *ipaddr, 834 const void *lladdr) 835 { 836 struct in6_addr tmp = {}; 837 838 lowpan_iphc_uncompress_lladdr(dev, &tmp, lladdr); 839 840 if (ctx) 841 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 842 843 return ipv6_addr_equal(&tmp, ipaddr); 844 } 845 lowpan_compress_ctx_addr(u8 ** hc_ptr,const struct net_device * dev,const struct in6_addr * ipaddr,const struct lowpan_iphc_ctx * ctx,const unsigned char * lladdr,bool sam)846 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev, 847 const struct in6_addr *ipaddr, 848 const struct lowpan_iphc_ctx *ctx, 849 const unsigned char *lladdr, bool sam) 850 { 851 struct in6_addr tmp; 852 u8 dam; 853 854 switch (lowpan_dev(dev)->lltype) { 855 case LOWPAN_LLTYPE_IEEE802154: 856 if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx, 857 lladdr)) { 858 dam = LOWPAN_IPHC_DAM_11; 859 goto out; 860 } 861 break; 862 default: 863 if (lowpan_iphc_addr_equal(dev, ctx, ipaddr, lladdr)) { 864 dam = LOWPAN_IPHC_DAM_11; 865 goto out; 866 } 867 break; 868 } 869 870 memset(&tmp, 0, sizeof(tmp)); 871 /* check for SAM/DAM = 10 */ 872 tmp.s6_addr[11] = 0xFF; 873 tmp.s6_addr[12] = 0xFE; 874 memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2); 875 /* context information are always used */ 876 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 877 if (ipv6_addr_equal(&tmp, ipaddr)) { 878 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2); 879 dam = LOWPAN_IPHC_DAM_10; 880 goto out; 881 } 882 883 memset(&tmp, 0, sizeof(tmp)); 884 /* check for SAM/DAM = 01, should always match */ 885 memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8); 886 /* context information are always used */ 887 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 888 if (ipv6_addr_equal(&tmp, ipaddr)) { 889 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8); 890 dam = LOWPAN_IPHC_DAM_01; 891 goto out; 892 } 893 894 WARN_ONCE(1, "context found but no address mode matched\n"); 895 return LOWPAN_IPHC_DAM_00; 896 out: 897 898 if (sam) 899 return lowpan_iphc_dam_to_sam_value[dam]; 900 else 901 return dam; 902 } 903 904 static inline bool lowpan_iphc_compress_802154_lladdr(const struct in6_addr * ipaddr,const void * lladdr)905 lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr, 906 const void *lladdr) 907 { 908 const struct ieee802154_addr *addr = lladdr; 909 unsigned char extended_addr[EUI64_ADDR_LEN]; 910 bool lladdr_compress = false; 911 struct in6_addr tmp = {}; 912 913 switch (addr->mode) { 914 case IEEE802154_ADDR_LONG: 915 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 916 if (is_addr_mac_addr_based(ipaddr, extended_addr)) 917 lladdr_compress = true; 918 break; 919 case IEEE802154_ADDR_SHORT: 920 /* fe:80::ff:fe00:XXXX 921 * \__/ 922 * short_addr 923 * 924 * Universe/Local bit is zero. 925 */ 926 tmp.s6_addr[0] = 0xFE; 927 tmp.s6_addr[1] = 0x80; 928 tmp.s6_addr[11] = 0xFF; 929 tmp.s6_addr[12] = 0xFE; 930 ieee802154_le16_to_be16(&tmp.s6_addr16[7], 931 &addr->short_addr); 932 if (ipv6_addr_equal(&tmp, ipaddr)) 933 lladdr_compress = true; 934 break; 935 default: 936 /* should never handled and filtered by 802154 6lowpan */ 937 WARN_ON_ONCE(1); 938 break; 939 } 940 941 return lladdr_compress; 942 } 943 lowpan_compress_addr_64(u8 ** hc_ptr,const struct net_device * dev,const struct in6_addr * ipaddr,const unsigned char * lladdr,bool sam)944 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev, 945 const struct in6_addr *ipaddr, 946 const unsigned char *lladdr, bool sam) 947 { 948 u8 dam = LOWPAN_IPHC_DAM_01; 949 950 switch (lowpan_dev(dev)->lltype) { 951 case LOWPAN_LLTYPE_IEEE802154: 952 if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) { 953 dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ 954 pr_debug("address compression 0 bits\n"); 955 goto out; 956 } 957 break; 958 default: 959 if (lowpan_iphc_addr_equal(dev, NULL, ipaddr, lladdr)) { 960 dam = LOWPAN_IPHC_DAM_11; 961 pr_debug("address compression 0 bits\n"); 962 goto out; 963 } 964 965 break; 966 } 967 968 if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 969 /* compress IID to 16 bits xxxx::XXXX */ 970 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 971 dam = LOWPAN_IPHC_DAM_10; /* 16-bits */ 972 raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 973 *hc_ptr - 2, 2); 974 goto out; 975 } 976 977 /* do not compress IID => xxxx::IID */ 978 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 979 raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 980 *hc_ptr - 8, 8); 981 982 out: 983 984 if (sam) 985 return lowpan_iphc_dam_to_sam_value[dam]; 986 else 987 return dam; 988 } 989 990 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */ lowpan_iphc_get_tc(const struct ipv6hdr * hdr)991 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr) 992 { 993 u8 dscp, ecn; 994 995 /* hdr->priority contains the higher bits of dscp, lower are part of 996 * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr. 997 */ 998 dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6); 999 /* ECN is at the two lower bits from first nibble of flow_lbl[0] */ 1000 ecn = (hdr->flow_lbl[0] & 0x30); 1001 /* for pretty debug output, also shift ecn to get the ecn value */ 1002 pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp); 1003 /* ECN is at 0x30 now, shift it to have ECN + DCSP */ 1004 return (ecn << 2) | dscp; 1005 } 1006 1007 /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */ lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr * hdr)1008 static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr) 1009 { 1010 return ((!(hdr->flow_lbl[0] & 0x0f)) && 1011 !hdr->flow_lbl[1] && !hdr->flow_lbl[2]); 1012 } 1013 1014 /* lowpan_iphc_tf_compress - compress the traffic class which is set by 1015 * ipv6hdr. Return the corresponding format identifier which is used. 1016 */ lowpan_iphc_tf_compress(u8 ** hc_ptr,const struct ipv6hdr * hdr)1017 static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr) 1018 { 1019 /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */ 1020 u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val; 1021 1022 /* printout the traffic class in hc format */ 1023 pr_debug("tc 0x%02x\n", tc); 1024 1025 if (lowpan_iphc_is_flow_lbl_zero(hdr)) { 1026 if (!tc) { 1027 /* 11: Traffic Class and Flow Label are elided. */ 1028 val = LOWPAN_IPHC_TF_11; 1029 } else { 1030 /* 10: ECN + DSCP (1 byte), Flow Label is elided. 1031 * 1032 * 0 1 2 3 4 5 6 7 1033 * +-+-+-+-+-+-+-+-+ 1034 * |ECN| DSCP | 1035 * +-+-+-+-+-+-+-+-+ 1036 */ 1037 lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc)); 1038 val = LOWPAN_IPHC_TF_10; 1039 } 1040 } else { 1041 /* check if dscp is zero, it's after the first two bit */ 1042 if (!(tc & 0x3f)) { 1043 /* 01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 1044 * 1045 * 1 2 1046 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 1047 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1048 * |ECN|rsv| Flow Label | 1049 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1050 */ 1051 memcpy(&tf[0], &hdr->flow_lbl[0], 3); 1052 /* zero the highest 4-bits, contains DCSP + ECN */ 1053 tf[0] &= ~0xf0; 1054 /* set ECN */ 1055 tf[0] |= (tc & 0xc0); 1056 1057 lowpan_push_hc_data(hc_ptr, tf, 3); 1058 val = LOWPAN_IPHC_TF_01; 1059 } else { 1060 /* 00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 1061 * 1062 * 1 2 3 1063 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 1064 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1065 * |ECN| DSCP | rsv | Flow Label | 1066 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1067 */ 1068 memcpy(&tf[0], &tc, sizeof(tc)); 1069 /* highest nibble of flow_lbl[0] is part of DSCP + ECN 1070 * which will be the 4-bit pad and will be filled with 1071 * zeros afterwards. 1072 */ 1073 memcpy(&tf[1], &hdr->flow_lbl[0], 3); 1074 /* zero the 4-bit pad, which is reserved */ 1075 tf[1] &= ~0xf0; 1076 1077 lowpan_push_hc_data(hc_ptr, tf, 4); 1078 val = LOWPAN_IPHC_TF_00; 1079 } 1080 } 1081 1082 return val; 1083 } 1084 lowpan_iphc_mcast_ctx_addr_compress(u8 ** hc_ptr,const struct lowpan_iphc_ctx * ctx,const struct in6_addr * ipaddr)1085 static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr, 1086 const struct lowpan_iphc_ctx *ctx, 1087 const struct in6_addr *ipaddr) 1088 { 1089 u8 data[6]; 1090 1091 /* flags/scope, reserved (RIID) */ 1092 memcpy(data, &ipaddr->s6_addr[1], 2); 1093 /* group ID */ 1094 memcpy(&data[1], &ipaddr->s6_addr[11], 4); 1095 lowpan_push_hc_data(hc_ptr, data, 6); 1096 1097 return LOWPAN_IPHC_DAM_00; 1098 } 1099 lowpan_iphc_mcast_addr_compress(u8 ** hc_ptr,const struct in6_addr * ipaddr)1100 static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr, 1101 const struct in6_addr *ipaddr) 1102 { 1103 u8 val; 1104 1105 if (lowpan_is_mcast_addr_compressable8(ipaddr)) { 1106 pr_debug("compressed to 1 octet\n"); 1107 /* use last byte */ 1108 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1); 1109 val = LOWPAN_IPHC_DAM_11; 1110 } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) { 1111 pr_debug("compressed to 4 octets\n"); 1112 /* second byte + the last three */ 1113 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 1114 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3); 1115 val = LOWPAN_IPHC_DAM_10; 1116 } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) { 1117 pr_debug("compressed to 6 octets\n"); 1118 /* second byte + the last five */ 1119 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 1120 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5); 1121 val = LOWPAN_IPHC_DAM_01; 1122 } else { 1123 pr_debug("using full address\n"); 1124 lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16); 1125 val = LOWPAN_IPHC_DAM_00; 1126 } 1127 1128 return val; 1129 } 1130 lowpan_header_compress(struct sk_buff * skb,const struct net_device * dev,const void * daddr,const void * saddr)1131 int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev, 1132 const void *daddr, const void *saddr) 1133 { 1134 u8 iphc0, iphc1, *hc_ptr, cid = 0; 1135 struct ipv6hdr *hdr; 1136 u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {}; 1137 struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry; 1138 int ret, ipv6_daddr_type, ipv6_saddr_type; 1139 1140 if (skb->protocol != htons(ETH_P_IPV6)) 1141 return -EINVAL; 1142 1143 hdr = ipv6_hdr(skb); 1144 hc_ptr = head + 2; 1145 1146 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 1147 "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 1148 hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 1149 hdr->hop_limit, &hdr->daddr); 1150 1151 raw_dump_table(__func__, "raw skb network header dump", 1152 skb_network_header(skb), sizeof(struct ipv6hdr)); 1153 1154 /* As we copy some bit-length fields, in the IPHC encoding bytes, 1155 * we sometimes use |= 1156 * If the field is 0, and the current bit value in memory is 1, 1157 * this does not work. We therefore reset the IPHC encoding here 1158 */ 1159 iphc0 = LOWPAN_DISPATCH_IPHC; 1160 iphc1 = 0; 1161 1162 raw_dump_table(__func__, "sending raw skb network uncompressed packet", 1163 skb->data, skb->len); 1164 1165 ipv6_daddr_type = ipv6_addr_type(&hdr->daddr); 1166 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 1167 if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) 1168 dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr); 1169 else 1170 dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr); 1171 if (dci) { 1172 memcpy(&dci_entry, dci, sizeof(*dci)); 1173 cid |= dci->id; 1174 } 1175 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 1176 1177 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 1178 sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr); 1179 if (sci) { 1180 memcpy(&sci_entry, sci, sizeof(*sci)); 1181 cid |= (sci->id << 4); 1182 } 1183 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 1184 1185 /* if cid is zero it will be compressed */ 1186 if (cid) { 1187 iphc1 |= LOWPAN_IPHC_CID; 1188 lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid)); 1189 } 1190 1191 /* Traffic Class, Flow Label compression */ 1192 iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr); 1193 1194 /* NOTE: payload length is always compressed */ 1195 1196 /* Check if we provide the nhc format for nexthdr and compression 1197 * functionality. If not nexthdr is handled inline and not compressed. 1198 */ 1199 ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr); 1200 if (ret == -ENOENT) 1201 lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, 1202 sizeof(hdr->nexthdr)); 1203 else 1204 iphc0 |= LOWPAN_IPHC_NH; 1205 1206 /* Hop limit 1207 * if 1: compress, encoding is 01 1208 * if 64: compress, encoding is 10 1209 * if 255: compress, encoding is 11 1210 * else do not compress 1211 */ 1212 switch (hdr->hop_limit) { 1213 case 1: 1214 iphc0 |= LOWPAN_IPHC_HLIM_01; 1215 break; 1216 case 64: 1217 iphc0 |= LOWPAN_IPHC_HLIM_10; 1218 break; 1219 case 255: 1220 iphc0 |= LOWPAN_IPHC_HLIM_11; 1221 break; 1222 default: 1223 lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 1224 sizeof(hdr->hop_limit)); 1225 } 1226 1227 ipv6_saddr_type = ipv6_addr_type(&hdr->saddr); 1228 /* source address compression */ 1229 if (ipv6_saddr_type == IPV6_ADDR_ANY) { 1230 pr_debug("source address is unspecified, setting SAC\n"); 1231 iphc1 |= LOWPAN_IPHC_SAC; 1232 } else { 1233 if (sci) { 1234 iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1235 &hdr->saddr, 1236 &sci_entry, saddr, 1237 true); 1238 iphc1 |= LOWPAN_IPHC_SAC; 1239 } else { 1240 if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL && 1241 lowpan_is_linklocal_zero_padded(hdr->saddr)) { 1242 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 1243 &hdr->saddr, 1244 saddr, true); 1245 pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 1246 &hdr->saddr, iphc1); 1247 } else { 1248 pr_debug("send the full source address\n"); 1249 lowpan_push_hc_data(&hc_ptr, 1250 hdr->saddr.s6_addr, 16); 1251 } 1252 } 1253 } 1254 1255 /* destination address compression */ 1256 if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) { 1257 pr_debug("destination address is multicast: "); 1258 iphc1 |= LOWPAN_IPHC_M; 1259 if (dci) { 1260 iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr, 1261 &dci_entry, 1262 &hdr->daddr); 1263 iphc1 |= LOWPAN_IPHC_DAC; 1264 } else { 1265 iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr, 1266 &hdr->daddr); 1267 } 1268 } else { 1269 if (dci) { 1270 iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1271 &hdr->daddr, 1272 &dci_entry, daddr, 1273 false); 1274 iphc1 |= LOWPAN_IPHC_DAC; 1275 } else { 1276 if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL && 1277 lowpan_is_linklocal_zero_padded(hdr->daddr)) { 1278 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 1279 &hdr->daddr, 1280 daddr, false); 1281 pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n", 1282 &hdr->daddr, iphc1); 1283 } else { 1284 pr_debug("dest address unicast %pI6c\n", 1285 &hdr->daddr); 1286 lowpan_push_hc_data(&hc_ptr, 1287 hdr->daddr.s6_addr, 16); 1288 } 1289 } 1290 } 1291 1292 /* next header compression */ 1293 if (iphc0 & LOWPAN_IPHC_NH) { 1294 ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); 1295 if (ret < 0) 1296 return ret; 1297 } 1298 1299 head[0] = iphc0; 1300 head[1] = iphc1; 1301 1302 skb_pull(skb, sizeof(struct ipv6hdr)); 1303 skb_reset_transport_header(skb); 1304 memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 1305 skb_reset_network_header(skb); 1306 1307 pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 1308 1309 raw_dump_table(__func__, "raw skb data dump compressed", 1310 skb->data, skb->len); 1311 return 0; 1312 } 1313 EXPORT_SYMBOL_GPL(lowpan_header_compress); 1314