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 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 * 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 * 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 * 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 281 /* Uncompress address function for source and 282 * destination address(non-multicast). 283 * 284 * address_mode is the masked value for sam or dam value 285 */ 286 static int lowpan_iphc_uncompress_addr(struct sk_buff *skb, 287 const struct net_device *dev, 288 struct in6_addr *ipaddr, 289 u8 address_mode, const void *lladdr) 290 { 291 bool fail; 292 293 switch (address_mode) { 294 /* SAM and DAM are the same here */ 295 case LOWPAN_IPHC_DAM_00: 296 /* for global link addresses */ 297 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 298 break; 299 case LOWPAN_IPHC_SAM_01: 300 case LOWPAN_IPHC_DAM_01: 301 /* fe:80::XXXX:XXXX:XXXX:XXXX */ 302 ipaddr->s6_addr[0] = 0xFE; 303 ipaddr->s6_addr[1] = 0x80; 304 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 305 break; 306 case LOWPAN_IPHC_SAM_10: 307 case LOWPAN_IPHC_DAM_10: 308 /* fe:80::ff:fe00:XXXX */ 309 ipaddr->s6_addr[0] = 0xFE; 310 ipaddr->s6_addr[1] = 0x80; 311 ipaddr->s6_addr[11] = 0xFF; 312 ipaddr->s6_addr[12] = 0xFE; 313 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 314 break; 315 case LOWPAN_IPHC_SAM_11: 316 case LOWPAN_IPHC_DAM_11: 317 fail = false; 318 switch (lowpan_dev(dev)->lltype) { 319 case LOWPAN_LLTYPE_IEEE802154: 320 lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 321 break; 322 default: 323 lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 324 break; 325 } 326 break; 327 default: 328 pr_debug("Invalid address mode value: 0x%x\n", address_mode); 329 return -EINVAL; 330 } 331 332 if (fail) { 333 pr_debug("Failed to fetch skb data\n"); 334 return -EIO; 335 } 336 337 raw_dump_inline(NULL, "Reconstructed ipv6 addr is", 338 ipaddr->s6_addr, 16); 339 340 return 0; 341 } 342 343 /* Uncompress address function for source context 344 * based address(non-multicast). 345 */ 346 static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb, 347 const struct net_device *dev, 348 const struct lowpan_iphc_ctx *ctx, 349 struct in6_addr *ipaddr, 350 u8 address_mode, const void *lladdr) 351 { 352 bool fail; 353 354 switch (address_mode) { 355 /* SAM and DAM are the same here */ 356 case LOWPAN_IPHC_DAM_00: 357 fail = false; 358 /* SAM_00 -> unspec address :: 359 * Do nothing, address is already :: 360 * 361 * DAM 00 -> reserved should never occur. 362 */ 363 break; 364 case LOWPAN_IPHC_SAM_01: 365 case LOWPAN_IPHC_DAM_01: 366 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); 367 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 368 break; 369 case LOWPAN_IPHC_SAM_10: 370 case LOWPAN_IPHC_DAM_10: 371 ipaddr->s6_addr[11] = 0xFF; 372 ipaddr->s6_addr[12] = 0xFE; 373 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); 374 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 375 break; 376 case LOWPAN_IPHC_SAM_11: 377 case LOWPAN_IPHC_DAM_11: 378 fail = false; 379 switch (lowpan_dev(dev)->lltype) { 380 case LOWPAN_LLTYPE_IEEE802154: 381 lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr); 382 break; 383 default: 384 lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr); 385 break; 386 } 387 ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen); 388 break; 389 default: 390 pr_debug("Invalid sam value: 0x%x\n", address_mode); 391 return -EINVAL; 392 } 393 394 if (fail) { 395 pr_debug("Failed to fetch skb data\n"); 396 return -EIO; 397 } 398 399 raw_dump_inline(NULL, 400 "Reconstructed context based ipv6 src addr is", 401 ipaddr->s6_addr, 16); 402 403 return 0; 404 } 405 406 /* Uncompress function for multicast destination address, 407 * when M bit is set. 408 */ 409 static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, 410 struct in6_addr *ipaddr, 411 u8 address_mode) 412 { 413 bool fail; 414 415 switch (address_mode) { 416 case LOWPAN_IPHC_DAM_00: 417 /* 00: 128 bits. The full address 418 * is carried in-line. 419 */ 420 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); 421 break; 422 case LOWPAN_IPHC_DAM_01: 423 /* 01: 48 bits. The address takes 424 * the form ffXX::00XX:XXXX:XXXX. 425 */ 426 ipaddr->s6_addr[0] = 0xFF; 427 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 428 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); 429 break; 430 case LOWPAN_IPHC_DAM_10: 431 /* 10: 32 bits. The address takes 432 * the form ffXX::00XX:XXXX. 433 */ 434 ipaddr->s6_addr[0] = 0xFF; 435 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); 436 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); 437 break; 438 case LOWPAN_IPHC_DAM_11: 439 /* 11: 8 bits. The address takes 440 * the form ff02::00XX. 441 */ 442 ipaddr->s6_addr[0] = 0xFF; 443 ipaddr->s6_addr[1] = 0x02; 444 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); 445 break; 446 default: 447 pr_debug("DAM value has a wrong value: 0x%x\n", address_mode); 448 return -EINVAL; 449 } 450 451 if (fail) { 452 pr_debug("Failed to fetch skb data\n"); 453 return -EIO; 454 } 455 456 raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", 457 ipaddr->s6_addr, 16); 458 459 return 0; 460 } 461 462 static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb, 463 struct lowpan_iphc_ctx *ctx, 464 struct in6_addr *ipaddr, 465 u8 address_mode) 466 { 467 struct in6_addr network_pfx = {}; 468 bool fail; 469 470 ipaddr->s6_addr[0] = 0xFF; 471 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2); 472 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4); 473 if (fail) 474 return -EIO; 475 476 /* take prefix_len and network prefix from the context */ 477 ipaddr->s6_addr[3] = ctx->plen; 478 /* get network prefix to copy into multicast address */ 479 ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen); 480 /* setting network prefix */ 481 memcpy(&ipaddr->s6_addr[4], &network_pfx, 8); 482 483 return 0; 484 } 485 486 /* get the ecn values from iphc tf format and set it to ipv6hdr */ 487 static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf) 488 { 489 /* get the two higher bits which is ecn */ 490 u8 ecn = tf[0] & 0xc0; 491 492 /* ECN takes 0x30 in hdr->flow_lbl[0] */ 493 hdr->flow_lbl[0] |= (ecn >> 2); 494 } 495 496 /* get the dscp values from iphc tf format and set it to ipv6hdr */ 497 static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf) 498 { 499 /* DSCP is at place after ECN */ 500 u8 dscp = tf[0] & 0x3f; 501 502 /* The four highest bits need to be set at hdr->priority */ 503 hdr->priority |= ((dscp & 0x3c) >> 2); 504 /* The two lower bits is part of hdr->flow_lbl[0] */ 505 hdr->flow_lbl[0] |= ((dscp & 0x03) << 6); 506 } 507 508 /* get the flow label values from iphc tf format and set it to ipv6hdr */ 509 static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl) 510 { 511 /* flow label is always some array started with lower nibble of 512 * flow_lbl[0] and followed with two bytes afterwards. Inside inline 513 * data the flow_lbl position can be different, which will be handled 514 * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit 515 * shifted, the different lbl pointer will handle that. 516 * 517 * The flow label will started at lower nibble of flow_lbl[0], the 518 * higher nibbles are part of DSCP + ECN. 519 */ 520 hdr->flow_lbl[0] |= lbl[0] & 0x0f; 521 memcpy(&hdr->flow_lbl[1], &lbl[1], 2); 522 } 523 524 /* lowpan_iphc_tf_decompress - decompress the traffic class. 525 * This function will return zero on success, a value lower than zero if 526 * failed. 527 */ 528 static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr, 529 u8 val) 530 { 531 u8 tf[4]; 532 533 /* Traffic Class and Flow Label */ 534 switch (val) { 535 case LOWPAN_IPHC_TF_00: 536 /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */ 537 if (lowpan_fetch_skb(skb, tf, 4)) 538 return -EINVAL; 539 540 /* 1 2 3 541 * 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 542 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 543 * |ECN| DSCP | rsv | Flow Label | 544 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 545 */ 546 lowpan_iphc_tf_set_ecn(hdr, tf); 547 lowpan_iphc_tf_set_dscp(hdr, tf); 548 lowpan_iphc_tf_set_lbl(hdr, &tf[1]); 549 break; 550 case LOWPAN_IPHC_TF_01: 551 /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */ 552 if (lowpan_fetch_skb(skb, tf, 3)) 553 return -EINVAL; 554 555 /* 1 2 556 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 557 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 558 * |ECN|rsv| Flow Label | 559 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 560 */ 561 lowpan_iphc_tf_set_ecn(hdr, tf); 562 lowpan_iphc_tf_set_lbl(hdr, &tf[0]); 563 break; 564 case LOWPAN_IPHC_TF_10: 565 /* ECN + DSCP (1 byte), Flow Label is elided. */ 566 if (lowpan_fetch_skb(skb, tf, 1)) 567 return -EINVAL; 568 569 /* 0 1 2 3 4 5 6 7 570 * +-+-+-+-+-+-+-+-+ 571 * |ECN| DSCP | 572 * +-+-+-+-+-+-+-+-+ 573 */ 574 lowpan_iphc_tf_set_ecn(hdr, tf); 575 lowpan_iphc_tf_set_dscp(hdr, tf); 576 break; 577 case LOWPAN_IPHC_TF_11: 578 /* Traffic Class and Flow Label are elided */ 579 break; 580 default: 581 WARN_ON_ONCE(1); 582 return -EINVAL; 583 } 584 585 return 0; 586 } 587 588 /* TTL uncompression values */ 589 static const u8 lowpan_ttl_values[] = { 590 [LOWPAN_IPHC_HLIM_01] = 1, 591 [LOWPAN_IPHC_HLIM_10] = 64, 592 [LOWPAN_IPHC_HLIM_11] = 255, 593 }; 594 595 int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev, 596 const void *daddr, const void *saddr) 597 { 598 struct ipv6hdr hdr = {}; 599 struct lowpan_iphc_ctx *ci; 600 u8 iphc0, iphc1, cid = 0; 601 int err; 602 603 raw_dump_table(__func__, "raw skb data dump uncompressed", 604 skb->data, skb->len); 605 606 if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) || 607 lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1))) 608 return -EINVAL; 609 610 hdr.version = 6; 611 612 /* default CID = 0, another if the CID flag is set */ 613 if (iphc1 & LOWPAN_IPHC_CID) { 614 if (lowpan_fetch_skb(skb, &cid, sizeof(cid))) 615 return -EINVAL; 616 } 617 618 err = lowpan_iphc_tf_decompress(skb, &hdr, 619 iphc0 & LOWPAN_IPHC_TF_MASK); 620 if (err < 0) 621 return err; 622 623 /* Next Header */ 624 if (!(iphc0 & LOWPAN_IPHC_NH)) { 625 /* Next header is carried inline */ 626 if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr))) 627 return -EINVAL; 628 629 pr_debug("NH flag is set, next header carried inline: %02x\n", 630 hdr.nexthdr); 631 } 632 633 /* Hop Limit */ 634 if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) { 635 hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK]; 636 } else { 637 if (lowpan_fetch_skb(skb, &hdr.hop_limit, 638 sizeof(hdr.hop_limit))) 639 return -EINVAL; 640 } 641 642 if (iphc1 & LOWPAN_IPHC_SAC) { 643 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 644 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid)); 645 if (!ci) { 646 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 647 return -EINVAL; 648 } 649 650 pr_debug("SAC bit is set. Handle context based source address.\n"); 651 err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr, 652 iphc1 & LOWPAN_IPHC_SAM_MASK, 653 saddr); 654 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 655 } else { 656 /* Source address uncompression */ 657 pr_debug("source address stateless compression\n"); 658 err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr, 659 iphc1 & LOWPAN_IPHC_SAM_MASK, 660 saddr); 661 } 662 663 /* Check on error of previous branch */ 664 if (err) 665 return -EINVAL; 666 667 switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) { 668 case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC: 669 skb->pkt_type = PACKET_BROADCAST; 670 671 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 672 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 673 if (!ci) { 674 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 675 return -EINVAL; 676 } 677 678 /* multicast with context */ 679 pr_debug("dest: context-based mcast compression\n"); 680 err = lowpan_uncompress_multicast_ctx_daddr(skb, ci, 681 &hdr.daddr, 682 iphc1 & LOWPAN_IPHC_DAM_MASK); 683 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 684 break; 685 case LOWPAN_IPHC_M: 686 skb->pkt_type = PACKET_BROADCAST; 687 688 /* multicast */ 689 err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr, 690 iphc1 & LOWPAN_IPHC_DAM_MASK); 691 break; 692 case LOWPAN_IPHC_DAC: 693 skb->pkt_type = PACKET_HOST; 694 695 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 696 ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid)); 697 if (!ci) { 698 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 699 return -EINVAL; 700 } 701 702 /* Destination address context based uncompression */ 703 pr_debug("DAC bit is set. Handle context based destination address.\n"); 704 err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr, 705 iphc1 & LOWPAN_IPHC_DAM_MASK, 706 daddr); 707 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 708 break; 709 default: 710 skb->pkt_type = PACKET_HOST; 711 712 err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr, 713 iphc1 & LOWPAN_IPHC_DAM_MASK, 714 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_dev(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 inline bool 773 lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr, 774 const struct lowpan_iphc_ctx *ctx, 775 const void *lladdr) 776 { 777 const struct ieee802154_addr *addr = lladdr; 778 unsigned char extended_addr[EUI64_ADDR_LEN]; 779 bool lladdr_compress = false; 780 struct in6_addr tmp = {}; 781 782 switch (addr->mode) { 783 case IEEE802154_ADDR_LONG: 784 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 785 /* check for SAM/DAM = 11 */ 786 memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN); 787 /* second bit-flip (Universe/Local) is done according RFC2464 */ 788 tmp.s6_addr[8] ^= 0x02; 789 /* context information are always used */ 790 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 791 if (ipv6_addr_equal(&tmp, ipaddr)) 792 lladdr_compress = true; 793 break; 794 case IEEE802154_ADDR_SHORT: 795 tmp.s6_addr[11] = 0xFF; 796 tmp.s6_addr[12] = 0xFE; 797 ieee802154_le16_to_be16(&tmp.s6_addr16[7], 798 &addr->short_addr); 799 /* context information are always used */ 800 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 801 if (ipv6_addr_equal(&tmp, ipaddr)) 802 lladdr_compress = true; 803 break; 804 default: 805 /* should never handled and filtered by 802154 6lowpan */ 806 WARN_ON_ONCE(1); 807 break; 808 } 809 810 return lladdr_compress; 811 } 812 813 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev, 814 const struct in6_addr *ipaddr, 815 const struct lowpan_iphc_ctx *ctx, 816 const unsigned char *lladdr, bool sam) 817 { 818 struct in6_addr tmp = {}; 819 u8 dam; 820 821 switch (lowpan_dev(dev)->lltype) { 822 case LOWPAN_LLTYPE_IEEE802154: 823 if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx, 824 lladdr)) { 825 dam = LOWPAN_IPHC_DAM_11; 826 goto out; 827 } 828 break; 829 default: 830 /* check for SAM/DAM = 11 */ 831 memcpy(&tmp.s6_addr[8], lladdr, EUI64_ADDR_LEN); 832 /* second bit-flip (Universe/Local) is done according RFC2464 */ 833 tmp.s6_addr[8] ^= 0x02; 834 /* context information are always used */ 835 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 836 if (ipv6_addr_equal(&tmp, ipaddr)) { 837 dam = LOWPAN_IPHC_DAM_11; 838 goto out; 839 } 840 break; 841 } 842 843 memset(&tmp, 0, sizeof(tmp)); 844 /* check for SAM/DAM = 10 */ 845 tmp.s6_addr[11] = 0xFF; 846 tmp.s6_addr[12] = 0xFE; 847 memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2); 848 /* context information are always used */ 849 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 850 if (ipv6_addr_equal(&tmp, ipaddr)) { 851 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2); 852 dam = LOWPAN_IPHC_DAM_10; 853 goto out; 854 } 855 856 memset(&tmp, 0, sizeof(tmp)); 857 /* check for SAM/DAM = 01, should always match */ 858 memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8); 859 /* context information are always used */ 860 ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen); 861 if (ipv6_addr_equal(&tmp, ipaddr)) { 862 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8); 863 dam = LOWPAN_IPHC_DAM_01; 864 goto out; 865 } 866 867 WARN_ONCE(1, "context found but no address mode matched\n"); 868 return LOWPAN_IPHC_DAM_00; 869 out: 870 871 if (sam) 872 return lowpan_iphc_dam_to_sam_value[dam]; 873 else 874 return dam; 875 } 876 877 static inline bool 878 lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr, 879 const void *lladdr) 880 { 881 const struct ieee802154_addr *addr = lladdr; 882 unsigned char extended_addr[EUI64_ADDR_LEN]; 883 bool lladdr_compress = false; 884 struct in6_addr tmp = {}; 885 886 switch (addr->mode) { 887 case IEEE802154_ADDR_LONG: 888 ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr); 889 if (is_addr_mac_addr_based(ipaddr, extended_addr)) 890 lladdr_compress = true; 891 break; 892 case IEEE802154_ADDR_SHORT: 893 /* fe:80::ff:fe00:XXXX 894 * \__/ 895 * short_addr 896 * 897 * Universe/Local bit is zero. 898 */ 899 tmp.s6_addr[0] = 0xFE; 900 tmp.s6_addr[1] = 0x80; 901 tmp.s6_addr[11] = 0xFF; 902 tmp.s6_addr[12] = 0xFE; 903 ieee802154_le16_to_be16(&tmp.s6_addr16[7], 904 &addr->short_addr); 905 if (ipv6_addr_equal(&tmp, ipaddr)) 906 lladdr_compress = true; 907 break; 908 default: 909 /* should never handled and filtered by 802154 6lowpan */ 910 WARN_ON_ONCE(1); 911 break; 912 } 913 914 return lladdr_compress; 915 } 916 917 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev, 918 const struct in6_addr *ipaddr, 919 const unsigned char *lladdr, bool sam) 920 { 921 u8 dam = LOWPAN_IPHC_DAM_01; 922 923 switch (lowpan_dev(dev)->lltype) { 924 case LOWPAN_LLTYPE_IEEE802154: 925 if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) { 926 dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ 927 pr_debug("address compression 0 bits\n"); 928 goto out; 929 } 930 break; 931 default: 932 if (is_addr_mac_addr_based(ipaddr, lladdr)) { 933 dam = LOWPAN_IPHC_DAM_11; /* 0-bits */ 934 pr_debug("address compression 0 bits\n"); 935 goto out; 936 } 937 break; 938 } 939 940 if (lowpan_is_iid_16_bit_compressable(ipaddr)) { 941 /* compress IID to 16 bits xxxx::XXXX */ 942 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2); 943 dam = LOWPAN_IPHC_DAM_10; /* 16-bits */ 944 raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)", 945 *hc_ptr - 2, 2); 946 goto out; 947 } 948 949 /* do not compress IID => xxxx::IID */ 950 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8); 951 raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)", 952 *hc_ptr - 8, 8); 953 954 out: 955 956 if (sam) 957 return lowpan_iphc_dam_to_sam_value[dam]; 958 else 959 return dam; 960 } 961 962 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */ 963 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr) 964 { 965 u8 dscp, ecn; 966 967 /* hdr->priority contains the higher bits of dscp, lower are part of 968 * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr. 969 */ 970 dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6); 971 /* ECN is at the two lower bits from first nibble of flow_lbl[0] */ 972 ecn = (hdr->flow_lbl[0] & 0x30); 973 /* for pretty debug output, also shift ecn to get the ecn value */ 974 pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp); 975 /* ECN is at 0x30 now, shift it to have ECN + DCSP */ 976 return (ecn << 2) | dscp; 977 } 978 979 /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */ 980 static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr) 981 { 982 return ((!(hdr->flow_lbl[0] & 0x0f)) && 983 !hdr->flow_lbl[1] && !hdr->flow_lbl[2]); 984 } 985 986 /* lowpan_iphc_tf_compress - compress the traffic class which is set by 987 * ipv6hdr. Return the corresponding format identifier which is used. 988 */ 989 static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr) 990 { 991 /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */ 992 u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val; 993 994 /* printout the traffic class in hc format */ 995 pr_debug("tc 0x%02x\n", tc); 996 997 if (lowpan_iphc_is_flow_lbl_zero(hdr)) { 998 if (!tc) { 999 /* 11: Traffic Class and Flow Label are elided. */ 1000 val = LOWPAN_IPHC_TF_11; 1001 } else { 1002 /* 10: ECN + DSCP (1 byte), Flow Label is elided. 1003 * 1004 * 0 1 2 3 4 5 6 7 1005 * +-+-+-+-+-+-+-+-+ 1006 * |ECN| DSCP | 1007 * +-+-+-+-+-+-+-+-+ 1008 */ 1009 lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc)); 1010 val = LOWPAN_IPHC_TF_10; 1011 } 1012 } else { 1013 /* check if dscp is zero, it's after the first two bit */ 1014 if (!(tc & 0x3f)) { 1015 /* 01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided 1016 * 1017 * 1 2 1018 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 1019 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1020 * |ECN|rsv| Flow Label | 1021 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1022 */ 1023 memcpy(&tf[0], &hdr->flow_lbl[0], 3); 1024 /* zero the highest 4-bits, contains DCSP + ECN */ 1025 tf[0] &= ~0xf0; 1026 /* set ECN */ 1027 tf[0] |= (tc & 0xc0); 1028 1029 lowpan_push_hc_data(hc_ptr, tf, 3); 1030 val = LOWPAN_IPHC_TF_01; 1031 } else { 1032 /* 00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) 1033 * 1034 * 1 2 3 1035 * 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 1036 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1037 * |ECN| DSCP | rsv | Flow Label | 1038 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 1039 */ 1040 memcpy(&tf[0], &tc, sizeof(tc)); 1041 /* highest nibble of flow_lbl[0] is part of DSCP + ECN 1042 * which will be the 4-bit pad and will be filled with 1043 * zeros afterwards. 1044 */ 1045 memcpy(&tf[1], &hdr->flow_lbl[0], 3); 1046 /* zero the 4-bit pad, which is reserved */ 1047 tf[1] &= ~0xf0; 1048 1049 lowpan_push_hc_data(hc_ptr, tf, 4); 1050 val = LOWPAN_IPHC_TF_00; 1051 } 1052 } 1053 1054 return val; 1055 } 1056 1057 static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr, 1058 const struct lowpan_iphc_ctx *ctx, 1059 const struct in6_addr *ipaddr) 1060 { 1061 u8 data[6]; 1062 1063 /* flags/scope, reserved (RIID) */ 1064 memcpy(data, &ipaddr->s6_addr[1], 2); 1065 /* group ID */ 1066 memcpy(&data[1], &ipaddr->s6_addr[11], 4); 1067 lowpan_push_hc_data(hc_ptr, data, 6); 1068 1069 return LOWPAN_IPHC_DAM_00; 1070 } 1071 1072 static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr, 1073 const struct in6_addr *ipaddr) 1074 { 1075 u8 val; 1076 1077 if (lowpan_is_mcast_addr_compressable8(ipaddr)) { 1078 pr_debug("compressed to 1 octet\n"); 1079 /* use last byte */ 1080 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1); 1081 val = LOWPAN_IPHC_DAM_11; 1082 } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) { 1083 pr_debug("compressed to 4 octets\n"); 1084 /* second byte + the last three */ 1085 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 1086 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3); 1087 val = LOWPAN_IPHC_DAM_10; 1088 } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) { 1089 pr_debug("compressed to 6 octets\n"); 1090 /* second byte + the last five */ 1091 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1); 1092 lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5); 1093 val = LOWPAN_IPHC_DAM_01; 1094 } else { 1095 pr_debug("using full address\n"); 1096 lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16); 1097 val = LOWPAN_IPHC_DAM_00; 1098 } 1099 1100 return val; 1101 } 1102 1103 int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev, 1104 const void *daddr, const void *saddr) 1105 { 1106 u8 iphc0, iphc1, *hc_ptr, cid = 0; 1107 struct ipv6hdr *hdr; 1108 u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {}; 1109 struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry; 1110 int ret, ipv6_daddr_type, ipv6_saddr_type; 1111 1112 if (skb->protocol != htons(ETH_P_IPV6)) 1113 return -EINVAL; 1114 1115 hdr = ipv6_hdr(skb); 1116 hc_ptr = head + 2; 1117 1118 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n" 1119 "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n", 1120 hdr->version, ntohs(hdr->payload_len), hdr->nexthdr, 1121 hdr->hop_limit, &hdr->daddr); 1122 1123 raw_dump_table(__func__, "raw skb network header dump", 1124 skb_network_header(skb), sizeof(struct ipv6hdr)); 1125 1126 /* As we copy some bit-length fields, in the IPHC encoding bytes, 1127 * we sometimes use |= 1128 * If the field is 0, and the current bit value in memory is 1, 1129 * this does not work. We therefore reset the IPHC encoding here 1130 */ 1131 iphc0 = LOWPAN_DISPATCH_IPHC; 1132 iphc1 = 0; 1133 1134 raw_dump_table(__func__, "sending raw skb network uncompressed packet", 1135 skb->data, skb->len); 1136 1137 ipv6_daddr_type = ipv6_addr_type(&hdr->daddr); 1138 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 1139 if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) 1140 dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr); 1141 else 1142 dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr); 1143 if (dci) { 1144 memcpy(&dci_entry, dci, sizeof(*dci)); 1145 cid |= dci->id; 1146 } 1147 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 1148 1149 spin_lock_bh(&lowpan_dev(dev)->ctx.lock); 1150 sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr); 1151 if (sci) { 1152 memcpy(&sci_entry, sci, sizeof(*sci)); 1153 cid |= (sci->id << 4); 1154 } 1155 spin_unlock_bh(&lowpan_dev(dev)->ctx.lock); 1156 1157 /* if cid is zero it will be compressed */ 1158 if (cid) { 1159 iphc1 |= LOWPAN_IPHC_CID; 1160 lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid)); 1161 } 1162 1163 /* Traffic Class, Flow Label compression */ 1164 iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr); 1165 1166 /* NOTE: payload length is always compressed */ 1167 1168 /* Check if we provide the nhc format for nexthdr and compression 1169 * functionality. If not nexthdr is handled inline and not compressed. 1170 */ 1171 ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr); 1172 if (ret == -ENOENT) 1173 lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr, 1174 sizeof(hdr->nexthdr)); 1175 else 1176 iphc0 |= LOWPAN_IPHC_NH; 1177 1178 /* Hop limit 1179 * if 1: compress, encoding is 01 1180 * if 64: compress, encoding is 10 1181 * if 255: compress, encoding is 11 1182 * else do not compress 1183 */ 1184 switch (hdr->hop_limit) { 1185 case 1: 1186 iphc0 |= LOWPAN_IPHC_HLIM_01; 1187 break; 1188 case 64: 1189 iphc0 |= LOWPAN_IPHC_HLIM_10; 1190 break; 1191 case 255: 1192 iphc0 |= LOWPAN_IPHC_HLIM_11; 1193 break; 1194 default: 1195 lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit, 1196 sizeof(hdr->hop_limit)); 1197 } 1198 1199 ipv6_saddr_type = ipv6_addr_type(&hdr->saddr); 1200 /* source address compression */ 1201 if (ipv6_saddr_type == IPV6_ADDR_ANY) { 1202 pr_debug("source address is unspecified, setting SAC\n"); 1203 iphc1 |= LOWPAN_IPHC_SAC; 1204 } else { 1205 if (sci) { 1206 iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1207 &hdr->saddr, 1208 &sci_entry, saddr, 1209 true); 1210 iphc1 |= LOWPAN_IPHC_SAC; 1211 } else { 1212 if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL && 1213 lowpan_is_linklocal_zero_padded(hdr->saddr)) { 1214 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 1215 &hdr->saddr, 1216 saddr, true); 1217 pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n", 1218 &hdr->saddr, iphc1); 1219 } else { 1220 pr_debug("send the full source address\n"); 1221 lowpan_push_hc_data(&hc_ptr, 1222 hdr->saddr.s6_addr, 16); 1223 } 1224 } 1225 } 1226 1227 /* destination address compression */ 1228 if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) { 1229 pr_debug("destination address is multicast: "); 1230 iphc1 |= LOWPAN_IPHC_M; 1231 if (dci) { 1232 iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr, 1233 &dci_entry, 1234 &hdr->daddr); 1235 iphc1 |= LOWPAN_IPHC_DAC; 1236 } else { 1237 iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr, 1238 &hdr->daddr); 1239 } 1240 } else { 1241 if (dci) { 1242 iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev, 1243 &hdr->daddr, 1244 &dci_entry, daddr, 1245 false); 1246 iphc1 |= LOWPAN_IPHC_DAC; 1247 } else { 1248 if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL && 1249 lowpan_is_linklocal_zero_padded(hdr->daddr)) { 1250 iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev, 1251 &hdr->daddr, 1252 daddr, false); 1253 pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n", 1254 &hdr->daddr, iphc1); 1255 } else { 1256 pr_debug("dest address unicast %pI6c\n", 1257 &hdr->daddr); 1258 lowpan_push_hc_data(&hc_ptr, 1259 hdr->daddr.s6_addr, 16); 1260 } 1261 } 1262 } 1263 1264 /* next header compression */ 1265 if (iphc0 & LOWPAN_IPHC_NH) { 1266 ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr); 1267 if (ret < 0) 1268 return ret; 1269 } 1270 1271 head[0] = iphc0; 1272 head[1] = iphc1; 1273 1274 skb_pull(skb, sizeof(struct ipv6hdr)); 1275 skb_reset_transport_header(skb); 1276 memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head); 1277 skb_reset_network_header(skb); 1278 1279 pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len); 1280 1281 raw_dump_table(__func__, "raw skb data dump compressed", 1282 skb->data, skb->len); 1283 return 0; 1284 } 1285 EXPORT_SYMBOL_GPL(lowpan_header_compress); 1286