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