blob: c85e71e0c7ffc640bd9592ce52b03dbde6cad926 [file] [log] [blame]
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001/*
2 * Copyright 2011, Siemens AG
3 * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
4 */
5
6/*
7 * Based on patches from Jon Smirl <jonsmirl@gmail.com>
8 * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
9 *
10 * This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License version 2
12 * as published by the Free Software Foundation.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License along
20 * with this program; if not, write to the Free Software Foundation, Inc.,
21 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
22 */
23
24/* Jon's code is based on 6lowpan implementation for Contiki which is:
25 * Copyright (c) 2008, Swedish Institute of Computer Science.
26 * All rights reserved.
27 *
28 * Redistribution and use in source and binary forms, with or without
29 * modification, are permitted provided that the following conditions
30 * are met:
31 * 1. Redistributions of source code must retain the above copyright
32 * notice, this list of conditions and the following disclaimer.
33 * 2. Redistributions in binary form must reproduce the above copyright
34 * notice, this list of conditions and the following disclaimer in the
35 * documentation and/or other materials provided with the distribution.
36 * 3. Neither the name of the Institute nor the names of its contributors
37 * may be used to endorse or promote products derived from this software
38 * without specific prior written permission.
39 *
40 * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
41 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43 * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
44 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
45 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
46 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
47 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
48 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
49 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
50 * SUCH DAMAGE.
51 */
52
Alexander Smirnov44331fe2011-08-24 19:34:42 -070053#include <linux/bitops.h>
54#include <linux/if_arp.h>
55#include <linux/module.h>
56#include <linux/moduleparam.h>
57#include <linux/netdevice.h>
58#include <net/af_ieee802154.h>
59#include <net/ieee802154.h>
60#include <net/ieee802154_netdev.h>
61#include <net/ipv6.h>
62
63#include "6lowpan.h"
64
65/* TTL uncompression values */
66static const u8 lowpan_ttl_values[] = {0, 1, 64, 255};
67
68static LIST_HEAD(lowpan_devices);
69
Alexander Smirnov44331fe2011-08-24 19:34:42 -070070/* private device info */
71struct lowpan_dev_info {
72 struct net_device *real_dev; /* real WPAN device ptr */
73 struct mutex dev_list_mtx; /* mutex for list ops */
Tony Cheneauababf382013-03-26 18:09:24 +000074 unsigned short fragment_tag;
Alexander Smirnov44331fe2011-08-24 19:34:42 -070075};
76
77struct lowpan_dev_record {
78 struct net_device *ldev;
79 struct list_head list;
80};
81
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +000082struct lowpan_fragment {
83 struct sk_buff *skb; /* skb to be assembled */
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +000084 u16 length; /* length to be assemled */
85 u32 bytes_rcv; /* bytes received */
86 u16 tag; /* current fragment tag */
87 struct timer_list timer; /* assembling timer */
88 struct list_head list; /* fragments list */
89};
90
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +000091static LIST_HEAD(lowpan_fragments);
alex.bluesman.smirnov@gmail.com4d27de12012-07-10 21:22:42 +000092static DEFINE_SPINLOCK(flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +000093
Alexander Smirnov44331fe2011-08-24 19:34:42 -070094static inline struct
95lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
96{
97 return netdev_priv(dev);
98}
99
100static inline void lowpan_address_flip(u8 *src, u8 *dest)
101{
102 int i;
103 for (i = 0; i < IEEE802154_ADDR_LEN; i++)
104 (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
105}
106
107/* list of all 6lowpan devices, uses for package delivering */
108/* print data in line */
109static inline void lowpan_raw_dump_inline(const char *caller, char *msg,
110 unsigned char *buf, int len)
111{
112#ifdef DEBUG
113 if (msg)
114 pr_debug("(%s) %s: ", caller, msg);
115 print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,
116 16, 1, buf, len, false);
117#endif /* DEBUG */
118}
119
120/*
121 * print data in a table format:
122 *
123 * addr: xx xx xx xx xx xx
124 * addr: xx xx xx xx xx xx
125 * ...
126 */
127static inline void lowpan_raw_dump_table(const char *caller, char *msg,
128 unsigned char *buf, int len)
129{
130#ifdef DEBUG
131 if (msg)
132 pr_debug("(%s) %s:\n", caller, msg);
133 print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET,
134 16, 1, buf, len, false);
135#endif /* DEBUG */
136}
137
138static u8
139lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr,
140 const unsigned char *lladdr)
141{
142 u8 val = 0;
143
144 if (is_addr_mac_addr_based(ipaddr, lladdr))
145 val = 3; /* 0-bits */
146 else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
147 /* compress IID to 16 bits xxxx::XXXX */
148 memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
149 *hc06_ptr += 2;
150 val = 2; /* 16-bits */
151 } else {
152 /* do not compress IID => xxxx::IID */
153 memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
154 *hc06_ptr += 8;
155 val = 1; /* 64-bits */
156 }
157
158 return rol8(val, shift);
159}
160
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700161/*
Alexander Aringce2463b2013-08-16 21:59:58 +0200162 * Uncompress address function for source and
163 * destination address(non-multicast).
164 *
165 * address_mode is sam value or dam value.
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700166 */
167static int
Alexander Aringce2463b2013-08-16 21:59:58 +0200168lowpan_uncompress_addr(struct sk_buff *skb,
169 struct in6_addr *ipaddr,
170 const u8 address_mode,
171 const struct ieee802154_addr *lladdr)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700172{
Alexander Aringce2463b2013-08-16 21:59:58 +0200173 bool fail;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700174
Alexander Aringce2463b2013-08-16 21:59:58 +0200175 switch (address_mode) {
176 case LOWPAN_IPHC_ADDR_00:
177 /* for global link addresses */
178 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
179 break;
180 case LOWPAN_IPHC_ADDR_01:
181 /* fe:80::XXXX:XXXX:XXXX:XXXX */
182 ipaddr->s6_addr[0] = 0xFE;
183 ipaddr->s6_addr[1] = 0x80;
184 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
185 break;
186 case LOWPAN_IPHC_ADDR_02:
187 /* fe:80::ff:fe00:XXXX */
188 ipaddr->s6_addr[0] = 0xFE;
189 ipaddr->s6_addr[1] = 0x80;
190 ipaddr->s6_addr[11] = 0xFF;
191 ipaddr->s6_addr[12] = 0xFE;
192 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
193 break;
194 case LOWPAN_IPHC_ADDR_03:
195 fail = false;
196 switch (lladdr->addr_type) {
197 case IEEE802154_ADDR_LONG:
198 /* fe:80::XXXX:XXXX:XXXX:XXXX
199 * \_________________/
200 * hwaddr
201 */
202 ipaddr->s6_addr[0] = 0xFE;
203 ipaddr->s6_addr[1] = 0x80;
204 memcpy(&ipaddr->s6_addr[8], lladdr->hwaddr,
205 IEEE802154_ADDR_LEN);
206 /* second bit-flip (Universe/Local)
207 * is done according RFC2464
208 */
209 ipaddr->s6_addr[8] ^= 0x02;
210 break;
211 case IEEE802154_ADDR_SHORT:
212 /* fe:80::ff:fe00:XXXX
213 * \__/
214 * short_addr
215 *
216 * Universe/Local bit is zero.
217 */
218 ipaddr->s6_addr[0] = 0xFE;
219 ipaddr->s6_addr[1] = 0x80;
220 ipaddr->s6_addr[11] = 0xFF;
221 ipaddr->s6_addr[12] = 0xFE;
222 ipaddr->s6_addr16[7] = htons(lladdr->short_addr);
223 break;
224 default:
225 pr_debug("Invalid addr_type set\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700226 return -EINVAL;
Alexander Aringce2463b2013-08-16 21:59:58 +0200227 }
228 break;
229 default:
230 pr_debug("Invalid address mode value: 0x%x\n", address_mode);
231 return -EINVAL;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700232 }
233
Alexander Aringce2463b2013-08-16 21:59:58 +0200234 if (fail) {
235 pr_debug("Failed to fetch skb data\n");
236 return -EIO;
237 }
238
239 lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 addr is:\n",
240 ipaddr->s6_addr, 16);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700241
242 return 0;
243}
244
Alexander Aring65d892c2013-08-16 21:59:59 +0200245/* Uncompress address function for source context
246 * based address(non-multicast).
247 */
248static int
249lowpan_uncompress_context_based_src_addr(struct sk_buff *skb,
250 struct in6_addr *ipaddr,
251 const u8 sam)
252{
253 switch (sam) {
254 case LOWPAN_IPHC_ADDR_00:
255 /* unspec address ::
256 * Do nothing, address is already ::
257 */
258 break;
259 case LOWPAN_IPHC_ADDR_01:
260 /* TODO */
261 case LOWPAN_IPHC_ADDR_02:
262 /* TODO */
263 case LOWPAN_IPHC_ADDR_03:
264 /* TODO */
265 netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam);
266 return -EINVAL;
267 default:
268 pr_debug("Invalid sam value: 0x%x\n", sam);
269 return -EINVAL;
270 }
271
272 lowpan_raw_dump_inline(NULL,
273 "Reconstructed context based ipv6 src addr is:\n",
274 ipaddr->s6_addr, 16);
275
276 return 0;
277}
278
Alexander Aring84c2e7b2013-08-16 21:59:57 +0200279/* Uncompress function for multicast destination address,
280 * when M bit is set.
281 */
282static int
283lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
284 struct in6_addr *ipaddr,
285 const u8 dam)
286{
287 bool fail;
288
289 switch (dam) {
290 case LOWPAN_IPHC_DAM_00:
291 /* 00: 128 bits. The full address
292 * is carried in-line.
293 */
294 fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
295 break;
296 case LOWPAN_IPHC_DAM_01:
297 /* 01: 48 bits. The address takes
298 * the form ffXX::00XX:XXXX:XXXX.
299 */
300 ipaddr->s6_addr[0] = 0xFF;
301 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
302 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
303 break;
304 case LOWPAN_IPHC_DAM_10:
305 /* 10: 32 bits. The address takes
306 * the form ffXX::00XX:XXXX.
307 */
308 ipaddr->s6_addr[0] = 0xFF;
309 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
310 fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
311 break;
312 case LOWPAN_IPHC_DAM_11:
313 /* 11: 8 bits. The address takes
314 * the form ff02::00XX.
315 */
316 ipaddr->s6_addr[0] = 0xFF;
317 ipaddr->s6_addr[1] = 0x02;
318 fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
319 break;
320 default:
321 pr_debug("DAM value has a wrong value: 0x%x\n", dam);
322 return -EINVAL;
323 }
324
325 if (fail) {
326 pr_debug("Failed to fetch skb data\n");
327 return -EIO;
328 }
329
330 lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is:\n",
331 ipaddr->s6_addr, 16);
332
333 return 0;
334}
335
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000336static void
337lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
338{
339 struct udphdr *uh = udp_hdr(skb);
340
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000341 if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) ==
342 LOWPAN_NHC_UDP_4BIT_PORT) &&
343 ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) ==
344 LOWPAN_NHC_UDP_4BIT_PORT)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000345 pr_debug("UDP header: both ports compression to 4 bits\n");
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000346 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11;
347 **(hc06_ptr + 1) = /* subtraction is faster */
348 (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) +
349 ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4));
350 *hc06_ptr += 2;
351 } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) ==
352 LOWPAN_NHC_UDP_8BIT_PORT) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000353 pr_debug("UDP header: remove 8 bits of dest\n");
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000354 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01;
355 memcpy(*hc06_ptr + 1, &uh->source, 2);
356 **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT);
357 *hc06_ptr += 4;
358 } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) ==
359 LOWPAN_NHC_UDP_8BIT_PORT) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000360 pr_debug("UDP header: remove 8 bits of source\n");
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000361 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10;
362 memcpy(*hc06_ptr + 1, &uh->dest, 2);
363 **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT);
364 *hc06_ptr += 4;
365 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000366 pr_debug("UDP header: can't compress\n");
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000367 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00;
368 memcpy(*hc06_ptr + 1, &uh->source, 2);
369 memcpy(*hc06_ptr + 3, &uh->dest, 2);
370 *hc06_ptr += 5;
371 }
372
373 /* checksum is always inline */
374 memcpy(*hc06_ptr, &uh->check, 2);
375 *hc06_ptr += 2;
Tony Cheneau24363b62013-03-25 17:59:32 +0000376
377 /* skip the UDP header */
378 skb_pull(skb, sizeof(struct udphdr));
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000379}
380
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000381static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700382{
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000383 if (unlikely(!pskb_may_pull(skb, 1)))
384 return -EINVAL;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700385
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000386 *val = skb->data[0];
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700387 skb_pull(skb, 1);
388
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000389 return 0;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700390}
391
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000392static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000393{
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000394 if (unlikely(!pskb_may_pull(skb, 2)))
395 return -EINVAL;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000396
Tony Cheneau45760392012-07-11 06:51:15 +0000397 *val = (skb->data[0] << 8) | skb->data[1];
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000398 skb_pull(skb, 2);
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000399
400 return 0;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000401}
402
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000403static int
Tony Cheneau24363b62013-03-25 17:59:32 +0000404lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000405{
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000406 u8 tmp;
407
Tony Cheneaud4787a12012-07-11 06:51:14 +0000408 if (!uh)
409 goto err;
410
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000411 if (lowpan_fetch_skb_u8(skb, &tmp))
412 goto err;
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000413
414 if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000415 pr_debug("UDP header uncompression\n");
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000416 switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
417 case LOWPAN_NHC_UDP_CS_P_00:
418 memcpy(&uh->source, &skb->data[0], 2);
419 memcpy(&uh->dest, &skb->data[2], 2);
420 skb_pull(skb, 4);
421 break;
422 case LOWPAN_NHC_UDP_CS_P_01:
423 memcpy(&uh->source, &skb->data[0], 2);
424 uh->dest =
425 skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT;
426 skb_pull(skb, 3);
427 break;
428 case LOWPAN_NHC_UDP_CS_P_10:
429 uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT;
430 memcpy(&uh->dest, &skb->data[1], 2);
431 skb_pull(skb, 3);
432 break;
433 case LOWPAN_NHC_UDP_CS_P_11:
434 uh->source =
435 LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4);
436 uh->dest =
437 LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f);
438 skb_pull(skb, 1);
439 break;
440 default:
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000441 pr_debug("ERROR: unknown UDP format\n");
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000442 goto err;
443 break;
444 }
445
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000446 pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
447 uh->source, uh->dest);
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000448
449 /* copy checksum */
450 memcpy(&uh->check, &skb->data[0], 2);
451 skb_pull(skb, 2);
Tony Cheneau24363b62013-03-25 17:59:32 +0000452
453 /*
454 * UDP lenght needs to be infered from the lower layers
455 * here, we obtain the hint from the remaining size of the
456 * frame
457 */
458 uh->len = htons(skb->len + sizeof(struct udphdr));
459 pr_debug("uncompressed UDP length: src = %d", uh->len);
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000460 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000461 pr_debug("ERROR: unsupported NH format\n");
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000462 goto err;
463 }
464
465 return 0;
466err:
467 return -EINVAL;
468}
469
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700470static int lowpan_header_create(struct sk_buff *skb,
471 struct net_device *dev,
472 unsigned short type, const void *_daddr,
Eric Dumazet95c96172012-04-15 05:58:06 +0000473 const void *_saddr, unsigned int len)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700474{
475 u8 tmp, iphc0, iphc1, *hc06_ptr;
476 struct ipv6hdr *hdr;
477 const u8 *saddr = _saddr;
478 const u8 *daddr = _daddr;
Alexander Aringfc4e98d2013-02-05 10:23:43 +0000479 u8 head[100];
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700480 struct ieee802154_addr sa, da;
481
Alexander Aringfc4e98d2013-02-05 10:23:43 +0000482 /* TODO:
483 * if this package isn't ipv6 one, where should it be routed?
484 */
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700485 if (type != ETH_P_IPV6)
486 return 0;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700487
488 hdr = ipv6_hdr(skb);
489 hc06_ptr = head + 2;
490
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000491 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n"
492 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
493 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700494
495 lowpan_raw_dump_table(__func__, "raw skb network header dump",
496 skb_network_header(skb), sizeof(struct ipv6hdr));
497
498 if (!saddr)
499 saddr = dev->dev_addr;
500
501 lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
502
503 /*
504 * As we copy some bit-length fields, in the IPHC encoding bytes,
505 * we sometimes use |=
506 * If the field is 0, and the current bit value in memory is 1,
507 * this does not work. We therefore reset the IPHC encoding here
508 */
509 iphc0 = LOWPAN_DISPATCH_IPHC;
510 iphc1 = 0;
511
512 /* TODO: context lookup */
513
514 lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
515
516 /*
517 * Traffic class, flow label
518 * If flow label is 0, compress it. If traffic class is 0, compress it
519 * We have to process both in the same time as the offset of traffic
520 * class depends on the presence of version and flow label
521 */
522
523 /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
524 tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
525 tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
526
527 if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
528 (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
529 /* flow label can be compressed */
530 iphc0 |= LOWPAN_IPHC_FL_C;
531 if ((hdr->priority == 0) &&
532 ((hdr->flow_lbl[0] & 0xF0) == 0)) {
533 /* compress (elide) all */
534 iphc0 |= LOWPAN_IPHC_TC_C;
535 } else {
536 /* compress only the flow label */
537 *hc06_ptr = tmp;
538 hc06_ptr += 1;
539 }
540 } else {
541 /* Flow label cannot be compressed */
542 if ((hdr->priority == 0) &&
543 ((hdr->flow_lbl[0] & 0xF0) == 0)) {
544 /* compress only traffic class */
545 iphc0 |= LOWPAN_IPHC_TC_C;
546 *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
547 memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
548 hc06_ptr += 3;
549 } else {
550 /* compress nothing */
551 memcpy(hc06_ptr, &hdr, 4);
552 /* replace the top byte with new ECN | DSCP format */
553 *hc06_ptr = tmp;
554 hc06_ptr += 4;
555 }
556 }
557
558 /* NOTE: payload length is always compressed */
559
560 /* Next Header is compress if UDP */
561 if (hdr->nexthdr == UIP_PROTO_UDP)
562 iphc0 |= LOWPAN_IPHC_NH_C;
563
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700564 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
565 *hc06_ptr = hdr->nexthdr;
566 hc06_ptr += 1;
567 }
568
569 /*
570 * Hop limit
571 * if 1: compress, encoding is 01
572 * if 64: compress, encoding is 10
573 * if 255: compress, encoding is 11
574 * else do not compress
575 */
576 switch (hdr->hop_limit) {
577 case 1:
578 iphc0 |= LOWPAN_IPHC_TTL_1;
579 break;
580 case 64:
581 iphc0 |= LOWPAN_IPHC_TTL_64;
582 break;
583 case 255:
584 iphc0 |= LOWPAN_IPHC_TTL_255;
585 break;
586 default:
587 *hc06_ptr = hdr->hop_limit;
alex.bluesman.smirnov@gmail.com5c00c0c2012-06-25 03:49:02 +0000588 hc06_ptr += 1;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700589 break;
590 }
591
592 /* source address compression */
593 if (is_addr_unspecified(&hdr->saddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000594 pr_debug("source address is unspecified, setting SAC\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700595 iphc1 |= LOWPAN_IPHC_SAC;
596 /* TODO: context lookup */
597 } else if (is_addr_link_local(&hdr->saddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000598 pr_debug("source address is link-local\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700599 iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
600 LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
601 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000602 pr_debug("send the full source address\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700603 memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
604 hc06_ptr += 16;
605 }
606
607 /* destination address compression */
608 if (is_addr_mcast(&hdr->daddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000609 pr_debug("destination address is multicast: ");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700610 iphc1 |= LOWPAN_IPHC_M;
611 if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
612 pr_debug("compressed to 1 octet\n");
613 iphc1 |= LOWPAN_IPHC_DAM_11;
614 /* use last byte */
615 *hc06_ptr = hdr->daddr.s6_addr[15];
616 hc06_ptr += 1;
617 } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
618 pr_debug("compressed to 4 octets\n");
619 iphc1 |= LOWPAN_IPHC_DAM_10;
620 /* second byte + the last three */
621 *hc06_ptr = hdr->daddr.s6_addr[1];
622 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
623 hc06_ptr += 4;
624 } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
625 pr_debug("compressed to 6 octets\n");
626 iphc1 |= LOWPAN_IPHC_DAM_01;
627 /* second byte + the last five */
628 *hc06_ptr = hdr->daddr.s6_addr[1];
629 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
630 hc06_ptr += 6;
631 } else {
632 pr_debug("using full address\n");
633 iphc1 |= LOWPAN_IPHC_DAM_00;
634 memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
635 hc06_ptr += 16;
636 }
637 } else {
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700638 /* TODO: context lookup */
639 if (is_addr_link_local(&hdr->daddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000640 pr_debug("dest address is unicast and link-local\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700641 iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
642 LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
643 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000644 pr_debug("dest address is unicast: using full one\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700645 memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
646 hc06_ptr += 16;
647 }
648 }
649
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000650 /* UDP header compression */
651 if (hdr->nexthdr == UIP_PROTO_UDP)
652 lowpan_compress_udp_header(&hc06_ptr, skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700653
654 head[0] = iphc0;
655 head[1] = iphc1;
656
657 skb_pull(skb, sizeof(struct ipv6hdr));
658 memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
659
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700660 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
661 skb->len);
662
663 /*
664 * NOTE1: I'm still unsure about the fact that compression and WPAN
665 * header are created here and not later in the xmit. So wait for
666 * an opinion of net maintainers.
667 */
668 /*
669 * NOTE2: to be absolutely correct, we must derive PANid information
670 * from MAC subif of the 'dev' and 'real_dev' network devices, but
671 * this isn't implemented in mainline yet, so currently we assign 0xff
672 */
673 {
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000674 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
Tony Cheneauc7d0ab22013-03-25 17:59:30 +0000675 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000676
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700677 /* prepare wpan address data */
678 sa.addr_type = IEEE802154_ADDR_LONG;
Tony Cheneau43de7aa2013-03-25 17:59:31 +0000679 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700680
Tony Cheneau43de7aa2013-03-25 17:59:31 +0000681 memcpy(&(sa.hwaddr), saddr, 8);
682 /* intra-PAN communications */
683 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
684
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000685 /*
686 * if the destination address is the broadcast address, use the
687 * corresponding short address
688 */
689 if (lowpan_is_addr_broadcast(daddr)) {
690 da.addr_type = IEEE802154_ADDR_SHORT;
691 da.short_addr = IEEE802154_ADDR_BROADCAST;
692 } else {
693 da.addr_type = IEEE802154_ADDR_LONG;
Tony Cheneau6bdeaba2013-03-26 18:09:25 +0000694 memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000695
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000696 /* request acknowledgment */
Tony Cheneauf333a152013-03-25 17:59:23 +0000697 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000698 }
Tony Cheneauf333a152013-03-25 17:59:23 +0000699
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700700 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
701 type, (void *)&da, (void *)&sa, skb->len);
702 }
703}
704
Alan Ott0c446212013-01-16 19:09:47 +0000705static int lowpan_give_skb_to_devices(struct sk_buff *skb)
706{
707 struct lowpan_dev_record *entry;
708 struct sk_buff *skb_cp;
709 int stat = NET_RX_SUCCESS;
710
711 rcu_read_lock();
712 list_for_each_entry_rcu(entry, &lowpan_devices, list)
713 if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
714 skb_cp = skb_copy(skb, GFP_ATOMIC);
715 if (!skb_cp) {
716 stat = -ENOMEM;
717 break;
718 }
719
720 skb_cp->dev = entry->ldev;
721 stat = netif_rx(skb_cp);
722 }
723 rcu_read_unlock();
724
725 return stat;
726}
727
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700728static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
729{
730 struct sk_buff *new;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700731 int stat = NET_RX_SUCCESS;
732
733 new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
alex.bluesman.smirnov@gmail.comdcef1152011-09-01 03:55:15 +0000734 GFP_ATOMIC);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700735 kfree_skb(skb);
736
alex.bluesman.smirnov@gmail.comdcef1152011-09-01 03:55:15 +0000737 if (!new)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700738 return -ENOMEM;
739
740 skb_push(new, sizeof(struct ipv6hdr));
741 skb_reset_network_header(new);
742 skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
743
744 new->protocol = htons(ETH_P_IPV6);
745 new->pkt_type = PACKET_HOST;
746
Alan Ott0c446212013-01-16 19:09:47 +0000747 stat = lowpan_give_skb_to_devices(new);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700748
749 kfree_skb(new);
750
751 return stat;
752}
753
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000754static void lowpan_fragment_timer_expired(unsigned long entry_addr)
755{
756 struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
757
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000758 pr_debug("timer expired for frame with tag %d\n", entry->tag);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000759
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000760 list_del(&entry->list);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000761 dev_kfree_skb(entry->skb);
762 kfree(entry);
763}
764
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000765static struct lowpan_fragment *
Tony Cheneaud991b982013-03-25 17:59:26 +0000766lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 tag)
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000767{
768 struct lowpan_fragment *frame;
769
770 frame = kzalloc(sizeof(struct lowpan_fragment),
771 GFP_ATOMIC);
772 if (!frame)
773 goto frame_err;
774
775 INIT_LIST_HEAD(&frame->list);
776
Tony Cheneau5e968552012-07-11 06:51:16 +0000777 frame->length = len;
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000778 frame->tag = tag;
779
780 /* allocate buffer for frame assembling */
alex.bluesman.smirnov@gmail.com79ff1db2012-07-10 21:22:45 +0000781 frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length +
782 sizeof(struct ipv6hdr));
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000783
784 if (!frame->skb)
785 goto skb_err;
786
787 frame->skb->priority = skb->priority;
788 frame->skb->dev = skb->dev;
789
790 /* reserve headroom for uncompressed ipv6 header */
791 skb_reserve(frame->skb, sizeof(struct ipv6hdr));
792 skb_put(frame->skb, frame->length);
793
David Hauweele31afe1f2013-08-16 21:59:55 +0200794 /* copy the first control block to keep a
795 * trace of the link-layer addresses in case
796 * of a link-local compressed address
797 */
798 memcpy(frame->skb->cb, skb->cb, sizeof(skb->cb));
799
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000800 init_timer(&frame->timer);
801 /* time out is the same as for ipv6 - 60 sec */
802 frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
803 frame->timer.data = (unsigned long)frame;
804 frame->timer.function = lowpan_fragment_timer_expired;
805
806 add_timer(&frame->timer);
807
808 list_add_tail(&frame->list, &lowpan_fragments);
809
810 return frame;
811
812skb_err:
813 kfree(frame);
814frame_err:
815 return NULL;
816}
817
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700818static int
819lowpan_process_data(struct sk_buff *skb)
820{
Alexander Aring84ce1ddf2013-08-16 21:59:54 +0200821 struct ipv6hdr hdr = {};
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700822 u8 tmp, iphc0, iphc1, num_context = 0;
Alexander Aringce2463b2013-08-16 21:59:58 +0200823 const struct ieee802154_addr *_saddr, *_daddr;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700824 int err;
825
826 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
827 skb->len);
828 /* at least two bytes will be used for the encoding */
829 if (skb->len < 2)
830 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000831
832 if (lowpan_fetch_skb_u8(skb, &iphc0))
833 goto drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000834
835 /* fragments assembling */
836 switch (iphc0 & LOWPAN_DISPATCH_MASK) {
837 case LOWPAN_DISPATCH_FRAG1:
838 case LOWPAN_DISPATCH_FRAGN:
839 {
840 struct lowpan_fragment *frame;
Tony Cheneau5e968552012-07-11 06:51:16 +0000841 /* slen stores the rightmost 8 bits of the 11 bits length */
Tony Cheneaud991b982013-03-25 17:59:26 +0000842 u8 slen, offset = 0;
Tony Cheneau5e968552012-07-11 06:51:16 +0000843 u16 len, tag;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000844 bool found = false;
845
Tony Cheneau5e968552012-07-11 06:51:16 +0000846 if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000847 lowpan_fetch_skb_u16(skb, &tag)) /* fragment tag */
848 goto drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000849
Tony Cheneau5e968552012-07-11 06:51:16 +0000850 /* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
851 len = ((iphc0 & 7) << 8) | slen;
852
Tony Cheneau9da29242013-03-25 17:59:27 +0000853 if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
854 pr_debug("%s received a FRAG1 packet (tag: %d, "
855 "size of the entire IP packet: %d)",
856 __func__, tag, len);
857 } else { /* FRAGN */
Tony Cheneaud991b982013-03-25 17:59:26 +0000858 if (lowpan_fetch_skb_u8(skb, &offset))
859 goto unlock_and_drop;
Tony Cheneau9da29242013-03-25 17:59:27 +0000860 pr_debug("%s received a FRAGN packet (tag: %d, "
861 "size of the entire IP packet: %d, "
862 "offset: %d)", __func__, tag, len, offset * 8);
Tony Cheneaud991b982013-03-25 17:59:26 +0000863 }
864
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000865 /*
866 * check if frame assembling with the same tag is
867 * already in progress
868 */
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000869 spin_lock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000870
871 list_for_each_entry(frame, &lowpan_fragments, list)
872 if (frame->tag == tag) {
873 found = true;
874 break;
875 }
876
877 /* alloc new frame structure */
878 if (!found) {
Tony Cheneau9da29242013-03-25 17:59:27 +0000879 pr_debug("%s first fragment received for tag %d, "
880 "begin packet reassembly", __func__, tag);
Tony Cheneau5e968552012-07-11 06:51:16 +0000881 frame = lowpan_alloc_new_frame(skb, len, tag);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000882 if (!frame)
883 goto unlock_and_drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000884 }
885
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000886 /* if payload fits buffer, copy it */
887 if (likely((offset * 8 + skb->len) <= frame->length))
888 skb_copy_to_linear_data_offset(frame->skb, offset * 8,
889 skb->data, skb->len);
890 else
891 goto unlock_and_drop;
892
893 frame->bytes_rcv += skb->len;
894
895 /* frame assembling complete */
896 if ((frame->bytes_rcv == frame->length) &&
897 frame->timer.expires > jiffies) {
898 /* if timer haven't expired - first of all delete it */
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000899 del_timer_sync(&frame->timer);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000900 list_del(&frame->list);
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000901 spin_unlock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000902
Tony Cheneau9da29242013-03-25 17:59:27 +0000903 pr_debug("%s successfully reassembled fragment "
904 "(tag %d)", __func__, tag);
905
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000906 dev_kfree_skb(skb);
907 skb = frame->skb;
908 kfree(frame);
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000909
910 if (lowpan_fetch_skb_u8(skb, &iphc0))
Dan Carpenter747cf6e2012-06-26 20:53:09 +0000911 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000912
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000913 break;
914 }
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000915 spin_unlock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000916
917 return kfree_skb(skb), 0;
918 }
919 default:
920 break;
921 }
922
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000923 if (lowpan_fetch_skb_u8(skb, &iphc1))
924 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700925
Alexander Aringce2463b2013-08-16 21:59:58 +0200926 _saddr = &mac_cb(skb)->sa;
927 _daddr = &mac_cb(skb)->da;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700928
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000929 pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700930
931 /* another if the CID flag is set */
932 if (iphc1 & LOWPAN_IPHC_CID) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000933 pr_debug("CID flag is set, increase header with one\n");
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000934 if (lowpan_fetch_skb_u8(skb, &num_context))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700935 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700936 }
937
938 hdr.version = 6;
939
940 /* Traffic Class and Flow Label */
941 switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
942 /*
943 * Traffic Class and FLow Label carried in-line
944 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
945 */
946 case 0: /* 00b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000947 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700948 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000949
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700950 memcpy(&hdr.flow_lbl, &skb->data[0], 3);
951 skb_pull(skb, 3);
952 hdr.priority = ((tmp >> 2) & 0x0f);
953 hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
954 (hdr.flow_lbl[0] & 0x0f);
955 break;
956 /*
957 * Traffic class carried in-line
958 * ECN + DSCP (1 byte), Flow Label is elided
959 */
960 case 1: /* 10b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000961 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700962 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000963
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700964 hdr.priority = ((tmp >> 2) & 0x0f);
965 hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700966 break;
967 /*
968 * Flow Label carried in-line
969 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
970 */
971 case 2: /* 01b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000972 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700973 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000974
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700975 hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
976 memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
977 skb_pull(skb, 2);
978 break;
979 /* Traffic Class and Flow Label are elided */
980 case 3: /* 11b */
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700981 break;
982 default:
983 break;
984 }
985
986 /* Next Header */
987 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
988 /* Next header is carried inline */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000989 if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700990 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000991
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000992 pr_debug("NH flag is set, next header carried inline: %02x\n",
993 hdr.nexthdr);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700994 }
995
996 /* Hop Limit */
997 if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
998 hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
999 else {
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +00001000 if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001001 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001002 }
1003
1004 /* Extract SAM to the tmp variable */
1005 tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
1006
Alexander Aring65d892c2013-08-16 21:59:59 +02001007 if (iphc1 & LOWPAN_IPHC_SAC) {
1008 /* Source address context based uncompression */
1009 pr_debug("SAC bit is set. Handle context based source address.\n");
1010 err = lowpan_uncompress_context_based_src_addr(
1011 skb, &hdr.saddr, tmp);
1012 } else {
1013 /* Source address uncompression */
1014 pr_debug("source address stateless compression\n");
1015 err = lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr);
1016 }
1017
1018 /* Check on error of previous branch */
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001019 if (err)
1020 goto drop;
1021
1022 /* Extract DAM to the tmp variable */
1023 tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
1024
1025 /* check for Multicast Compression */
1026 if (iphc1 & LOWPAN_IPHC_M) {
1027 if (iphc1 & LOWPAN_IPHC_DAC) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001028 pr_debug("dest: context-based mcast compression\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001029 /* TODO: implement this */
1030 } else {
Alexander Aring84c2e7b2013-08-16 21:59:57 +02001031 err = lowpan_uncompress_multicast_daddr(
1032 skb, &hdr.daddr, tmp);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001033 if (err)
1034 goto drop;
1035 }
1036 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001037 pr_debug("dest: stateless compression\n");
Alexander Aringce2463b2013-08-16 21:59:58 +02001038 err = lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001039 if (err)
1040 goto drop;
1041 }
1042
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +00001043 /* UDP data uncompression */
Tony Cheneauf5c20f52013-03-25 17:59:22 +00001044 if (iphc0 & LOWPAN_IPHC_NH_C) {
Tony Cheneau24363b62013-03-25 17:59:32 +00001045 struct udphdr uh;
1046 struct sk_buff *new;
1047 if (lowpan_uncompress_udp_header(skb, &uh))
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +00001048 goto drop;
Tony Cheneau24363b62013-03-25 17:59:32 +00001049
1050 /*
1051 * replace the compressed UDP head by the uncompressed UDP
1052 * header
1053 */
1054 new = skb_copy_expand(skb, sizeof(struct udphdr),
1055 skb_tailroom(skb), GFP_ATOMIC);
1056 kfree_skb(skb);
1057
1058 if (!new)
1059 return -ENOMEM;
1060
1061 skb = new;
1062
1063 skb_push(skb, sizeof(struct udphdr));
1064 skb_reset_transport_header(skb);
1065 skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
1066
1067 lowpan_raw_dump_table(__func__, "raw UDP header dump",
1068 (u8 *)&uh, sizeof(uh));
1069
Tony Cheneauf5c20f52013-03-25 17:59:22 +00001070 hdr.nexthdr = UIP_PROTO_UDP;
1071 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001072
1073 /* Not fragmented package */
1074 hdr.payload_len = htons(skb->len);
1075
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001076 pr_debug("skb headroom size = %d, data length = %d\n",
1077 skb_headroom(skb), skb->len);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001078
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001079 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t"
1080 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001081 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
1082
1083 lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
1084 sizeof(hdr));
1085 return lowpan_skb_deliver(skb, &hdr);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001086
1087unlock_and_drop:
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001088 spin_unlock_bh(&flist_lock);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001089drop:
Dan Carpenter90d09632011-08-30 03:45:52 +00001090 kfree_skb(skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001091 return -EINVAL;
1092}
1093
alex.bluesman.smirnov@gmail.com42c36292012-07-01 19:58:46 +00001094static int lowpan_set_address(struct net_device *dev, void *p)
1095{
1096 struct sockaddr *sa = p;
1097
1098 if (netif_running(dev))
1099 return -EBUSY;
1100
1101 /* TODO: validate addr */
1102 memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);
1103
1104 return 0;
1105}
1106
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001107static int lowpan_get_mac_header_length(struct sk_buff *skb)
1108{
1109 /*
1110 * Currently long addressing mode is supported only, so the overall
1111 * header size is 21:
1112 * FC SeqNum DPAN DA SA Sec
1113 * 2 + 1 + 2 + 8 + 8 + 0 = 21
1114 */
1115 return 21;
1116}
1117
1118static int
1119lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
Tony Cheneaud991b982013-03-25 17:59:26 +00001120 int mlen, int plen, int offset, int type)
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001121{
1122 struct sk_buff *frag;
1123 int hlen, ret;
1124
Tony Cheneaud991b982013-03-25 17:59:26 +00001125 hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
1126 LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001127
1128 lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
1129
1130 frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE);
1131 if (!frag)
1132 return -ENOMEM;
1133
1134 frag->priority = skb->priority;
1135 frag->dev = skb->dev;
1136
1137 /* copy header, MFR and payload */
1138 memcpy(skb_put(frag, mlen), skb->data, mlen);
1139 memcpy(skb_put(frag, hlen), head, hlen);
1140
1141 if (plen)
1142 skb_copy_from_linear_data_offset(skb, offset + mlen,
1143 skb_put(frag, plen), plen);
1144
1145 lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
1146 frag->len);
1147
1148 ret = dev_queue_xmit(frag);
1149
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001150 return ret;
1151}
1152
1153static int
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001154lowpan_skb_fragmentation(struct sk_buff *skb, struct net_device *dev)
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001155{
1156 int err, header_length, payload_length, tag, offset = 0;
1157 u8 head[5];
1158
1159 header_length = lowpan_get_mac_header_length(skb);
1160 payload_length = skb->len - header_length;
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001161 tag = lowpan_dev_info(dev)->fragment_tag++;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001162
1163 /* first fragment header */
Tony Cheneau5e968552012-07-11 06:51:16 +00001164 head[0] = LOWPAN_DISPATCH_FRAG1 | ((payload_length >> 8) & 0x7);
1165 head[1] = payload_length & 0xff;
Tony Cheneau45760392012-07-11 06:51:15 +00001166 head[2] = tag >> 8;
1167 head[3] = tag & 0xff;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001168
Tony Cheneaud991b982013-03-25 17:59:26 +00001169 err = lowpan_fragment_xmit(skb, head, header_length, LOWPAN_FRAG_SIZE,
1170 0, LOWPAN_DISPATCH_FRAG1);
1171
Tony Cheneau9da29242013-03-25 17:59:27 +00001172 if (err) {
1173 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
1174 __func__, tag);
Tony Cheneaud991b982013-03-25 17:59:26 +00001175 goto exit;
Tony Cheneau9da29242013-03-25 17:59:27 +00001176 }
Tony Cheneaud991b982013-03-25 17:59:26 +00001177
1178 offset = LOWPAN_FRAG_SIZE;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001179
1180 /* next fragment header */
1181 head[0] &= ~LOWPAN_DISPATCH_FRAG1;
1182 head[0] |= LOWPAN_DISPATCH_FRAGN;
1183
1184 while ((payload_length - offset > 0) && (err >= 0)) {
1185 int len = LOWPAN_FRAG_SIZE;
1186
1187 head[4] = offset / 8;
1188
1189 if (payload_length - offset < len)
1190 len = payload_length - offset;
1191
1192 err = lowpan_fragment_xmit(skb, head, header_length,
Tony Cheneaud991b982013-03-25 17:59:26 +00001193 len, offset, LOWPAN_DISPATCH_FRAGN);
Tony Cheneau9da29242013-03-25 17:59:27 +00001194 if (err) {
1195 pr_debug("%s unable to send a subsequent FRAGN packet "
1196 "(tag: %d, offset: %d", __func__, tag, offset);
Tony Cheneaud991b982013-03-25 17:59:26 +00001197 goto exit;
Tony Cheneau9da29242013-03-25 17:59:27 +00001198 }
Tony Cheneaud991b982013-03-25 17:59:26 +00001199
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001200 offset += len;
1201 }
1202
Tony Cheneaud991b982013-03-25 17:59:26 +00001203exit:
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001204 return err;
1205}
1206
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001207static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
1208{
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001209 int err = -1;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001210
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001211 pr_debug("package xmit\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001212
1213 skb->dev = lowpan_dev_info(dev)->real_dev;
1214 if (skb->dev == NULL) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001215 pr_debug("ERROR: no real wpan device found\n");
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001216 goto error;
1217 }
1218
Alan Ottb333b7e2012-11-29 15:55:44 +00001219 /* Send directly if less than the MTU minus the 2 checksum bytes. */
1220 if (skb->len <= IEEE802154_MTU - IEEE802154_MFR_SIZE) {
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001221 err = dev_queue_xmit(skb);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001222 goto out;
1223 }
1224
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001225 pr_debug("frame is too big, fragmentation is needed\n");
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001226 err = lowpan_skb_fragmentation(skb, dev);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001227error:
1228 dev_kfree_skb(skb);
1229out:
Alan Ottfc52eea2013-04-03 04:00:58 +00001230 if (err)
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001231 pr_debug("ERROR: xmit failed\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001232
Alan Ottfc52eea2013-04-03 04:00:58 +00001233 return (err < 0) ? NET_XMIT_DROP : err;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001234}
1235
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001236static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
1237{
1238 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1239 return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
1240}
1241
1242static u16 lowpan_get_pan_id(const struct net_device *dev)
1243{
1244 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1245 return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
1246}
1247
1248static u16 lowpan_get_short_addr(const struct net_device *dev)
1249{
1250 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1251 return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
1252}
1253
Tony Cheneauc7d0ab22013-03-25 17:59:30 +00001254static u8 lowpan_get_dsn(const struct net_device *dev)
1255{
1256 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1257 return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
1258}
1259
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001260static struct header_ops lowpan_header_ops = {
1261 .create = lowpan_header_create,
1262};
1263
1264static const struct net_device_ops lowpan_netdev_ops = {
1265 .ndo_start_xmit = lowpan_xmit,
alex.bluesman.smirnov@gmail.com42c36292012-07-01 19:58:46 +00001266 .ndo_set_mac_address = lowpan_set_address,
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001267};
1268
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001269static struct ieee802154_mlme_ops lowpan_mlme = {
1270 .get_pan_id = lowpan_get_pan_id,
1271 .get_phy = lowpan_get_phy,
1272 .get_short_addr = lowpan_get_short_addr,
Tony Cheneauc7d0ab22013-03-25 17:59:30 +00001273 .get_dsn = lowpan_get_dsn,
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001274};
1275
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001276static void lowpan_setup(struct net_device *dev)
1277{
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001278 dev->addr_len = IEEE802154_ADDR_LEN;
1279 memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
1280 dev->type = ARPHRD_IEEE802154;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001281 /* Frame Control + Sequence Number + Address fields + Security Header */
1282 dev->hard_header_len = 2 + 1 + 20 + 14;
1283 dev->needed_tailroom = 2; /* FCS */
1284 dev->mtu = 1281;
1285 dev->tx_queue_len = 0;
alex.bluesman.smirnov@gmail.com4d039f62011-11-10 07:39:37 +00001286 dev->flags = IFF_BROADCAST | IFF_MULTICAST;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001287 dev->watchdog_timeo = 0;
1288
1289 dev->netdev_ops = &lowpan_netdev_ops;
1290 dev->header_ops = &lowpan_header_ops;
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001291 dev->ml_priv = &lowpan_mlme;
Alan Otta2dc375e2012-09-01 05:57:07 +00001292 dev->destructor = free_netdev;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001293}
1294
1295static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
1296{
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001297 if (tb[IFLA_ADDRESS]) {
1298 if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
1299 return -EINVAL;
1300 }
1301 return 0;
1302}
1303
1304static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1305 struct packet_type *pt, struct net_device *orig_dev)
1306{
Alan Otta437d272012-09-01 05:57:06 +00001307 struct sk_buff *local_skb;
1308
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001309 if (!netif_running(dev))
1310 goto drop;
1311
1312 if (dev->type != ARPHRD_IEEE802154)
1313 goto drop;
1314
1315 /* check that it's our buffer */
Alan Ottee21c7e2013-01-16 19:09:48 +00001316 if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
1317 /* Copy the packet so that the IPv6 header is
1318 * properly aligned.
1319 */
1320 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
1321 skb_tailroom(skb), GFP_ATOMIC);
Alan Otta437d272012-09-01 05:57:06 +00001322 if (!local_skb)
1323 goto drop;
Alan Otta437d272012-09-01 05:57:06 +00001324
Alan Ottee21c7e2013-01-16 19:09:48 +00001325 local_skb->protocol = htons(ETH_P_IPV6);
1326 local_skb->pkt_type = PACKET_HOST;
1327
1328 /* Pull off the 1-byte of 6lowpan header. */
1329 skb_pull(local_skb, 1);
1330 skb_reset_network_header(local_skb);
1331 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
1332
1333 lowpan_give_skb_to_devices(local_skb);
1334
1335 kfree_skb(local_skb);
Alan Otta437d272012-09-01 05:57:06 +00001336 kfree_skb(skb);
Alan Ottee21c7e2013-01-16 19:09:48 +00001337 } else {
1338 switch (skb->data[0] & 0xe0) {
1339 case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
1340 case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
1341 case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
1342 local_skb = skb_clone(skb, GFP_ATOMIC);
1343 if (!local_skb)
1344 goto drop;
1345 lowpan_process_data(local_skb);
1346
1347 kfree_skb(skb);
1348 break;
1349 default:
1350 break;
1351 }
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001352 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001353
1354 return NET_RX_SUCCESS;
1355
1356drop:
1357 kfree_skb(skb);
1358 return NET_RX_DROP;
1359}
1360
1361static int lowpan_newlink(struct net *src_net, struct net_device *dev,
1362 struct nlattr *tb[], struct nlattr *data[])
1363{
1364 struct net_device *real_dev;
1365 struct lowpan_dev_record *entry;
1366
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001367 pr_debug("adding new link\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001368
1369 if (!tb[IFLA_LINK])
1370 return -EINVAL;
1371 /* find and hold real wpan device */
1372 real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
1373 if (!real_dev)
1374 return -ENODEV;
1375
1376 lowpan_dev_info(dev)->real_dev = real_dev;
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001377 lowpan_dev_info(dev)->fragment_tag = 0;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001378 mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
1379
1380 entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
Dan Carpenterdc00fd42011-08-30 03:51:09 +00001381 if (!entry) {
1382 dev_put(real_dev);
1383 lowpan_dev_info(dev)->real_dev = NULL;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001384 return -ENOMEM;
Dan Carpenterdc00fd42011-08-30 03:51:09 +00001385 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001386
1387 entry->ldev = dev;
1388
1389 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1390 INIT_LIST_HEAD(&entry->list);
1391 list_add_tail(&entry->list, &lowpan_devices);
1392 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1393
1394 register_netdevice(dev);
1395
1396 return 0;
1397}
1398
1399static void lowpan_dellink(struct net_device *dev, struct list_head *head)
1400{
1401 struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
1402 struct net_device *real_dev = lowpan_dev->real_dev;
alex.bluesman.smirnov@gmail.com8deff4a2012-04-25 23:24:57 +00001403 struct lowpan_dev_record *entry, *tmp;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001404
1405 ASSERT_RTNL();
1406
1407 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
Dan Carpenteraec9db32011-08-30 03:46:40 +00001408 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001409 if (entry->ldev == dev) {
1410 list_del(&entry->list);
1411 kfree(entry);
1412 }
Dan Carpenteraec9db32011-08-30 03:46:40 +00001413 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001414 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1415
1416 mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
1417
1418 unregister_netdevice_queue(dev, head);
1419
1420 dev_put(real_dev);
1421}
1422
1423static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
1424 .kind = "lowpan",
1425 .priv_size = sizeof(struct lowpan_dev_info),
1426 .setup = lowpan_setup,
1427 .newlink = lowpan_newlink,
1428 .dellink = lowpan_dellink,
1429 .validate = lowpan_validate,
1430};
1431
1432static inline int __init lowpan_netlink_init(void)
1433{
1434 return rtnl_link_register(&lowpan_link_ops);
1435}
1436
David S. Millera07fdce2013-02-06 15:54:38 -05001437static inline void lowpan_netlink_fini(void)
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001438{
1439 rtnl_link_unregister(&lowpan_link_ops);
1440}
1441
Alan Otta2dc375e2012-09-01 05:57:07 +00001442static int lowpan_device_event(struct notifier_block *unused,
Jiri Pirko351638e2013-05-28 01:30:21 +00001443 unsigned long event, void *ptr)
Alan Otta2dc375e2012-09-01 05:57:07 +00001444{
Jiri Pirko351638e2013-05-28 01:30:21 +00001445 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
Alan Otta2dc375e2012-09-01 05:57:07 +00001446 LIST_HEAD(del_list);
1447 struct lowpan_dev_record *entry, *tmp;
1448
1449 if (dev->type != ARPHRD_IEEE802154)
1450 goto out;
1451
1452 if (event == NETDEV_UNREGISTER) {
1453 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1454 if (lowpan_dev_info(entry->ldev)->real_dev == dev)
1455 lowpan_dellink(entry->ldev, &del_list);
1456 }
1457
1458 unregister_netdevice_many(&del_list);
Peter Senna Tschudin4c835012012-09-18 07:10:43 +00001459 }
Alan Otta2dc375e2012-09-01 05:57:07 +00001460
1461out:
1462 return NOTIFY_DONE;
1463}
1464
1465static struct notifier_block lowpan_dev_notifier = {
1466 .notifier_call = lowpan_device_event,
1467};
1468
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001469static struct packet_type lowpan_packet_type = {
1470 .type = __constant_htons(ETH_P_IEEE802154),
1471 .func = lowpan_rcv,
1472};
1473
1474static int __init lowpan_init_module(void)
1475{
1476 int err = 0;
1477
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001478 err = lowpan_netlink_init();
1479 if (err < 0)
1480 goto out;
1481
1482 dev_add_pack(&lowpan_packet_type);
Alan Otta2dc375e2012-09-01 05:57:07 +00001483
1484 err = register_netdevice_notifier(&lowpan_dev_notifier);
1485 if (err < 0) {
1486 dev_remove_pack(&lowpan_packet_type);
1487 lowpan_netlink_fini();
1488 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001489out:
1490 return err;
1491}
1492
1493static void __exit lowpan_cleanup_module(void)
1494{
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001495 struct lowpan_fragment *frame, *tframe;
1496
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001497 lowpan_netlink_fini();
1498
1499 dev_remove_pack(&lowpan_packet_type);
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001500
Alan Otta2dc375e2012-09-01 05:57:07 +00001501 unregister_netdevice_notifier(&lowpan_dev_notifier);
1502
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001503 /* Now 6lowpan packet_type is removed, so no new fragments are
1504 * expected on RX, therefore that's the time to clean incomplete
1505 * fragments.
1506 */
1507 spin_lock_bh(&flist_lock);
1508 list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
1509 del_timer_sync(&frame->timer);
1510 list_del(&frame->list);
1511 dev_kfree_skb(frame->skb);
1512 kfree(frame);
1513 }
1514 spin_unlock_bh(&flist_lock);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001515}
1516
1517module_init(lowpan_init_module);
1518module_exit(lowpan_cleanup_module);
1519MODULE_LICENSE("GPL");
1520MODULE_ALIAS_RTNL_LINK("lowpan");