net: compat: Update get_compat_msghdr() to match copy_msghdr_from_user() behaviour
[firefly-linux-kernel-4.4.55.git] / net / ieee802154 / 6lowpan.c
1 /*
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
53 #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 */
66 static const u8 lowpan_ttl_values[] = {0, 1, 64, 255};
67
68 static LIST_HEAD(lowpan_devices);
69
70 /*
71  * Uncompression of linklocal:
72  *   0 -> 16 bytes from packet
73  *   1 -> 2  bytes from prefix - bunch of zeroes and 8 from packet
74  *   2 -> 2  bytes from prefix - zeroes + 2 from packet
75  *   3 -> 2  bytes from prefix - infer 8 bytes from lladdr
76  *
77  *  NOTE: => the uncompress function does change 0xf to 0x10
78  *  NOTE: 0x00 => no-autoconfig => unspecified
79  */
80 static const u8 lowpan_unc_llconf[] = {0x0f, 0x28, 0x22, 0x20};
81
82 /*
83  * Uncompression of ctx-based:
84  *   0 -> 0 bits  from packet [unspecified / reserved]
85  *   1 -> 8 bytes from prefix - bunch of zeroes and 8 from packet
86  *   2 -> 8 bytes from prefix - zeroes + 2 from packet
87  *   3 -> 8 bytes from prefix - infer 8 bytes from lladdr
88  */
89 static const u8 lowpan_unc_ctxconf[] = {0x00, 0x88, 0x82, 0x80};
90
91 /*
92  * Uncompression of ctx-base
93  *   0 -> 0 bits from packet
94  *   1 -> 2 bytes from prefix - bunch of zeroes 5 from packet
95  *   2 -> 2 bytes from prefix - zeroes + 3 from packet
96  *   3 -> 2 bytes from prefix - infer 1 bytes from lladdr
97  */
98 static const u8 lowpan_unc_mxconf[] = {0x0f, 0x25, 0x23, 0x21};
99
100 /* Link local prefix */
101 static const u8 lowpan_llprefix[] = {0xfe, 0x80};
102
103 /* private device info */
104 struct lowpan_dev_info {
105         struct net_device       *real_dev; /* real WPAN device ptr */
106         struct mutex            dev_list_mtx; /* mutex for list ops */
107         unsigned short          fragment_tag;
108 };
109
110 struct lowpan_dev_record {
111         struct net_device *ldev;
112         struct list_head list;
113 };
114
115 struct lowpan_fragment {
116         struct sk_buff          *skb;           /* skb to be assembled */
117         u16                     length;         /* length to be assemled */
118         u32                     bytes_rcv;      /* bytes received */
119         u16                     tag;            /* current fragment tag */
120         struct timer_list       timer;          /* assembling timer */
121         struct list_head        list;           /* fragments list */
122 };
123
124 static LIST_HEAD(lowpan_fragments);
125 static DEFINE_SPINLOCK(flist_lock);
126
127 static inline struct
128 lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
129 {
130         return netdev_priv(dev);
131 }
132
133 static inline void lowpan_address_flip(u8 *src, u8 *dest)
134 {
135         int i;
136         for (i = 0; i < IEEE802154_ADDR_LEN; i++)
137                 (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
138 }
139
140 /* list of all 6lowpan devices, uses for package delivering */
141 /* print data in line */
142 static inline void lowpan_raw_dump_inline(const char *caller, char *msg,
143                                    unsigned char *buf, int len)
144 {
145 #ifdef DEBUG
146         if (msg)
147                 pr_debug("(%s) %s: ", caller, msg);
148         print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,
149                        16, 1, buf, len, false);
150 #endif /* DEBUG */
151 }
152
153 /*
154  * print data in a table format:
155  *
156  * addr: xx xx xx xx xx xx
157  * addr: xx xx xx xx xx xx
158  * ...
159  */
160 static inline void lowpan_raw_dump_table(const char *caller, char *msg,
161                                    unsigned char *buf, int len)
162 {
163 #ifdef DEBUG
164         if (msg)
165                 pr_debug("(%s) %s:\n", caller, msg);
166         print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET,
167                        16, 1, buf, len, false);
168 #endif /* DEBUG */
169 }
170
171 static u8
172 lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr,
173                  const unsigned char *lladdr)
174 {
175         u8 val = 0;
176
177         if (is_addr_mac_addr_based(ipaddr, lladdr))
178                 val = 3; /* 0-bits */
179         else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
180                 /* compress IID to 16 bits xxxx::XXXX */
181                 memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
182                 *hc06_ptr += 2;
183                 val = 2; /* 16-bits */
184         } else {
185                 /* do not compress IID => xxxx::IID */
186                 memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
187                 *hc06_ptr += 8;
188                 val = 1; /* 64-bits */
189         }
190
191         return rol8(val, shift);
192 }
193
194 static void
195 lowpan_uip_ds6_set_addr_iid(struct in6_addr *ipaddr, unsigned char *lladdr)
196 {
197         memcpy(&ipaddr->s6_addr[8], lladdr, IEEE802154_ADDR_LEN);
198         /* second bit-flip (Universe/Local) is done according RFC2464 */
199         ipaddr->s6_addr[8] ^= 0x02;
200 }
201
202 /*
203  * Uncompress addresses based on a prefix and a postfix with zeroes in
204  * between. If the postfix is zero in length it will use the link address
205  * to configure the IP address (autoconf style).
206  * pref_post_count takes a byte where the first nibble specify prefix count
207  * and the second postfix count (NOTE: 15/0xf => 16 bytes copy).
208  */
209 static int
210 lowpan_uncompress_addr(struct sk_buff *skb, struct in6_addr *ipaddr,
211         u8 const *prefix, u8 pref_post_count, unsigned char *lladdr)
212 {
213         u8 prefcount = pref_post_count >> 4;
214         u8 postcount = pref_post_count & 0x0f;
215
216         /* full nibble 15 => 16 */
217         prefcount = (prefcount == 15 ? 16 : prefcount);
218         postcount = (postcount == 15 ? 16 : postcount);
219
220         if (lladdr)
221                 lowpan_raw_dump_inline(__func__, "linklocal address",
222                                                 lladdr, IEEE802154_ADDR_LEN);
223         if (prefcount > 0)
224                 memcpy(ipaddr, prefix, prefcount);
225
226         if (prefcount + postcount < 16)
227                 memset(&ipaddr->s6_addr[prefcount], 0,
228                                         16 - (prefcount + postcount));
229
230         if (postcount > 0) {
231                 memcpy(&ipaddr->s6_addr[16 - postcount], skb->data, postcount);
232                 skb_pull(skb, postcount);
233         } else if (prefcount > 0) {
234                 if (lladdr == NULL)
235                         return -EINVAL;
236
237                 /* no IID based configuration if no prefix and no data */
238                 lowpan_uip_ds6_set_addr_iid(ipaddr, lladdr);
239         }
240
241         pr_debug("uncompressing %d + %d => ", prefcount, postcount);
242         lowpan_raw_dump_inline(NULL, NULL, ipaddr->s6_addr, 16);
243
244         return 0;
245 }
246
247 static void
248 lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
249 {
250         struct udphdr *uh = udp_hdr(skb);
251
252         if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) ==
253                                 LOWPAN_NHC_UDP_4BIT_PORT) &&
254             ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) ==
255                                 LOWPAN_NHC_UDP_4BIT_PORT)) {
256                 pr_debug("UDP header: both ports compression to 4 bits\n");
257                 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11;
258                 **(hc06_ptr + 1) = /* subtraction is faster */
259                    (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) +
260                        ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4));
261                 *hc06_ptr += 2;
262         } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) ==
263                         LOWPAN_NHC_UDP_8BIT_PORT) {
264                 pr_debug("UDP header: remove 8 bits of dest\n");
265                 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01;
266                 memcpy(*hc06_ptr + 1, &uh->source, 2);
267                 **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT);
268                 *hc06_ptr += 4;
269         } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) ==
270                         LOWPAN_NHC_UDP_8BIT_PORT) {
271                 pr_debug("UDP header: remove 8 bits of source\n");
272                 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10;
273                 memcpy(*hc06_ptr + 1, &uh->dest, 2);
274                 **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT);
275                 *hc06_ptr += 4;
276         } else {
277                 pr_debug("UDP header: can't compress\n");
278                 **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00;
279                 memcpy(*hc06_ptr + 1, &uh->source, 2);
280                 memcpy(*hc06_ptr + 3, &uh->dest, 2);
281                 *hc06_ptr += 5;
282         }
283
284         /* checksum is always inline */
285         memcpy(*hc06_ptr, &uh->check, 2);
286         *hc06_ptr += 2;
287
288         /* skip the UDP header */
289         skb_pull(skb, sizeof(struct udphdr));
290 }
291
292 static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
293 {
294         if (unlikely(!pskb_may_pull(skb, 1)))
295                 return -EINVAL;
296
297         *val = skb->data[0];
298         skb_pull(skb, 1);
299
300         return 0;
301 }
302
303 static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
304 {
305         if (unlikely(!pskb_may_pull(skb, 2)))
306                 return -EINVAL;
307
308         *val = (skb->data[0] << 8) | skb->data[1];
309         skb_pull(skb, 2);
310
311         return 0;
312 }
313
314 static int
315 lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
316 {
317         u8 tmp;
318
319         if (!uh)
320                 goto err;
321
322         if (lowpan_fetch_skb_u8(skb, &tmp))
323                 goto err;
324
325         if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
326                 pr_debug("UDP header uncompression\n");
327                 switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
328                 case LOWPAN_NHC_UDP_CS_P_00:
329                         memcpy(&uh->source, &skb->data[0], 2);
330                         memcpy(&uh->dest, &skb->data[2], 2);
331                         skb_pull(skb, 4);
332                         break;
333                 case LOWPAN_NHC_UDP_CS_P_01:
334                         memcpy(&uh->source, &skb->data[0], 2);
335                         uh->dest =
336                            skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT;
337                         skb_pull(skb, 3);
338                         break;
339                 case LOWPAN_NHC_UDP_CS_P_10:
340                         uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT;
341                         memcpy(&uh->dest, &skb->data[1], 2);
342                         skb_pull(skb, 3);
343                         break;
344                 case LOWPAN_NHC_UDP_CS_P_11:
345                         uh->source =
346                            LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4);
347                         uh->dest =
348                            LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f);
349                         skb_pull(skb, 1);
350                         break;
351                 default:
352                         pr_debug("ERROR: unknown UDP format\n");
353                         goto err;
354                         break;
355                 }
356
357                 pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
358                          uh->source, uh->dest);
359
360                 /* copy checksum */
361                 memcpy(&uh->check, &skb->data[0], 2);
362                 skb_pull(skb, 2);
363
364                 /*
365                  * UDP lenght needs to be infered from the lower layers
366                  * here, we obtain the hint from the remaining size of the
367                  * frame
368                  */
369                 uh->len = htons(skb->len + sizeof(struct udphdr));
370                 pr_debug("uncompressed UDP length: src = %d", uh->len);
371         } else {
372                 pr_debug("ERROR: unsupported NH format\n");
373                 goto err;
374         }
375
376         return 0;
377 err:
378         return -EINVAL;
379 }
380
381 static int lowpan_header_create(struct sk_buff *skb,
382                            struct net_device *dev,
383                            unsigned short type, const void *_daddr,
384                            const void *_saddr, unsigned int len)
385 {
386         u8 tmp, iphc0, iphc1, *hc06_ptr;
387         struct ipv6hdr *hdr;
388         const u8 *saddr = _saddr;
389         const u8 *daddr = _daddr;
390         u8 head[100];
391         struct ieee802154_addr sa, da;
392
393         /* TODO:
394          * if this package isn't ipv6 one, where should it be routed?
395          */
396         if (type != ETH_P_IPV6)
397                 return 0;
398
399         hdr = ipv6_hdr(skb);
400         hc06_ptr = head + 2;
401
402         pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
403                  "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
404                  ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
405
406         lowpan_raw_dump_table(__func__, "raw skb network header dump",
407                 skb_network_header(skb), sizeof(struct ipv6hdr));
408
409         if (!saddr)
410                 saddr = dev->dev_addr;
411
412         lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
413
414         /*
415          * As we copy some bit-length fields, in the IPHC encoding bytes,
416          * we sometimes use |=
417          * If the field is 0, and the current bit value in memory is 1,
418          * this does not work. We therefore reset the IPHC encoding here
419          */
420         iphc0 = LOWPAN_DISPATCH_IPHC;
421         iphc1 = 0;
422
423         /* TODO: context lookup */
424
425         lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
426
427         /*
428          * Traffic class, flow label
429          * If flow label is 0, compress it. If traffic class is 0, compress it
430          * We have to process both in the same time as the offset of traffic
431          * class depends on the presence of version and flow label
432          */
433
434         /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
435         tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
436         tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
437
438         if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
439              (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
440                 /* flow label can be compressed */
441                 iphc0 |= LOWPAN_IPHC_FL_C;
442                 if ((hdr->priority == 0) &&
443                    ((hdr->flow_lbl[0] & 0xF0) == 0)) {
444                         /* compress (elide) all */
445                         iphc0 |= LOWPAN_IPHC_TC_C;
446                 } else {
447                         /* compress only the flow label */
448                         *hc06_ptr = tmp;
449                         hc06_ptr += 1;
450                 }
451         } else {
452                 /* Flow label cannot be compressed */
453                 if ((hdr->priority == 0) &&
454                    ((hdr->flow_lbl[0] & 0xF0) == 0)) {
455                         /* compress only traffic class */
456                         iphc0 |= LOWPAN_IPHC_TC_C;
457                         *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
458                         memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
459                         hc06_ptr += 3;
460                 } else {
461                         /* compress nothing */
462                         memcpy(hc06_ptr, &hdr, 4);
463                         /* replace the top byte with new ECN | DSCP format */
464                         *hc06_ptr = tmp;
465                         hc06_ptr += 4;
466                 }
467         }
468
469         /* NOTE: payload length is always compressed */
470
471         /* Next Header is compress if UDP */
472         if (hdr->nexthdr == UIP_PROTO_UDP)
473                 iphc0 |= LOWPAN_IPHC_NH_C;
474
475         if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
476                 *hc06_ptr = hdr->nexthdr;
477                 hc06_ptr += 1;
478         }
479
480         /*
481          * Hop limit
482          * if 1:   compress, encoding is 01
483          * if 64:  compress, encoding is 10
484          * if 255: compress, encoding is 11
485          * else do not compress
486          */
487         switch (hdr->hop_limit) {
488         case 1:
489                 iphc0 |= LOWPAN_IPHC_TTL_1;
490                 break;
491         case 64:
492                 iphc0 |= LOWPAN_IPHC_TTL_64;
493                 break;
494         case 255:
495                 iphc0 |= LOWPAN_IPHC_TTL_255;
496                 break;
497         default:
498                 *hc06_ptr = hdr->hop_limit;
499                 hc06_ptr += 1;
500                 break;
501         }
502
503         /* source address compression */
504         if (is_addr_unspecified(&hdr->saddr)) {
505                 pr_debug("source address is unspecified, setting SAC\n");
506                 iphc1 |= LOWPAN_IPHC_SAC;
507         /* TODO: context lookup */
508         } else if (is_addr_link_local(&hdr->saddr)) {
509                 pr_debug("source address is link-local\n");
510                 iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
511                                 LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
512         } else {
513                 pr_debug("send the full source address\n");
514                 memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
515                 hc06_ptr += 16;
516         }
517
518         /* destination address compression */
519         if (is_addr_mcast(&hdr->daddr)) {
520                 pr_debug("destination address is multicast: ");
521                 iphc1 |= LOWPAN_IPHC_M;
522                 if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
523                         pr_debug("compressed to 1 octet\n");
524                         iphc1 |= LOWPAN_IPHC_DAM_11;
525                         /* use last byte */
526                         *hc06_ptr = hdr->daddr.s6_addr[15];
527                         hc06_ptr += 1;
528                 } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
529                         pr_debug("compressed to 4 octets\n");
530                         iphc1 |= LOWPAN_IPHC_DAM_10;
531                         /* second byte + the last three */
532                         *hc06_ptr = hdr->daddr.s6_addr[1];
533                         memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
534                         hc06_ptr += 4;
535                 } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
536                         pr_debug("compressed to 6 octets\n");
537                         iphc1 |= LOWPAN_IPHC_DAM_01;
538                         /* second byte + the last five */
539                         *hc06_ptr = hdr->daddr.s6_addr[1];
540                         memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
541                         hc06_ptr += 6;
542                 } else {
543                         pr_debug("using full address\n");
544                         iphc1 |= LOWPAN_IPHC_DAM_00;
545                         memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
546                         hc06_ptr += 16;
547                 }
548         } else {
549                 /* TODO: context lookup */
550                 if (is_addr_link_local(&hdr->daddr)) {
551                         pr_debug("dest address is unicast and link-local\n");
552                         iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
553                                 LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
554                 } else {
555                         pr_debug("dest address is unicast: using full one\n");
556                         memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
557                         hc06_ptr += 16;
558                 }
559         }
560
561         /* UDP header compression */
562         if (hdr->nexthdr == UIP_PROTO_UDP)
563                 lowpan_compress_udp_header(&hc06_ptr, skb);
564
565         head[0] = iphc0;
566         head[1] = iphc1;
567
568         skb_pull(skb, sizeof(struct ipv6hdr));
569         memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
570
571         lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
572                                 skb->len);
573
574         /*
575          * NOTE1: I'm still unsure about the fact that compression and WPAN
576          * header are created here and not later in the xmit. So wait for
577          * an opinion of net maintainers.
578          */
579         /*
580          * NOTE2: to be absolutely correct, we must derive PANid information
581          * from MAC subif of the 'dev' and 'real_dev' network devices, but
582          * this isn't implemented in mainline yet, so currently we assign 0xff
583          */
584         {
585                 mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
586                 mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
587
588                 /* prepare wpan address data */
589                 sa.addr_type = IEEE802154_ADDR_LONG;
590                 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
591
592                 memcpy(&(sa.hwaddr), saddr, 8);
593                 /* intra-PAN communications */
594                 da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
595
596                 /*
597                  * if the destination address is the broadcast address, use the
598                  * corresponding short address
599                  */
600                 if (lowpan_is_addr_broadcast(daddr)) {
601                         da.addr_type = IEEE802154_ADDR_SHORT;
602                         da.short_addr = IEEE802154_ADDR_BROADCAST;
603                 } else {
604                         da.addr_type = IEEE802154_ADDR_LONG;
605                         memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
606
607                         /* request acknowledgment */
608                         mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
609                 }
610
611                 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
612                                 type, (void *)&da, (void *)&sa, skb->len);
613         }
614 }
615
616 static int lowpan_give_skb_to_devices(struct sk_buff *skb)
617 {
618         struct lowpan_dev_record *entry;
619         struct sk_buff *skb_cp;
620         int stat = NET_RX_SUCCESS;
621
622         rcu_read_lock();
623         list_for_each_entry_rcu(entry, &lowpan_devices, list)
624                 if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
625                         skb_cp = skb_copy(skb, GFP_ATOMIC);
626                         if (!skb_cp) {
627                                 stat = -ENOMEM;
628                                 break;
629                         }
630
631                         skb_cp->dev = entry->ldev;
632                         stat = netif_rx(skb_cp);
633                 }
634         rcu_read_unlock();
635
636         return stat;
637 }
638
639 static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
640 {
641         struct sk_buff *new;
642         int stat = NET_RX_SUCCESS;
643
644         new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
645                                                                 GFP_ATOMIC);
646         kfree_skb(skb);
647
648         if (!new)
649                 return -ENOMEM;
650
651         skb_push(new, sizeof(struct ipv6hdr));
652         skb_reset_network_header(new);
653         skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
654
655         new->protocol = htons(ETH_P_IPV6);
656         new->pkt_type = PACKET_HOST;
657
658         stat = lowpan_give_skb_to_devices(new);
659
660         kfree_skb(new);
661
662         return stat;
663 }
664
665 static void lowpan_fragment_timer_expired(unsigned long entry_addr)
666 {
667         struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
668
669         pr_debug("timer expired for frame with tag %d\n", entry->tag);
670
671         list_del(&entry->list);
672         dev_kfree_skb(entry->skb);
673         kfree(entry);
674 }
675
676 static struct lowpan_fragment *
677 lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 tag)
678 {
679         struct lowpan_fragment *frame;
680
681         frame = kzalloc(sizeof(struct lowpan_fragment),
682                         GFP_ATOMIC);
683         if (!frame)
684                 goto frame_err;
685
686         INIT_LIST_HEAD(&frame->list);
687
688         frame->length = len;
689         frame->tag = tag;
690
691         /* allocate buffer for frame assembling */
692         frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length +
693                                                sizeof(struct ipv6hdr));
694
695         if (!frame->skb)
696                 goto skb_err;
697
698         frame->skb->priority = skb->priority;
699         frame->skb->dev = skb->dev;
700
701         /* reserve headroom for uncompressed ipv6 header */
702         skb_reserve(frame->skb, sizeof(struct ipv6hdr));
703         skb_put(frame->skb, frame->length);
704
705         init_timer(&frame->timer);
706         /* time out is the same as for ipv6 - 60 sec */
707         frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
708         frame->timer.data = (unsigned long)frame;
709         frame->timer.function = lowpan_fragment_timer_expired;
710
711         add_timer(&frame->timer);
712
713         list_add_tail(&frame->list, &lowpan_fragments);
714
715         return frame;
716
717 skb_err:
718         kfree(frame);
719 frame_err:
720         return NULL;
721 }
722
723 static int
724 lowpan_process_data(struct sk_buff *skb)
725 {
726         struct ipv6hdr hdr;
727         u8 tmp, iphc0, iphc1, num_context = 0;
728         u8 *_saddr, *_daddr;
729         int err;
730
731         lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
732                                 skb->len);
733         /* at least two bytes will be used for the encoding */
734         if (skb->len < 2)
735                 goto drop;
736
737         if (lowpan_fetch_skb_u8(skb, &iphc0))
738                 goto drop;
739
740         /* fragments assembling */
741         switch (iphc0 & LOWPAN_DISPATCH_MASK) {
742         case LOWPAN_DISPATCH_FRAG1:
743         case LOWPAN_DISPATCH_FRAGN:
744         {
745                 struct lowpan_fragment *frame;
746                 /* slen stores the rightmost 8 bits of the 11 bits length */
747                 u8 slen, offset = 0;
748                 u16 len, tag;
749                 bool found = false;
750
751                 if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
752                     lowpan_fetch_skb_u16(skb, &tag))  /* fragment tag */
753                         goto drop;
754
755                 /* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
756                 len = ((iphc0 & 7) << 8) | slen;
757
758                 if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
759                         pr_debug("%s received a FRAG1 packet (tag: %d, "
760                                  "size of the entire IP packet: %d)",
761                                  __func__, tag, len);
762                 } else { /* FRAGN */
763                         if (lowpan_fetch_skb_u8(skb, &offset))
764                                 goto unlock_and_drop;
765                         pr_debug("%s received a FRAGN packet (tag: %d, "
766                                  "size of the entire IP packet: %d, "
767                                  "offset: %d)", __func__, tag, len, offset * 8);
768                 }
769
770                 /*
771                  * check if frame assembling with the same tag is
772                  * already in progress
773                  */
774                 spin_lock_bh(&flist_lock);
775
776                 list_for_each_entry(frame, &lowpan_fragments, list)
777                         if (frame->tag == tag) {
778                                 found = true;
779                                 break;
780                         }
781
782                 /* alloc new frame structure */
783                 if (!found) {
784                         pr_debug("%s first fragment received for tag %d, "
785                                  "begin packet reassembly", __func__, tag);
786                         frame = lowpan_alloc_new_frame(skb, len, tag);
787                         if (!frame)
788                                 goto unlock_and_drop;
789                 }
790
791                 /* if payload fits buffer, copy it */
792                 if (likely((offset * 8 + skb->len) <= frame->length))
793                         skb_copy_to_linear_data_offset(frame->skb, offset * 8,
794                                                         skb->data, skb->len);
795                 else
796                         goto unlock_and_drop;
797
798                 frame->bytes_rcv += skb->len;
799
800                 /* frame assembling complete */
801                 if ((frame->bytes_rcv == frame->length) &&
802                      frame->timer.expires > jiffies) {
803                         /* if timer haven't expired - first of all delete it */
804                         del_timer_sync(&frame->timer);
805                         list_del(&frame->list);
806                         spin_unlock_bh(&flist_lock);
807
808                         pr_debug("%s successfully reassembled fragment "
809                                  "(tag %d)", __func__, tag);
810
811                         dev_kfree_skb(skb);
812                         skb = frame->skb;
813                         kfree(frame);
814
815                         if (lowpan_fetch_skb_u8(skb, &iphc0))
816                                 goto drop;
817
818                         break;
819                 }
820                 spin_unlock_bh(&flist_lock);
821
822                 return kfree_skb(skb), 0;
823         }
824         default:
825                 break;
826         }
827
828         if (lowpan_fetch_skb_u8(skb, &iphc1))
829                 goto drop;
830
831         _saddr = mac_cb(skb)->sa.hwaddr;
832         _daddr = mac_cb(skb)->da.hwaddr;
833
834         pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1);
835
836         /* another if the CID flag is set */
837         if (iphc1 & LOWPAN_IPHC_CID) {
838                 pr_debug("CID flag is set, increase header with one\n");
839                 if (lowpan_fetch_skb_u8(skb, &num_context))
840                         goto drop;
841         }
842
843         hdr.version = 6;
844
845         /* Traffic Class and Flow Label */
846         switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
847         /*
848          * Traffic Class and FLow Label carried in-line
849          * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
850          */
851         case 0: /* 00b */
852                 if (lowpan_fetch_skb_u8(skb, &tmp))
853                         goto drop;
854
855                 memcpy(&hdr.flow_lbl, &skb->data[0], 3);
856                 skb_pull(skb, 3);
857                 hdr.priority = ((tmp >> 2) & 0x0f);
858                 hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
859                                         (hdr.flow_lbl[0] & 0x0f);
860                 break;
861         /*
862          * Traffic class carried in-line
863          * ECN + DSCP (1 byte), Flow Label is elided
864          */
865         case 2: /* 10b */
866                 if (lowpan_fetch_skb_u8(skb, &tmp))
867                         goto drop;
868
869                 hdr.priority = ((tmp >> 2) & 0x0f);
870                 hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
871                 hdr.flow_lbl[1] = 0;
872                 hdr.flow_lbl[2] = 0;
873                 break;
874         /*
875          * Flow Label carried in-line
876          * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
877          */
878         case 1: /* 01b */
879                 if (lowpan_fetch_skb_u8(skb, &tmp))
880                         goto drop;
881
882                 hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
883                 memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
884                 skb_pull(skb, 2);
885                 break;
886         /* Traffic Class and Flow Label are elided */
887         case 3: /* 11b */
888                 hdr.priority = 0;
889                 hdr.flow_lbl[0] = 0;
890                 hdr.flow_lbl[1] = 0;
891                 hdr.flow_lbl[2] = 0;
892                 break;
893         default:
894                 break;
895         }
896
897         /* Next Header */
898         if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
899                 /* Next header is carried inline */
900                 if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
901                         goto drop;
902
903                 pr_debug("NH flag is set, next header carried inline: %02x\n",
904                          hdr.nexthdr);
905         }
906
907         /* Hop Limit */
908         if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
909                 hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
910         else {
911                 if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
912                         goto drop;
913         }
914
915         /* Extract SAM to the tmp variable */
916         tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
917
918         /* Source address uncompression */
919         pr_debug("source address stateless compression\n");
920         err = lowpan_uncompress_addr(skb, &hdr.saddr, lowpan_llprefix,
921                                 lowpan_unc_llconf[tmp], skb->data);
922         if (err)
923                 goto drop;
924
925         /* Extract DAM to the tmp variable */
926         tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
927
928         /* check for Multicast Compression */
929         if (iphc1 & LOWPAN_IPHC_M) {
930                 if (iphc1 & LOWPAN_IPHC_DAC) {
931                         pr_debug("dest: context-based mcast compression\n");
932                         /* TODO: implement this */
933                 } else {
934                         u8 prefix[] = {0xff, 0x02};
935
936                         pr_debug("dest: non context-based mcast compression\n");
937                         if (0 < tmp && tmp < 3) {
938                                 if (lowpan_fetch_skb_u8(skb, &prefix[1]))
939                                         goto drop;
940                         }
941
942                         err = lowpan_uncompress_addr(skb, &hdr.daddr, prefix,
943                                         lowpan_unc_mxconf[tmp], NULL);
944                         if (err)
945                                 goto drop;
946                 }
947         } else {
948                 pr_debug("dest: stateless compression\n");
949                 err = lowpan_uncompress_addr(skb, &hdr.daddr, lowpan_llprefix,
950                                 lowpan_unc_llconf[tmp], skb->data);
951                 if (err)
952                         goto drop;
953         }
954
955         /* UDP data uncompression */
956         if (iphc0 & LOWPAN_IPHC_NH_C) {
957                 struct udphdr uh;
958                 struct sk_buff *new;
959                 if (lowpan_uncompress_udp_header(skb, &uh))
960                         goto drop;
961
962                 /*
963                  * replace the compressed UDP head by the uncompressed UDP
964                  * header
965                  */
966                 new = skb_copy_expand(skb, sizeof(struct udphdr),
967                                       skb_tailroom(skb), GFP_ATOMIC);
968                 kfree_skb(skb);
969
970                 if (!new)
971                         return -ENOMEM;
972
973                 skb = new;
974
975                 skb_push(skb, sizeof(struct udphdr));
976                 skb_reset_transport_header(skb);
977                 skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
978
979                 lowpan_raw_dump_table(__func__, "raw UDP header dump",
980                                       (u8 *)&uh, sizeof(uh));
981
982                 hdr.nexthdr = UIP_PROTO_UDP;
983         }
984
985         /* Not fragmented package */
986         hdr.payload_len = htons(skb->len);
987
988         pr_debug("skb headroom size = %d, data length = %d\n",
989                  skb_headroom(skb), skb->len);
990
991         pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
992                  "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
993                  ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
994
995         lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
996                                                         sizeof(hdr));
997         return lowpan_skb_deliver(skb, &hdr);
998
999 unlock_and_drop:
1000         spin_unlock_bh(&flist_lock);
1001 drop:
1002         kfree_skb(skb);
1003         return -EINVAL;
1004 }
1005
1006 static int lowpan_set_address(struct net_device *dev, void *p)
1007 {
1008         struct sockaddr *sa = p;
1009
1010         if (netif_running(dev))
1011                 return -EBUSY;
1012
1013         /* TODO: validate addr */
1014         memcpy(dev->dev_addr, sa->sa_data, dev->addr_len);
1015
1016         return 0;
1017 }
1018
1019 static int lowpan_get_mac_header_length(struct sk_buff *skb)
1020 {
1021         /*
1022          * Currently long addressing mode is supported only, so the overall
1023          * header size is 21:
1024          * FC SeqNum DPAN DA  SA  Sec
1025          * 2  +  1  +  2 + 8 + 8 + 0  = 21
1026          */
1027         return 21;
1028 }
1029
1030 static int
1031 lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
1032                         int mlen, int plen, int offset, int type)
1033 {
1034         struct sk_buff *frag;
1035         int hlen, ret;
1036
1037         hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
1038                         LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
1039
1040         lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
1041
1042         frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE);
1043         if (!frag)
1044                 return -ENOMEM;
1045
1046         frag->priority = skb->priority;
1047         frag->dev = skb->dev;
1048
1049         /* copy header, MFR and payload */
1050         memcpy(skb_put(frag, mlen), skb->data, mlen);
1051         memcpy(skb_put(frag, hlen), head, hlen);
1052
1053         if (plen)
1054                 skb_copy_from_linear_data_offset(skb, offset + mlen,
1055                                         skb_put(frag, plen), plen);
1056
1057         lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
1058                                                                 frag->len);
1059
1060         ret = dev_queue_xmit(frag);
1061
1062         return ret;
1063 }
1064
1065 static int
1066 lowpan_skb_fragmentation(struct sk_buff *skb, struct net_device *dev)
1067 {
1068         int  err, header_length, payload_length, tag, offset = 0;
1069         u8 head[5];
1070
1071         header_length = lowpan_get_mac_header_length(skb);
1072         payload_length = skb->len - header_length;
1073         tag = lowpan_dev_info(dev)->fragment_tag++;
1074
1075         /* first fragment header */
1076         head[0] = LOWPAN_DISPATCH_FRAG1 | ((payload_length >> 8) & 0x7);
1077         head[1] = payload_length & 0xff;
1078         head[2] = tag >> 8;
1079         head[3] = tag & 0xff;
1080
1081         err = lowpan_fragment_xmit(skb, head, header_length, LOWPAN_FRAG_SIZE,
1082                                    0, LOWPAN_DISPATCH_FRAG1);
1083
1084         if (err) {
1085                 pr_debug("%s unable to send FRAG1 packet (tag: %d)",
1086                          __func__, tag);
1087                 goto exit;
1088         }
1089
1090         offset = LOWPAN_FRAG_SIZE;
1091
1092         /* next fragment header */
1093         head[0] &= ~LOWPAN_DISPATCH_FRAG1;
1094         head[0] |= LOWPAN_DISPATCH_FRAGN;
1095
1096         while ((payload_length - offset > 0) && (err >= 0)) {
1097                 int len = LOWPAN_FRAG_SIZE;
1098
1099                 head[4] = offset / 8;
1100
1101                 if (payload_length - offset < len)
1102                         len = payload_length - offset;
1103
1104                 err = lowpan_fragment_xmit(skb, head, header_length,
1105                                            len, offset, LOWPAN_DISPATCH_FRAGN);
1106                 if (err) {
1107                         pr_debug("%s unable to send a subsequent FRAGN packet "
1108                                  "(tag: %d, offset: %d", __func__, tag, offset);
1109                         goto exit;
1110                 }
1111
1112                 offset += len;
1113         }
1114
1115 exit:
1116         return err;
1117 }
1118
1119 static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
1120 {
1121         int err = -1;
1122
1123         pr_debug("package xmit\n");
1124
1125         skb->dev = lowpan_dev_info(dev)->real_dev;
1126         if (skb->dev == NULL) {
1127                 pr_debug("ERROR: no real wpan device found\n");
1128                 goto error;
1129         }
1130
1131         /* Send directly if less than the MTU minus the 2 checksum bytes. */
1132         if (skb->len <= IEEE802154_MTU - IEEE802154_MFR_SIZE) {
1133                 err = dev_queue_xmit(skb);
1134                 goto out;
1135         }
1136
1137         pr_debug("frame is too big, fragmentation is needed\n");
1138         err = lowpan_skb_fragmentation(skb, dev);
1139 error:
1140         dev_kfree_skb(skb);
1141 out:
1142         if (err)
1143                 pr_debug("ERROR: xmit failed\n");
1144
1145         return (err < 0) ? NET_XMIT_DROP : err;
1146 }
1147
1148 static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
1149 {
1150         struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1151         return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
1152 }
1153
1154 static u16 lowpan_get_pan_id(const struct net_device *dev)
1155 {
1156         struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1157         return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
1158 }
1159
1160 static u16 lowpan_get_short_addr(const struct net_device *dev)
1161 {
1162         struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1163         return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
1164 }
1165
1166 static u8 lowpan_get_dsn(const struct net_device *dev)
1167 {
1168         struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
1169         return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
1170 }
1171
1172 static struct header_ops lowpan_header_ops = {
1173         .create = lowpan_header_create,
1174 };
1175
1176 static struct lock_class_key lowpan_tx_busylock;
1177 static struct lock_class_key lowpan_netdev_xmit_lock_key;
1178
1179 static void lowpan_set_lockdep_class_one(struct net_device *dev,
1180                                          struct netdev_queue *txq,
1181                                          void *_unused)
1182 {
1183         lockdep_set_class(&txq->_xmit_lock,
1184                           &lowpan_netdev_xmit_lock_key);
1185 }
1186
1187
1188 static int lowpan_dev_init(struct net_device *dev)
1189 {
1190         netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
1191         dev->qdisc_tx_busylock = &lowpan_tx_busylock;
1192         return 0;
1193 }
1194
1195 static const struct net_device_ops lowpan_netdev_ops = {
1196         .ndo_init               = lowpan_dev_init,
1197         .ndo_start_xmit         = lowpan_xmit,
1198         .ndo_set_mac_address    = lowpan_set_address,
1199 };
1200
1201 static struct ieee802154_mlme_ops lowpan_mlme = {
1202         .get_pan_id = lowpan_get_pan_id,
1203         .get_phy = lowpan_get_phy,
1204         .get_short_addr = lowpan_get_short_addr,
1205         .get_dsn = lowpan_get_dsn,
1206 };
1207
1208 static void lowpan_setup(struct net_device *dev)
1209 {
1210         dev->addr_len           = IEEE802154_ADDR_LEN;
1211         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
1212         dev->type               = ARPHRD_IEEE802154;
1213         /* Frame Control + Sequence Number + Address fields + Security Header */
1214         dev->hard_header_len    = 2 + 1 + 20 + 14;
1215         dev->needed_tailroom    = 2; /* FCS */
1216         dev->mtu                = 1281;
1217         dev->tx_queue_len       = 0;
1218         dev->flags              = IFF_BROADCAST | IFF_MULTICAST;
1219         dev->watchdog_timeo     = 0;
1220
1221         dev->netdev_ops         = &lowpan_netdev_ops;
1222         dev->header_ops         = &lowpan_header_ops;
1223         dev->ml_priv            = &lowpan_mlme;
1224         dev->destructor         = free_netdev;
1225 }
1226
1227 static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
1228 {
1229         if (tb[IFLA_ADDRESS]) {
1230                 if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
1231                         return -EINVAL;
1232         }
1233         return 0;
1234 }
1235
1236 static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1237         struct packet_type *pt, struct net_device *orig_dev)
1238 {
1239         struct sk_buff *local_skb;
1240
1241         if (!netif_running(dev))
1242                 goto drop;
1243
1244         if (dev->type != ARPHRD_IEEE802154)
1245                 goto drop;
1246
1247         /* check that it's our buffer */
1248         if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
1249                 /* Copy the packet so that the IPv6 header is
1250                  * properly aligned.
1251                  */
1252                 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
1253                                             skb_tailroom(skb), GFP_ATOMIC);
1254                 if (!local_skb)
1255                         goto drop;
1256
1257                 local_skb->protocol = htons(ETH_P_IPV6);
1258                 local_skb->pkt_type = PACKET_HOST;
1259
1260                 /* Pull off the 1-byte of 6lowpan header. */
1261                 skb_pull(local_skb, 1);
1262                 skb_reset_network_header(local_skb);
1263                 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
1264
1265                 lowpan_give_skb_to_devices(local_skb);
1266
1267                 kfree_skb(local_skb);
1268                 kfree_skb(skb);
1269         } else {
1270                 switch (skb->data[0] & 0xe0) {
1271                 case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
1272                 case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
1273                 case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
1274                         local_skb = skb_clone(skb, GFP_ATOMIC);
1275                         if (!local_skb)
1276                                 goto drop;
1277                         lowpan_process_data(local_skb);
1278
1279                         kfree_skb(skb);
1280                         break;
1281                 default:
1282                         break;
1283                 }
1284         }
1285
1286         return NET_RX_SUCCESS;
1287
1288 drop:
1289         kfree_skb(skb);
1290         return NET_RX_DROP;
1291 }
1292
1293 static int lowpan_newlink(struct net *src_net, struct net_device *dev,
1294                           struct nlattr *tb[], struct nlattr *data[])
1295 {
1296         struct net_device *real_dev;
1297         struct lowpan_dev_record *entry;
1298
1299         pr_debug("adding new link\n");
1300
1301         if (!tb[IFLA_LINK])
1302                 return -EINVAL;
1303         /* find and hold real wpan device */
1304         real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
1305         if (!real_dev)
1306                 return -ENODEV;
1307
1308         lowpan_dev_info(dev)->real_dev = real_dev;
1309         lowpan_dev_info(dev)->fragment_tag = 0;
1310         mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
1311
1312         entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
1313         if (!entry) {
1314                 dev_put(real_dev);
1315                 lowpan_dev_info(dev)->real_dev = NULL;
1316                 return -ENOMEM;
1317         }
1318
1319         entry->ldev = dev;
1320
1321         mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1322         INIT_LIST_HEAD(&entry->list);
1323         list_add_tail(&entry->list, &lowpan_devices);
1324         mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1325
1326         register_netdevice(dev);
1327
1328         return 0;
1329 }
1330
1331 static void lowpan_dellink(struct net_device *dev, struct list_head *head)
1332 {
1333         struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
1334         struct net_device *real_dev = lowpan_dev->real_dev;
1335         struct lowpan_dev_record *entry, *tmp;
1336
1337         ASSERT_RTNL();
1338
1339         mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1340         list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1341                 if (entry->ldev == dev) {
1342                         list_del(&entry->list);
1343                         kfree(entry);
1344                 }
1345         }
1346         mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1347
1348         mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
1349
1350         unregister_netdevice_queue(dev, head);
1351
1352         dev_put(real_dev);
1353 }
1354
1355 static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
1356         .kind           = "lowpan",
1357         .priv_size      = sizeof(struct lowpan_dev_info),
1358         .setup          = lowpan_setup,
1359         .newlink        = lowpan_newlink,
1360         .dellink        = lowpan_dellink,
1361         .validate       = lowpan_validate,
1362 };
1363
1364 static inline int __init lowpan_netlink_init(void)
1365 {
1366         return rtnl_link_register(&lowpan_link_ops);
1367 }
1368
1369 static inline void lowpan_netlink_fini(void)
1370 {
1371         rtnl_link_unregister(&lowpan_link_ops);
1372 }
1373
1374 static int lowpan_device_event(struct notifier_block *unused,
1375                                 unsigned long event,
1376                                 void *ptr)
1377 {
1378         struct net_device *dev = ptr;
1379         LIST_HEAD(del_list);
1380         struct lowpan_dev_record *entry, *tmp;
1381
1382         if (dev->type != ARPHRD_IEEE802154)
1383                 goto out;
1384
1385         if (event == NETDEV_UNREGISTER) {
1386                 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1387                         if (lowpan_dev_info(entry->ldev)->real_dev == dev)
1388                                 lowpan_dellink(entry->ldev, &del_list);
1389                 }
1390
1391                 unregister_netdevice_many(&del_list);
1392         }
1393
1394 out:
1395         return NOTIFY_DONE;
1396 }
1397
1398 static struct notifier_block lowpan_dev_notifier = {
1399         .notifier_call = lowpan_device_event,
1400 };
1401
1402 static struct packet_type lowpan_packet_type = {
1403         .type = __constant_htons(ETH_P_IEEE802154),
1404         .func = lowpan_rcv,
1405 };
1406
1407 static int __init lowpan_init_module(void)
1408 {
1409         int err = 0;
1410
1411         err = lowpan_netlink_init();
1412         if (err < 0)
1413                 goto out;
1414
1415         dev_add_pack(&lowpan_packet_type);
1416
1417         err = register_netdevice_notifier(&lowpan_dev_notifier);
1418         if (err < 0) {
1419                 dev_remove_pack(&lowpan_packet_type);
1420                 lowpan_netlink_fini();
1421         }
1422 out:
1423         return err;
1424 }
1425
1426 static void __exit lowpan_cleanup_module(void)
1427 {
1428         struct lowpan_fragment *frame, *tframe;
1429
1430         lowpan_netlink_fini();
1431
1432         dev_remove_pack(&lowpan_packet_type);
1433
1434         unregister_netdevice_notifier(&lowpan_dev_notifier);
1435
1436         /* Now 6lowpan packet_type is removed, so no new fragments are
1437          * expected on RX, therefore that's the time to clean incomplete
1438          * fragments.
1439          */
1440         spin_lock_bh(&flist_lock);
1441         list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
1442                 del_timer_sync(&frame->timer);
1443                 list_del(&frame->list);
1444                 dev_kfree_skb(frame->skb);
1445                 kfree(frame);
1446         }
1447         spin_unlock_bh(&flist_lock);
1448 }
1449
1450 module_init(lowpan_init_module);
1451 module_exit(lowpan_cleanup_module);
1452 MODULE_LICENSE("GPL");
1453 MODULE_ALIAS_RTNL_LINK("lowpan");