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