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