blob: 459e200c08a40e4e737798582524cd973064b143 [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;
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000443 }
444
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000445 pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
446 uh->source, uh->dest);
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000447
448 /* copy checksum */
449 memcpy(&uh->check, &skb->data[0], 2);
450 skb_pull(skb, 2);
Tony Cheneau24363b62013-03-25 17:59:32 +0000451
452 /*
453 * UDP lenght needs to be infered from the lower layers
454 * here, we obtain the hint from the remaining size of the
455 * frame
456 */
457 uh->len = htons(skb->len + sizeof(struct udphdr));
458 pr_debug("uncompressed UDP length: src = %d", uh->len);
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000459 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000460 pr_debug("ERROR: unsupported NH format\n");
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +0000461 goto err;
462 }
463
464 return 0;
465err:
466 return -EINVAL;
467}
468
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700469static int lowpan_header_create(struct sk_buff *skb,
470 struct net_device *dev,
471 unsigned short type, const void *_daddr,
Eric Dumazet95c96172012-04-15 05:58:06 +0000472 const void *_saddr, unsigned int len)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700473{
474 u8 tmp, iphc0, iphc1, *hc06_ptr;
475 struct ipv6hdr *hdr;
476 const u8 *saddr = _saddr;
477 const u8 *daddr = _daddr;
Alexander Aringfc4e98d2013-02-05 10:23:43 +0000478 u8 head[100];
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700479 struct ieee802154_addr sa, da;
480
Alexander Aringfc4e98d2013-02-05 10:23:43 +0000481 /* TODO:
482 * if this package isn't ipv6 one, where should it be routed?
483 */
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700484 if (type != ETH_P_IPV6)
485 return 0;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700486
487 hdr = ipv6_hdr(skb);
488 hc06_ptr = head + 2;
489
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000490 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n"
491 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
492 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700493
494 lowpan_raw_dump_table(__func__, "raw skb network header dump",
495 skb_network_header(skb), sizeof(struct ipv6hdr));
496
497 if (!saddr)
498 saddr = dev->dev_addr;
499
500 lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
501
502 /*
503 * As we copy some bit-length fields, in the IPHC encoding bytes,
504 * we sometimes use |=
505 * If the field is 0, and the current bit value in memory is 1,
506 * this does not work. We therefore reset the IPHC encoding here
507 */
508 iphc0 = LOWPAN_DISPATCH_IPHC;
509 iphc1 = 0;
510
511 /* TODO: context lookup */
512
513 lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
514
515 /*
516 * Traffic class, flow label
517 * If flow label is 0, compress it. If traffic class is 0, compress it
518 * We have to process both in the same time as the offset of traffic
519 * class depends on the presence of version and flow label
520 */
521
522 /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
523 tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
524 tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
525
526 if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
527 (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
528 /* flow label can be compressed */
529 iphc0 |= LOWPAN_IPHC_FL_C;
530 if ((hdr->priority == 0) &&
531 ((hdr->flow_lbl[0] & 0xF0) == 0)) {
532 /* compress (elide) all */
533 iphc0 |= LOWPAN_IPHC_TC_C;
534 } else {
535 /* compress only the flow label */
536 *hc06_ptr = tmp;
537 hc06_ptr += 1;
538 }
539 } else {
540 /* Flow label cannot be compressed */
541 if ((hdr->priority == 0) &&
542 ((hdr->flow_lbl[0] & 0xF0) == 0)) {
543 /* compress only traffic class */
544 iphc0 |= LOWPAN_IPHC_TC_C;
545 *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
546 memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
547 hc06_ptr += 3;
548 } else {
549 /* compress nothing */
550 memcpy(hc06_ptr, &hdr, 4);
551 /* replace the top byte with new ECN | DSCP format */
552 *hc06_ptr = tmp;
553 hc06_ptr += 4;
554 }
555 }
556
557 /* NOTE: payload length is always compressed */
558
559 /* Next Header is compress if UDP */
560 if (hdr->nexthdr == UIP_PROTO_UDP)
561 iphc0 |= LOWPAN_IPHC_NH_C;
562
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700563 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
564 *hc06_ptr = hdr->nexthdr;
565 hc06_ptr += 1;
566 }
567
568 /*
569 * Hop limit
570 * if 1: compress, encoding is 01
571 * if 64: compress, encoding is 10
572 * if 255: compress, encoding is 11
573 * else do not compress
574 */
575 switch (hdr->hop_limit) {
576 case 1:
577 iphc0 |= LOWPAN_IPHC_TTL_1;
578 break;
579 case 64:
580 iphc0 |= LOWPAN_IPHC_TTL_64;
581 break;
582 case 255:
583 iphc0 |= LOWPAN_IPHC_TTL_255;
584 break;
585 default:
586 *hc06_ptr = hdr->hop_limit;
alex.bluesman.smirnov@gmail.com5c00c0c2012-06-25 03:49:02 +0000587 hc06_ptr += 1;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700588 break;
589 }
590
591 /* source address compression */
592 if (is_addr_unspecified(&hdr->saddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000593 pr_debug("source address is unspecified, setting SAC\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700594 iphc1 |= LOWPAN_IPHC_SAC;
595 /* TODO: context lookup */
596 } else if (is_addr_link_local(&hdr->saddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000597 pr_debug("source address is link-local\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700598 iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
599 LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
600 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000601 pr_debug("send the full source address\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700602 memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
603 hc06_ptr += 16;
604 }
605
606 /* destination address compression */
607 if (is_addr_mcast(&hdr->daddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000608 pr_debug("destination address is multicast: ");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700609 iphc1 |= LOWPAN_IPHC_M;
610 if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
611 pr_debug("compressed to 1 octet\n");
612 iphc1 |= LOWPAN_IPHC_DAM_11;
613 /* use last byte */
614 *hc06_ptr = hdr->daddr.s6_addr[15];
615 hc06_ptr += 1;
616 } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
617 pr_debug("compressed to 4 octets\n");
618 iphc1 |= LOWPAN_IPHC_DAM_10;
619 /* second byte + the last three */
620 *hc06_ptr = hdr->daddr.s6_addr[1];
621 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
622 hc06_ptr += 4;
623 } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
624 pr_debug("compressed to 6 octets\n");
625 iphc1 |= LOWPAN_IPHC_DAM_01;
626 /* second byte + the last five */
627 *hc06_ptr = hdr->daddr.s6_addr[1];
628 memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
629 hc06_ptr += 6;
630 } else {
631 pr_debug("using full address\n");
632 iphc1 |= LOWPAN_IPHC_DAM_00;
633 memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
634 hc06_ptr += 16;
635 }
636 } else {
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700637 /* TODO: context lookup */
638 if (is_addr_link_local(&hdr->daddr)) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000639 pr_debug("dest address is unicast and link-local\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700640 iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
641 LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
642 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000643 pr_debug("dest address is unicast: using full one\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700644 memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
645 hc06_ptr += 16;
646 }
647 }
648
alex.bluesman.smirnov@gmail.com3bd5b952011-11-10 07:40:14 +0000649 /* UDP header compression */
650 if (hdr->nexthdr == UIP_PROTO_UDP)
651 lowpan_compress_udp_header(&hc06_ptr, skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700652
653 head[0] = iphc0;
654 head[1] = iphc1;
655
656 skb_pull(skb, sizeof(struct ipv6hdr));
Alexander Aring578d5242013-10-30 09:18:23 +0100657 skb_reset_transport_header(skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700658 memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
Alexander Aring578d5242013-10-30 09:18:23 +0100659 skb_reset_network_header(skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700660
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700661 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
662 skb->len);
663
664 /*
665 * NOTE1: I'm still unsure about the fact that compression and WPAN
666 * header are created here and not later in the xmit. So wait for
667 * an opinion of net maintainers.
668 */
669 /*
670 * NOTE2: to be absolutely correct, we must derive PANid information
671 * from MAC subif of the 'dev' and 'real_dev' network devices, but
672 * this isn't implemented in mainline yet, so currently we assign 0xff
673 */
674 {
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000675 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
Tony Cheneauc7d0ab22013-03-25 17:59:30 +0000676 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000677
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700678 /* prepare wpan address data */
679 sa.addr_type = IEEE802154_ADDR_LONG;
Tony Cheneau43de7aa2013-03-25 17:59:31 +0000680 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700681
Tony Cheneau43de7aa2013-03-25 17:59:31 +0000682 memcpy(&(sa.hwaddr), saddr, 8);
683 /* intra-PAN communications */
684 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
685
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000686 /*
687 * if the destination address is the broadcast address, use the
688 * corresponding short address
689 */
690 if (lowpan_is_addr_broadcast(daddr)) {
691 da.addr_type = IEEE802154_ADDR_SHORT;
692 da.short_addr = IEEE802154_ADDR_BROADCAST;
693 } else {
694 da.addr_type = IEEE802154_ADDR_LONG;
Tony Cheneau6bdeaba2013-03-26 18:09:25 +0000695 memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000696
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000697 /* request acknowledgment */
Tony Cheneauf333a152013-03-25 17:59:23 +0000698 mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
Tony Cheneau58ef67c2013-03-25 17:59:25 +0000699 }
Tony Cheneauf333a152013-03-25 17:59:23 +0000700
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700701 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
702 type, (void *)&da, (void *)&sa, skb->len);
703 }
704}
705
Alan Ott0c446212013-01-16 19:09:47 +0000706static int lowpan_give_skb_to_devices(struct sk_buff *skb)
707{
708 struct lowpan_dev_record *entry;
709 struct sk_buff *skb_cp;
710 int stat = NET_RX_SUCCESS;
711
712 rcu_read_lock();
713 list_for_each_entry_rcu(entry, &lowpan_devices, list)
714 if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
715 skb_cp = skb_copy(skb, GFP_ATOMIC);
716 if (!skb_cp) {
717 stat = -ENOMEM;
718 break;
719 }
720
721 skb_cp->dev = entry->ldev;
722 stat = netif_rx(skb_cp);
723 }
724 rcu_read_unlock();
725
726 return stat;
727}
728
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700729static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
730{
731 struct sk_buff *new;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700732 int stat = NET_RX_SUCCESS;
733
734 new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
alex.bluesman.smirnov@gmail.comdcef1152011-09-01 03:55:15 +0000735 GFP_ATOMIC);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700736 kfree_skb(skb);
737
alex.bluesman.smirnov@gmail.comdcef1152011-09-01 03:55:15 +0000738 if (!new)
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700739 return -ENOMEM;
740
741 skb_push(new, sizeof(struct ipv6hdr));
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700742 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;
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000788
789 /* reserve headroom for uncompressed ipv6 header */
790 skb_reserve(frame->skb, sizeof(struct ipv6hdr));
791 skb_put(frame->skb, frame->length);
792
David Hauweele31afe1f2013-08-16 21:59:55 +0200793 /* copy the first control block to keep a
794 * trace of the link-layer addresses in case
795 * of a link-local compressed address
796 */
797 memcpy(frame->skb->cb, skb->cb, sizeof(skb->cb));
798
alex.bluesman.smirnov@gmail.comc2e94d72012-04-25 23:35:50 +0000799 init_timer(&frame->timer);
800 /* time out is the same as for ipv6 - 60 sec */
801 frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
802 frame->timer.data = (unsigned long)frame;
803 frame->timer.function = lowpan_fragment_timer_expired;
804
805 add_timer(&frame->timer);
806
807 list_add_tail(&frame->list, &lowpan_fragments);
808
809 return frame;
810
811skb_err:
812 kfree(frame);
813frame_err:
814 return NULL;
815}
816
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700817static int
818lowpan_process_data(struct sk_buff *skb)
819{
Alexander Aring84ce1ddf2013-08-16 21:59:54 +0200820 struct ipv6hdr hdr = {};
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700821 u8 tmp, iphc0, iphc1, num_context = 0;
Alexander Aringce2463b2013-08-16 21:59:58 +0200822 const struct ieee802154_addr *_saddr, *_daddr;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700823 int err;
824
825 lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
826 skb->len);
827 /* at least two bytes will be used for the encoding */
828 if (skb->len < 2)
829 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000830
831 if (lowpan_fetch_skb_u8(skb, &iphc0))
832 goto drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000833
834 /* fragments assembling */
835 switch (iphc0 & LOWPAN_DISPATCH_MASK) {
836 case LOWPAN_DISPATCH_FRAG1:
837 case LOWPAN_DISPATCH_FRAGN:
838 {
839 struct lowpan_fragment *frame;
Tony Cheneau5e968552012-07-11 06:51:16 +0000840 /* slen stores the rightmost 8 bits of the 11 bits length */
Tony Cheneaud991b982013-03-25 17:59:26 +0000841 u8 slen, offset = 0;
Tony Cheneau5e968552012-07-11 06:51:16 +0000842 u16 len, tag;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000843 bool found = false;
844
Tony Cheneau5e968552012-07-11 06:51:16 +0000845 if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000846 lowpan_fetch_skb_u16(skb, &tag)) /* fragment tag */
847 goto drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000848
Tony Cheneau5e968552012-07-11 06:51:16 +0000849 /* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
850 len = ((iphc0 & 7) << 8) | slen;
851
Tony Cheneau9da29242013-03-25 17:59:27 +0000852 if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
853 pr_debug("%s received a FRAG1 packet (tag: %d, "
854 "size of the entire IP packet: %d)",
855 __func__, tag, len);
856 } else { /* FRAGN */
Tony Cheneaud991b982013-03-25 17:59:26 +0000857 if (lowpan_fetch_skb_u8(skb, &offset))
858 goto unlock_and_drop;
Tony Cheneau9da29242013-03-25 17:59:27 +0000859 pr_debug("%s received a FRAGN packet (tag: %d, "
860 "size of the entire IP packet: %d, "
861 "offset: %d)", __func__, tag, len, offset * 8);
Tony Cheneaud991b982013-03-25 17:59:26 +0000862 }
863
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000864 /*
865 * check if frame assembling with the same tag is
866 * already in progress
867 */
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000868 spin_lock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000869
870 list_for_each_entry(frame, &lowpan_fragments, list)
871 if (frame->tag == tag) {
872 found = true;
873 break;
874 }
875
876 /* alloc new frame structure */
877 if (!found) {
Tony Cheneau9da29242013-03-25 17:59:27 +0000878 pr_debug("%s first fragment received for tag %d, "
879 "begin packet reassembly", __func__, tag);
Tony Cheneau5e968552012-07-11 06:51:16 +0000880 frame = lowpan_alloc_new_frame(skb, len, tag);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000881 if (!frame)
882 goto unlock_and_drop;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000883 }
884
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000885 /* if payload fits buffer, copy it */
886 if (likely((offset * 8 + skb->len) <= frame->length))
887 skb_copy_to_linear_data_offset(frame->skb, offset * 8,
888 skb->data, skb->len);
889 else
890 goto unlock_and_drop;
891
892 frame->bytes_rcv += skb->len;
893
894 /* frame assembling complete */
895 if ((frame->bytes_rcv == frame->length) &&
896 frame->timer.expires > jiffies) {
897 /* if timer haven't expired - first of all delete it */
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000898 del_timer_sync(&frame->timer);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000899 list_del(&frame->list);
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000900 spin_unlock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000901
Tony Cheneau9da29242013-03-25 17:59:27 +0000902 pr_debug("%s successfully reassembled fragment "
903 "(tag %d)", __func__, tag);
904
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000905 dev_kfree_skb(skb);
906 skb = frame->skb;
907 kfree(frame);
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000908
909 if (lowpan_fetch_skb_u8(skb, &iphc0))
Dan Carpenter747cf6e2012-06-26 20:53:09 +0000910 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000911
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000912 break;
913 }
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +0000914 spin_unlock_bh(&flist_lock);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +0000915
916 return kfree_skb(skb), 0;
917 }
918 default:
919 break;
920 }
921
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000922 if (lowpan_fetch_skb_u8(skb, &iphc1))
923 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700924
Alexander Aringce2463b2013-08-16 21:59:58 +0200925 _saddr = &mac_cb(skb)->sa;
926 _daddr = &mac_cb(skb)->da;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700927
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000928 pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700929
930 /* another if the CID flag is set */
931 if (iphc1 & LOWPAN_IPHC_CID) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000932 pr_debug("CID flag is set, increase header with one\n");
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000933 if (lowpan_fetch_skb_u8(skb, &num_context))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700934 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700935 }
936
937 hdr.version = 6;
938
939 /* Traffic Class and Flow Label */
940 switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
941 /*
942 * Traffic Class and FLow Label carried in-line
943 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
944 */
945 case 0: /* 00b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000946 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700947 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000948
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700949 memcpy(&hdr.flow_lbl, &skb->data[0], 3);
950 skb_pull(skb, 3);
951 hdr.priority = ((tmp >> 2) & 0x0f);
952 hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
953 (hdr.flow_lbl[0] & 0x0f);
954 break;
955 /*
956 * Traffic class carried in-line
957 * ECN + DSCP (1 byte), Flow Label is elided
958 */
Jukka Rissanen1188f052013-11-13 11:03:39 +0200959 case 2: /* 10b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000960 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700961 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000962
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700963 hdr.priority = ((tmp >> 2) & 0x0f);
964 hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700965 break;
966 /*
967 * Flow Label carried in-line
968 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
969 */
Jukka Rissanen1188f052013-11-13 11:03:39 +0200970 case 1: /* 01b */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000971 if (lowpan_fetch_skb_u8(skb, &tmp))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700972 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000973
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700974 hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
975 memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
976 skb_pull(skb, 2);
977 break;
978 /* Traffic Class and Flow Label are elided */
979 case 3: /* 11b */
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700980 break;
981 default:
982 break;
983 }
984
985 /* Next Header */
986 if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
987 /* Next header is carried inline */
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000988 if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700989 goto drop;
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000990
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +0000991 pr_debug("NH flag is set, next header carried inline: %02x\n",
992 hdr.nexthdr);
Alexander Smirnov44331fe2011-08-24 19:34:42 -0700993 }
994
995 /* Hop Limit */
996 if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
997 hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
998 else {
alex.bluesman.smirnov@gmail.comc5d36872012-06-25 03:49:01 +0000999 if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001000 goto drop;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001001 }
1002
1003 /* Extract SAM to the tmp variable */
1004 tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
1005
Alexander Aring65d892c2013-08-16 21:59:59 +02001006 if (iphc1 & LOWPAN_IPHC_SAC) {
1007 /* Source address context based uncompression */
1008 pr_debug("SAC bit is set. Handle context based source address.\n");
1009 err = lowpan_uncompress_context_based_src_addr(
1010 skb, &hdr.saddr, tmp);
1011 } else {
1012 /* Source address uncompression */
1013 pr_debug("source address stateless compression\n");
1014 err = lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr);
1015 }
1016
1017 /* Check on error of previous branch */
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001018 if (err)
1019 goto drop;
1020
1021 /* Extract DAM to the tmp variable */
1022 tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
1023
1024 /* check for Multicast Compression */
1025 if (iphc1 & LOWPAN_IPHC_M) {
1026 if (iphc1 & LOWPAN_IPHC_DAC) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001027 pr_debug("dest: context-based mcast compression\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001028 /* TODO: implement this */
1029 } else {
Alexander Aring84c2e7b2013-08-16 21:59:57 +02001030 err = lowpan_uncompress_multicast_daddr(
1031 skb, &hdr.daddr, tmp);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001032 if (err)
1033 goto drop;
1034 }
1035 } else {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001036 pr_debug("dest: stateless compression\n");
Alexander Aringce2463b2013-08-16 21:59:58 +02001037 err = lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001038 if (err)
1039 goto drop;
1040 }
1041
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +00001042 /* UDP data uncompression */
Tony Cheneauf5c20f52013-03-25 17:59:22 +00001043 if (iphc0 & LOWPAN_IPHC_NH_C) {
Tony Cheneau24363b62013-03-25 17:59:32 +00001044 struct udphdr uh;
1045 struct sk_buff *new;
1046 if (lowpan_uncompress_udp_header(skb, &uh))
alex.bluesman.smirnov@gmail.comf8b1b5d2011-11-10 07:40:53 +00001047 goto drop;
Tony Cheneau24363b62013-03-25 17:59:32 +00001048
1049 /*
1050 * replace the compressed UDP head by the uncompressed UDP
1051 * header
1052 */
1053 new = skb_copy_expand(skb, sizeof(struct udphdr),
1054 skb_tailroom(skb), GFP_ATOMIC);
1055 kfree_skb(skb);
1056
1057 if (!new)
1058 return -ENOMEM;
1059
1060 skb = new;
1061
1062 skb_push(skb, sizeof(struct udphdr));
Tony Cheneau24363b62013-03-25 17:59:32 +00001063 skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
1064
1065 lowpan_raw_dump_table(__func__, "raw UDP header dump",
1066 (u8 *)&uh, sizeof(uh));
1067
Tony Cheneauf5c20f52013-03-25 17:59:22 +00001068 hdr.nexthdr = UIP_PROTO_UDP;
1069 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001070
1071 /* Not fragmented package */
1072 hdr.payload_len = htons(skb->len);
1073
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001074 pr_debug("skb headroom size = %d, data length = %d\n",
1075 skb_headroom(skb), skb->len);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001076
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001077 pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t"
1078 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001079 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
1080
1081 lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
1082 sizeof(hdr));
1083 return lowpan_skb_deliver(skb, &hdr);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001084
1085unlock_and_drop:
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001086 spin_unlock_bh(&flist_lock);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001087drop:
Dan Carpenter90d09632011-08-30 03:45:52 +00001088 kfree_skb(skb);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001089 return -EINVAL;
1090}
1091
alex.bluesman.smirnov@gmail.com42c36292012-07-01 19:58:46 +00001092static int lowpan_set_address(struct net_device *dev, void *p)
1093{
1094 struct sockaddr *sa = p;
1095
1096 if (netif_running(dev))
1097 return -EBUSY;
1098
1099 /* TODO: validate addr */
1100 memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);
1101
1102 return 0;
1103}
1104
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001105static int
1106lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
Tony Cheneaud991b982013-03-25 17:59:26 +00001107 int mlen, int plen, int offset, int type)
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001108{
1109 struct sk_buff *frag;
Alexander Aring545f3612013-10-28 10:24:16 +01001110 int hlen;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001111
Tony Cheneaud991b982013-03-25 17:59:26 +00001112 hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
1113 LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001114
1115 lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
1116
Alexander Aringb6144422013-10-28 10:24:18 +01001117 frag = netdev_alloc_skb(skb->dev,
1118 hlen + mlen + plen + IEEE802154_MFR_SIZE);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001119 if (!frag)
1120 return -ENOMEM;
1121
1122 frag->priority = skb->priority;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001123
1124 /* copy header, MFR and payload */
Alexander Aring3582b902013-10-30 09:18:24 +01001125 skb_put(frag, mlen);
1126 skb_copy_to_linear_data(frag, skb_mac_header(skb), mlen);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001127
Alexander Aring3582b902013-10-30 09:18:24 +01001128 skb_put(frag, hlen);
1129 skb_copy_to_linear_data_offset(frag, mlen, head, hlen);
1130
1131 skb_put(frag, plen);
1132 skb_copy_to_linear_data_offset(frag, mlen + hlen,
1133 skb_network_header(skb) + offset, plen);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001134
1135 lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
1136 frag->len);
1137
Alexander Aring545f3612013-10-28 10:24:16 +01001138 return dev_queue_xmit(frag);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001139}
1140
1141static int
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001142lowpan_skb_fragmentation(struct sk_buff *skb, struct net_device *dev)
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001143{
1144 int err, header_length, payload_length, tag, offset = 0;
1145 u8 head[5];
1146
Alexander Aring3e691622013-10-30 09:18:22 +01001147 header_length = skb->mac_len;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001148 payload_length = skb->len - header_length;
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001149 tag = lowpan_dev_info(dev)->fragment_tag++;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001150
1151 /* first fragment header */
Tony Cheneau5e968552012-07-11 06:51:16 +00001152 head[0] = LOWPAN_DISPATCH_FRAG1 | ((payload_length >> 8) & 0x7);
1153 head[1] = payload_length & 0xff;
Tony Cheneau45760392012-07-11 06:51:15 +00001154 head[2] = tag >> 8;
1155 head[3] = tag & 0xff;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001156
Tony Cheneaud991b982013-03-25 17:59:26 +00001157 err = lowpan_fragment_xmit(skb, head, header_length, LOWPAN_FRAG_SIZE,
1158 0, LOWPAN_DISPATCH_FRAG1);
1159
Tony Cheneau9da29242013-03-25 17:59:27 +00001160 if (err) {
1161 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
1162 __func__, tag);
Tony Cheneaud991b982013-03-25 17:59:26 +00001163 goto exit;
Tony Cheneau9da29242013-03-25 17:59:27 +00001164 }
Tony Cheneaud991b982013-03-25 17:59:26 +00001165
1166 offset = LOWPAN_FRAG_SIZE;
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001167
1168 /* next fragment header */
1169 head[0] &= ~LOWPAN_DISPATCH_FRAG1;
1170 head[0] |= LOWPAN_DISPATCH_FRAGN;
1171
Alexander Aring53cb57172013-10-28 10:24:17 +01001172 while (payload_length - offset > 0) {
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001173 int len = LOWPAN_FRAG_SIZE;
1174
1175 head[4] = offset / 8;
1176
1177 if (payload_length - offset < len)
1178 len = payload_length - offset;
1179
1180 err = lowpan_fragment_xmit(skb, head, header_length,
Tony Cheneaud991b982013-03-25 17:59:26 +00001181 len, offset, LOWPAN_DISPATCH_FRAGN);
Tony Cheneau9da29242013-03-25 17:59:27 +00001182 if (err) {
1183 pr_debug("%s unable to send a subsequent FRAGN packet "
1184 "(tag: %d, offset: %d", __func__, tag, offset);
Tony Cheneaud991b982013-03-25 17:59:26 +00001185 goto exit;
Tony Cheneau9da29242013-03-25 17:59:27 +00001186 }
Tony Cheneaud991b982013-03-25 17:59:26 +00001187
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001188 offset += len;
1189 }
1190
Tony Cheneaud991b982013-03-25 17:59:26 +00001191exit:
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001192 return err;
1193}
1194
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001195static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
1196{
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001197 int err = -1;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001198
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001199 pr_debug("package xmit\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001200
1201 skb->dev = lowpan_dev_info(dev)->real_dev;
1202 if (skb->dev == NULL) {
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001203 pr_debug("ERROR: no real wpan device found\n");
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001204 goto error;
1205 }
1206
Alan Ottb333b7e2012-11-29 15:55:44 +00001207 /* Send directly if less than the MTU minus the 2 checksum bytes. */
1208 if (skb->len <= IEEE802154_MTU - IEEE802154_MFR_SIZE) {
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001209 err = dev_queue_xmit(skb);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001210 goto out;
1211 }
1212
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001213 pr_debug("frame is too big, fragmentation is needed\n");
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001214 err = lowpan_skb_fragmentation(skb, dev);
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001215error:
1216 dev_kfree_skb(skb);
1217out:
Alan Ottfc52eea2013-04-03 04:00:58 +00001218 if (err)
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001219 pr_debug("ERROR: xmit failed\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001220
Alan Ottfc52eea2013-04-03 04:00:58 +00001221 return (err < 0) ? NET_XMIT_DROP : err;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001222}
1223
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001224static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
1225{
1226 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1227 return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
1228}
1229
1230static u16 lowpan_get_pan_id(const struct net_device *dev)
1231{
1232 struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1233 return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
1234}
1235
1236static u16 lowpan_get_short_addr(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_short_addr(real_dev);
1240}
1241
Tony Cheneauc7d0ab22013-03-25 17:59:30 +00001242static u8 lowpan_get_dsn(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_dsn(real_dev);
1246}
1247
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001248static struct header_ops lowpan_header_ops = {
1249 .create = lowpan_header_create,
1250};
1251
1252static const struct net_device_ops lowpan_netdev_ops = {
1253 .ndo_start_xmit = lowpan_xmit,
alex.bluesman.smirnov@gmail.com42c36292012-07-01 19:58:46 +00001254 .ndo_set_mac_address = lowpan_set_address,
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001255};
1256
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001257static struct ieee802154_mlme_ops lowpan_mlme = {
1258 .get_pan_id = lowpan_get_pan_id,
1259 .get_phy = lowpan_get_phy,
1260 .get_short_addr = lowpan_get_short_addr,
Tony Cheneauc7d0ab22013-03-25 17:59:30 +00001261 .get_dsn = lowpan_get_dsn,
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001262};
1263
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001264static void lowpan_setup(struct net_device *dev)
1265{
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001266 dev->addr_len = IEEE802154_ADDR_LEN;
1267 memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
1268 dev->type = ARPHRD_IEEE802154;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001269 /* Frame Control + Sequence Number + Address fields + Security Header */
1270 dev->hard_header_len = 2 + 1 + 20 + 14;
1271 dev->needed_tailroom = 2; /* FCS */
1272 dev->mtu = 1281;
1273 dev->tx_queue_len = 0;
alex.bluesman.smirnov@gmail.com4d039f62011-11-10 07:39:37 +00001274 dev->flags = IFF_BROADCAST | IFF_MULTICAST;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001275 dev->watchdog_timeo = 0;
1276
1277 dev->netdev_ops = &lowpan_netdev_ops;
1278 dev->header_ops = &lowpan_header_ops;
alex.bluesman.smirnov@gmail.com0848e402012-04-25 23:24:56 +00001279 dev->ml_priv = &lowpan_mlme;
Alan Otta2dc375e2012-09-01 05:57:07 +00001280 dev->destructor = free_netdev;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001281}
1282
1283static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
1284{
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001285 if (tb[IFLA_ADDRESS]) {
1286 if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
1287 return -EINVAL;
1288 }
1289 return 0;
1290}
1291
1292static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1293 struct packet_type *pt, struct net_device *orig_dev)
1294{
Alan Otta437d272012-09-01 05:57:06 +00001295 struct sk_buff *local_skb;
1296
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001297 if (!netif_running(dev))
1298 goto drop;
1299
1300 if (dev->type != ARPHRD_IEEE802154)
1301 goto drop;
1302
1303 /* check that it's our buffer */
Alan Ottee21c7e2013-01-16 19:09:48 +00001304 if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
1305 /* Copy the packet so that the IPv6 header is
1306 * properly aligned.
1307 */
1308 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
1309 skb_tailroom(skb), GFP_ATOMIC);
Alan Otta437d272012-09-01 05:57:06 +00001310 if (!local_skb)
1311 goto drop;
Alan Otta437d272012-09-01 05:57:06 +00001312
Alan Ottee21c7e2013-01-16 19:09:48 +00001313 local_skb->protocol = htons(ETH_P_IPV6);
1314 local_skb->pkt_type = PACKET_HOST;
1315
1316 /* Pull off the 1-byte of 6lowpan header. */
1317 skb_pull(local_skb, 1);
Alan Ottee21c7e2013-01-16 19:09:48 +00001318
1319 lowpan_give_skb_to_devices(local_skb);
1320
1321 kfree_skb(local_skb);
Alan Otta437d272012-09-01 05:57:06 +00001322 kfree_skb(skb);
Alan Ottee21c7e2013-01-16 19:09:48 +00001323 } else {
1324 switch (skb->data[0] & 0xe0) {
1325 case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
1326 case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
1327 case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
1328 local_skb = skb_clone(skb, GFP_ATOMIC);
1329 if (!local_skb)
1330 goto drop;
1331 lowpan_process_data(local_skb);
1332
1333 kfree_skb(skb);
1334 break;
1335 default:
1336 break;
1337 }
alex.bluesman.smirnov@gmail.com719269a2011-11-10 07:38:38 +00001338 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001339
1340 return NET_RX_SUCCESS;
1341
1342drop:
1343 kfree_skb(skb);
1344 return NET_RX_DROP;
1345}
1346
1347static int lowpan_newlink(struct net *src_net, struct net_device *dev,
1348 struct nlattr *tb[], struct nlattr *data[])
1349{
1350 struct net_device *real_dev;
1351 struct lowpan_dev_record *entry;
1352
alex.bluesman.smirnov@gmail.come71094f2012-06-25 03:49:03 +00001353 pr_debug("adding new link\n");
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001354
1355 if (!tb[IFLA_LINK])
1356 return -EINVAL;
1357 /* find and hold real wpan device */
1358 real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
1359 if (!real_dev)
1360 return -ENODEV;
Dan Carpenter78032f92013-11-07 10:44:45 +03001361 if (real_dev->type != ARPHRD_IEEE802154) {
1362 dev_put(real_dev);
Alan Ott7adac1e2013-10-05 23:15:18 -04001363 return -EINVAL;
Dan Carpenter78032f92013-11-07 10:44:45 +03001364 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001365
1366 lowpan_dev_info(dev)->real_dev = real_dev;
Tony Cheneaud4ac3232013-03-25 17:59:28 +00001367 lowpan_dev_info(dev)->fragment_tag = 0;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001368 mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
1369
1370 entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
Dan Carpenterdc00fd42011-08-30 03:51:09 +00001371 if (!entry) {
1372 dev_put(real_dev);
1373 lowpan_dev_info(dev)->real_dev = NULL;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001374 return -ENOMEM;
Dan Carpenterdc00fd42011-08-30 03:51:09 +00001375 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001376
1377 entry->ldev = dev;
1378
Alan Ottab2d95d2013-10-05 23:15:19 -04001379 /* Set the lowpan harware address to the wpan hardware address. */
1380 memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
1381
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001382 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1383 INIT_LIST_HEAD(&entry->list);
1384 list_add_tail(&entry->list, &lowpan_devices);
1385 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1386
1387 register_netdevice(dev);
1388
1389 return 0;
1390}
1391
1392static void lowpan_dellink(struct net_device *dev, struct list_head *head)
1393{
1394 struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
1395 struct net_device *real_dev = lowpan_dev->real_dev;
alex.bluesman.smirnov@gmail.com8deff4a2012-04-25 23:24:57 +00001396 struct lowpan_dev_record *entry, *tmp;
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001397
1398 ASSERT_RTNL();
1399
1400 mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
Dan Carpenteraec9db32011-08-30 03:46:40 +00001401 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001402 if (entry->ldev == dev) {
1403 list_del(&entry->list);
1404 kfree(entry);
1405 }
Dan Carpenteraec9db32011-08-30 03:46:40 +00001406 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001407 mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1408
1409 mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
1410
1411 unregister_netdevice_queue(dev, head);
1412
1413 dev_put(real_dev);
1414}
1415
1416static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
1417 .kind = "lowpan",
1418 .priv_size = sizeof(struct lowpan_dev_info),
1419 .setup = lowpan_setup,
1420 .newlink = lowpan_newlink,
1421 .dellink = lowpan_dellink,
1422 .validate = lowpan_validate,
1423};
1424
1425static inline int __init lowpan_netlink_init(void)
1426{
1427 return rtnl_link_register(&lowpan_link_ops);
1428}
1429
David S. Millera07fdce2013-02-06 15:54:38 -05001430static inline void lowpan_netlink_fini(void)
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001431{
1432 rtnl_link_unregister(&lowpan_link_ops);
1433}
1434
Alan Otta2dc375e2012-09-01 05:57:07 +00001435static int lowpan_device_event(struct notifier_block *unused,
Jiri Pirko351638e2013-05-28 01:30:21 +00001436 unsigned long event, void *ptr)
Alan Otta2dc375e2012-09-01 05:57:07 +00001437{
Jiri Pirko351638e2013-05-28 01:30:21 +00001438 struct net_device *dev = netdev_notifier_info_to_dev(ptr);
Alan Otta2dc375e2012-09-01 05:57:07 +00001439 LIST_HEAD(del_list);
1440 struct lowpan_dev_record *entry, *tmp;
1441
1442 if (dev->type != ARPHRD_IEEE802154)
1443 goto out;
1444
1445 if (event == NETDEV_UNREGISTER) {
1446 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1447 if (lowpan_dev_info(entry->ldev)->real_dev == dev)
1448 lowpan_dellink(entry->ldev, &del_list);
1449 }
1450
1451 unregister_netdevice_many(&del_list);
Peter Senna Tschudin4c835012012-09-18 07:10:43 +00001452 }
Alan Otta2dc375e2012-09-01 05:57:07 +00001453
1454out:
1455 return NOTIFY_DONE;
1456}
1457
1458static struct notifier_block lowpan_dev_notifier = {
1459 .notifier_call = lowpan_device_event,
1460};
1461
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001462static struct packet_type lowpan_packet_type = {
1463 .type = __constant_htons(ETH_P_IEEE802154),
1464 .func = lowpan_rcv,
1465};
1466
1467static int __init lowpan_init_module(void)
1468{
1469 int err = 0;
1470
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001471 err = lowpan_netlink_init();
1472 if (err < 0)
1473 goto out;
1474
1475 dev_add_pack(&lowpan_packet_type);
Alan Otta2dc375e2012-09-01 05:57:07 +00001476
1477 err = register_netdevice_notifier(&lowpan_dev_notifier);
1478 if (err < 0) {
1479 dev_remove_pack(&lowpan_packet_type);
1480 lowpan_netlink_fini();
1481 }
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001482out:
1483 return err;
1484}
1485
1486static void __exit lowpan_cleanup_module(void)
1487{
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001488 struct lowpan_fragment *frame, *tframe;
1489
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001490 lowpan_netlink_fini();
1491
1492 dev_remove_pack(&lowpan_packet_type);
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001493
Alan Otta2dc375e2012-09-01 05:57:07 +00001494 unregister_netdevice_notifier(&lowpan_dev_notifier);
1495
alex.bluesman.smirnov@gmail.com33c34c52012-07-10 21:22:48 +00001496 /* Now 6lowpan packet_type is removed, so no new fragments are
1497 * expected on RX, therefore that's the time to clean incomplete
1498 * fragments.
1499 */
1500 spin_lock_bh(&flist_lock);
1501 list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
1502 del_timer_sync(&frame->timer);
1503 list_del(&frame->list);
1504 dev_kfree_skb(frame->skb);
1505 kfree(frame);
1506 }
1507 spin_unlock_bh(&flist_lock);
Alexander Smirnov44331fe2011-08-24 19:34:42 -07001508}
1509
1510module_init(lowpan_init_module);
1511module_exit(lowpan_cleanup_module);
1512MODULE_LICENSE("GPL");
1513MODULE_ALIAS_RTNL_LINK("lowpan");