xref: /openbmc/linux/net/6lowpan/iphc.c (revision 4f205687)
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 		spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
670 		ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
671 		if (!ci) {
672 			spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
673 			return -EINVAL;
674 		}
675 
676 		/* multicast with context */
677 		pr_debug("dest: context-based mcast compression\n");
678 		err = lowpan_uncompress_multicast_ctx_daddr(skb, ci,
679 							    &hdr.daddr,
680 							    iphc1 & LOWPAN_IPHC_DAM_MASK);
681 		spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
682 		break;
683 	case LOWPAN_IPHC_M:
684 		/* multicast */
685 		err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr,
686 							iphc1 & LOWPAN_IPHC_DAM_MASK);
687 		break;
688 	case LOWPAN_IPHC_DAC:
689 		spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
690 		ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
691 		if (!ci) {
692 			spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
693 			return -EINVAL;
694 		}
695 
696 		/* Destination address context based uncompression */
697 		pr_debug("DAC bit is set. Handle context based destination address.\n");
698 		err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr,
699 						      iphc1 & LOWPAN_IPHC_DAM_MASK,
700 						      daddr);
701 		spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
702 		break;
703 	default:
704 		err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr,
705 						  iphc1 & LOWPAN_IPHC_DAM_MASK,
706 						  daddr);
707 		pr_debug("dest: stateless compression mode %d dest %pI6c\n",
708 			 iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr);
709 		break;
710 	}
711 
712 	if (err)
713 		return -EINVAL;
714 
715 	/* Next header data uncompression */
716 	if (iphc0 & LOWPAN_IPHC_NH) {
717 		err = lowpan_nhc_do_uncompression(skb, dev, &hdr);
718 		if (err < 0)
719 			return err;
720 	} else {
721 		err = skb_cow(skb, sizeof(hdr));
722 		if (unlikely(err))
723 			return err;
724 	}
725 
726 	switch (lowpan_dev(dev)->lltype) {
727 	case LOWPAN_LLTYPE_IEEE802154:
728 		if (lowpan_802154_cb(skb)->d_size)
729 			hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size -
730 						sizeof(struct ipv6hdr));
731 		else
732 			hdr.payload_len = htons(skb->len);
733 		break;
734 	default:
735 		hdr.payload_len = htons(skb->len);
736 		break;
737 	}
738 
739 	pr_debug("skb headroom size = %d, data length = %d\n",
740 		 skb_headroom(skb), skb->len);
741 
742 	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
743 		 "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
744 		hdr.version, ntohs(hdr.payload_len), hdr.nexthdr,
745 		hdr.hop_limit, &hdr.daddr);
746 
747 	skb_push(skb, sizeof(hdr));
748 	skb_reset_network_header(skb);
749 	skb_copy_to_linear_data(skb, &hdr, sizeof(hdr));
750 
751 	raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr));
752 
753 	return 0;
754 }
755 EXPORT_SYMBOL_GPL(lowpan_header_decompress);
756 
757 static const u8 lowpan_iphc_dam_to_sam_value[] = {
758 	[LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00,
759 	[LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01,
760 	[LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10,
761 	[LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11,
762 };
763 
764 static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct in6_addr *ipaddr,
765 				   const struct lowpan_iphc_ctx *ctx,
766 				   const unsigned char *lladdr, bool sam)
767 {
768 	struct in6_addr tmp = {};
769 	u8 dam;
770 
771 	/* check for SAM/DAM = 11 */
772 	memcpy(&tmp.s6_addr[8], lladdr, 8);
773 	/* second bit-flip (Universe/Local) is done according RFC2464 */
774 	tmp.s6_addr[8] ^= 0x02;
775 	/* context information are always used */
776 	ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
777 	if (ipv6_addr_equal(&tmp, ipaddr)) {
778 		dam = LOWPAN_IPHC_DAM_11;
779 		goto out;
780 	}
781 
782 	memset(&tmp, 0, sizeof(tmp));
783 	/* check for SAM/DAM = 10 */
784 	tmp.s6_addr[11] = 0xFF;
785 	tmp.s6_addr[12] = 0xFE;
786 	memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2);
787 	/* context information are always used */
788 	ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
789 	if (ipv6_addr_equal(&tmp, ipaddr)) {
790 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2);
791 		dam = LOWPAN_IPHC_DAM_10;
792 		goto out;
793 	}
794 
795 	memset(&tmp, 0, sizeof(tmp));
796 	/* check for SAM/DAM = 01, should always match */
797 	memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8);
798 	/* context information are always used */
799 	ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
800 	if (ipv6_addr_equal(&tmp, ipaddr)) {
801 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8);
802 		dam = LOWPAN_IPHC_DAM_01;
803 		goto out;
804 	}
805 
806 	WARN_ONCE(1, "context found but no address mode matched\n");
807 	return LOWPAN_IPHC_DAM_00;
808 out:
809 
810 	if (sam)
811 		return lowpan_iphc_dam_to_sam_value[dam];
812 	else
813 		return dam;
814 }
815 
816 static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct in6_addr *ipaddr,
817 				  const unsigned char *lladdr, bool sam)
818 {
819 	u8 dam = LOWPAN_IPHC_DAM_00;
820 
821 	if (is_addr_mac_addr_based(ipaddr, lladdr)) {
822 		dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
823 		pr_debug("address compression 0 bits\n");
824 	} else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
825 		/* compress IID to 16 bits xxxx::XXXX */
826 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2);
827 		dam = LOWPAN_IPHC_DAM_10; /* 16-bits */
828 		raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
829 				*hc_ptr - 2, 2);
830 	} else {
831 		/* do not compress IID => xxxx::IID */
832 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8);
833 		dam = LOWPAN_IPHC_DAM_01; /* 64-bits */
834 		raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
835 				*hc_ptr - 8, 8);
836 	}
837 
838 	if (sam)
839 		return lowpan_iphc_dam_to_sam_value[dam];
840 	else
841 		return dam;
842 }
843 
844 /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */
845 static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr)
846 {
847 	u8 dscp, ecn;
848 
849 	/* hdr->priority contains the higher bits of dscp, lower are part of
850 	 * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr.
851 	 */
852 	dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6);
853 	/* ECN is at the two lower bits from first nibble of flow_lbl[0] */
854 	ecn = (hdr->flow_lbl[0] & 0x30);
855 	/* for pretty debug output, also shift ecn to get the ecn value */
856 	pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp);
857 	/* ECN is at 0x30 now, shift it to have ECN + DCSP */
858 	return (ecn << 2) | dscp;
859 }
860 
861 /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */
862 static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr)
863 {
864 	return ((!(hdr->flow_lbl[0] & 0x0f)) &&
865 		!hdr->flow_lbl[1] && !hdr->flow_lbl[2]);
866 }
867 
868 /* lowpan_iphc_tf_compress - compress the traffic class which is set by
869  *	ipv6hdr. Return the corresponding format identifier which is used.
870  */
871 static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr)
872 {
873 	/* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */
874 	u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val;
875 
876 	/* printout the traffic class in hc format */
877 	pr_debug("tc 0x%02x\n", tc);
878 
879 	if (lowpan_iphc_is_flow_lbl_zero(hdr)) {
880 		if (!tc) {
881 			/* 11:  Traffic Class and Flow Label are elided. */
882 			val = LOWPAN_IPHC_TF_11;
883 		} else {
884 			/* 10:  ECN + DSCP (1 byte), Flow Label is elided.
885 			 *
886 			 *  0 1 2 3 4 5 6 7
887 			 * +-+-+-+-+-+-+-+-+
888 			 * |ECN|   DSCP    |
889 			 * +-+-+-+-+-+-+-+-+
890 			 */
891 			lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc));
892 			val = LOWPAN_IPHC_TF_10;
893 		}
894 	} else {
895 		/* check if dscp is zero, it's after the first two bit */
896 		if (!(tc & 0x3f)) {
897 			/* 01:  ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
898 			 *
899 			 *                     1                   2
900 			 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
901 			 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
902 			 * |ECN|rsv|             Flow Label                |
903 			 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
904 			 */
905 			memcpy(&tf[0], &hdr->flow_lbl[0], 3);
906 			/* zero the highest 4-bits, contains DCSP + ECN */
907 			tf[0] &= ~0xf0;
908 			/* set ECN */
909 			tf[0] |= (tc & 0xc0);
910 
911 			lowpan_push_hc_data(hc_ptr, tf, 3);
912 			val = LOWPAN_IPHC_TF_01;
913 		} else {
914 			/* 00:  ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
915 			 *
916 			 *                      1                   2                   3
917 			 *  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
918 			 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
919 			 * |ECN|   DSCP    |  rsv  |             Flow Label                |
920 			 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
921 			 */
922 			memcpy(&tf[0], &tc, sizeof(tc));
923 			/* highest nibble of flow_lbl[0] is part of DSCP + ECN
924 			 * which will be the 4-bit pad and will be filled with
925 			 * zeros afterwards.
926 			 */
927 			memcpy(&tf[1], &hdr->flow_lbl[0], 3);
928 			/* zero the 4-bit pad, which is reserved */
929 			tf[1] &= ~0xf0;
930 
931 			lowpan_push_hc_data(hc_ptr, tf, 4);
932 			val = LOWPAN_IPHC_TF_00;
933 		}
934 	}
935 
936 	return val;
937 }
938 
939 static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr,
940 					      const struct lowpan_iphc_ctx *ctx,
941 					      const struct in6_addr *ipaddr)
942 {
943 	u8 data[6];
944 
945 	/* flags/scope, reserved (RIID) */
946 	memcpy(data, &ipaddr->s6_addr[1], 2);
947 	/* group ID */
948 	memcpy(&data[1], &ipaddr->s6_addr[11], 4);
949 	lowpan_push_hc_data(hc_ptr, data, 6);
950 
951 	return LOWPAN_IPHC_DAM_00;
952 }
953 
954 static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr,
955 					  const struct in6_addr *ipaddr)
956 {
957 	u8 val;
958 
959 	if (lowpan_is_mcast_addr_compressable8(ipaddr)) {
960 		pr_debug("compressed to 1 octet\n");
961 		/* use last byte */
962 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1);
963 		val = LOWPAN_IPHC_DAM_11;
964 	} else if (lowpan_is_mcast_addr_compressable32(ipaddr)) {
965 		pr_debug("compressed to 4 octets\n");
966 		/* second byte + the last three */
967 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
968 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3);
969 		val = LOWPAN_IPHC_DAM_10;
970 	} else if (lowpan_is_mcast_addr_compressable48(ipaddr)) {
971 		pr_debug("compressed to 6 octets\n");
972 		/* second byte + the last five */
973 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
974 		lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5);
975 		val = LOWPAN_IPHC_DAM_01;
976 	} else {
977 		pr_debug("using full address\n");
978 		lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16);
979 		val = LOWPAN_IPHC_DAM_00;
980 	}
981 
982 	return val;
983 }
984 
985 int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev,
986 			   const void *daddr, const void *saddr)
987 {
988 	u8 iphc0, iphc1, *hc_ptr, cid = 0;
989 	struct ipv6hdr *hdr;
990 	u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {};
991 	struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry;
992 	int ret, ipv6_daddr_type, ipv6_saddr_type;
993 
994 	if (skb->protocol != htons(ETH_P_IPV6))
995 		return -EINVAL;
996 
997 	hdr = ipv6_hdr(skb);
998 	hc_ptr = head + 2;
999 
1000 	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
1001 		 "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
1002 		 hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
1003 		 hdr->hop_limit, &hdr->daddr);
1004 
1005 	raw_dump_table(__func__, "raw skb network header dump",
1006 		       skb_network_header(skb), sizeof(struct ipv6hdr));
1007 
1008 	/* As we copy some bit-length fields, in the IPHC encoding bytes,
1009 	 * we sometimes use |=
1010 	 * If the field is 0, and the current bit value in memory is 1,
1011 	 * this does not work. We therefore reset the IPHC encoding here
1012 	 */
1013 	iphc0 = LOWPAN_DISPATCH_IPHC;
1014 	iphc1 = 0;
1015 
1016 	raw_dump_inline(__func__, "saddr", saddr, EUI64_ADDR_LEN);
1017 	raw_dump_inline(__func__, "daddr", daddr, EUI64_ADDR_LEN);
1018 
1019 	raw_dump_table(__func__, "sending raw skb network uncompressed packet",
1020 		       skb->data, skb->len);
1021 
1022 	ipv6_daddr_type = ipv6_addr_type(&hdr->daddr);
1023 	spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
1024 	if (ipv6_daddr_type & IPV6_ADDR_MULTICAST)
1025 		dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr);
1026 	else
1027 		dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr);
1028 	if (dci) {
1029 		memcpy(&dci_entry, dci, sizeof(*dci));
1030 		cid |= dci->id;
1031 	}
1032 	spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
1033 
1034 	spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
1035 	sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr);
1036 	if (sci) {
1037 		memcpy(&sci_entry, sci, sizeof(*sci));
1038 		cid |= (sci->id << 4);
1039 	}
1040 	spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
1041 
1042 	/* if cid is zero it will be compressed */
1043 	if (cid) {
1044 		iphc1 |= LOWPAN_IPHC_CID;
1045 		lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid));
1046 	}
1047 
1048 	/* Traffic Class, Flow Label compression */
1049 	iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr);
1050 
1051 	/* NOTE: payload length is always compressed */
1052 
1053 	/* Check if we provide the nhc format for nexthdr and compression
1054 	 * functionality. If not nexthdr is handled inline and not compressed.
1055 	 */
1056 	ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr);
1057 	if (ret == -ENOENT)
1058 		lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr,
1059 				    sizeof(hdr->nexthdr));
1060 	else
1061 		iphc0 |= LOWPAN_IPHC_NH;
1062 
1063 	/* Hop limit
1064 	 * if 1:   compress, encoding is 01
1065 	 * if 64:  compress, encoding is 10
1066 	 * if 255: compress, encoding is 11
1067 	 * else do not compress
1068 	 */
1069 	switch (hdr->hop_limit) {
1070 	case 1:
1071 		iphc0 |= LOWPAN_IPHC_HLIM_01;
1072 		break;
1073 	case 64:
1074 		iphc0 |= LOWPAN_IPHC_HLIM_10;
1075 		break;
1076 	case 255:
1077 		iphc0 |= LOWPAN_IPHC_HLIM_11;
1078 		break;
1079 	default:
1080 		lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit,
1081 				    sizeof(hdr->hop_limit));
1082 	}
1083 
1084 	ipv6_saddr_type = ipv6_addr_type(&hdr->saddr);
1085 	/* source address compression */
1086 	if (ipv6_saddr_type == IPV6_ADDR_ANY) {
1087 		pr_debug("source address is unspecified, setting SAC\n");
1088 		iphc1 |= LOWPAN_IPHC_SAC;
1089 	} else {
1090 		if (sci) {
1091 			iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, &hdr->saddr,
1092 							  &sci_entry, saddr,
1093 							  true);
1094 			iphc1 |= LOWPAN_IPHC_SAC;
1095 		} else {
1096 			if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL &&
1097 			    lowpan_is_linklocal_zero_padded(hdr->saddr)) {
1098 				iphc1 |= lowpan_compress_addr_64(&hc_ptr,
1099 								 &hdr->saddr,
1100 								 saddr, true);
1101 				pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n",
1102 					 &hdr->saddr, iphc1);
1103 			} else {
1104 				pr_debug("send the full source address\n");
1105 				lowpan_push_hc_data(&hc_ptr,
1106 						    hdr->saddr.s6_addr, 16);
1107 			}
1108 		}
1109 	}
1110 
1111 	/* destination address compression */
1112 	if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) {
1113 		pr_debug("destination address is multicast: ");
1114 		iphc1 |= LOWPAN_IPHC_M;
1115 		if (dci) {
1116 			iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr,
1117 								     &dci_entry,
1118 								     &hdr->daddr);
1119 			iphc1 |= LOWPAN_IPHC_DAC;
1120 		} else {
1121 			iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr,
1122 								 &hdr->daddr);
1123 		}
1124 	} else {
1125 		if (dci) {
1126 			iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, &hdr->daddr,
1127 							  &dci_entry, daddr,
1128 							  false);
1129 			iphc1 |= LOWPAN_IPHC_DAC;
1130 		} else {
1131 			if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL &&
1132 			    lowpan_is_linklocal_zero_padded(hdr->daddr)) {
1133 				iphc1 |= lowpan_compress_addr_64(&hc_ptr,
1134 								 &hdr->daddr,
1135 								 daddr, false);
1136 				pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n",
1137 					 &hdr->daddr, iphc1);
1138 			} else {
1139 				pr_debug("dest address unicast %pI6c\n",
1140 					 &hdr->daddr);
1141 				lowpan_push_hc_data(&hc_ptr,
1142 						    hdr->daddr.s6_addr, 16);
1143 			}
1144 		}
1145 	}
1146 
1147 	/* next header compression */
1148 	if (iphc0 & LOWPAN_IPHC_NH) {
1149 		ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr);
1150 		if (ret < 0)
1151 			return ret;
1152 	}
1153 
1154 	head[0] = iphc0;
1155 	head[1] = iphc1;
1156 
1157 	skb_pull(skb, sizeof(struct ipv6hdr));
1158 	skb_reset_transport_header(skb);
1159 	memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head);
1160 	skb_reset_network_header(skb);
1161 
1162 	pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len);
1163 
1164 	raw_dump_table(__func__, "raw skb data dump compressed",
1165 		       skb->data, skb->len);
1166 	return 0;
1167 }
1168 EXPORT_SYMBOL_GPL(lowpan_header_compress);
1169