~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

TOMOYO Linux Cross Reference
Linux/net/ieee802154/6lowpan.c

Version: ~ [ linux-5.7 ] ~ [ linux-5.6.15 ] ~ [ linux-5.5.19 ] ~ [ linux-5.4.43 ] ~ [ linux-5.3.18 ] ~ [ linux-5.2.21 ] ~ [ linux-5.1.21 ] ~ [ linux-5.0.21 ] ~ [ linux-4.20.17 ] ~ [ linux-4.19.125 ] ~ [ linux-4.18.20 ] ~ [ linux-4.17.19 ] ~ [ linux-4.16.18 ] ~ [ linux-4.15.18 ] ~ [ linux-4.14.182 ] ~ [ linux-4.13.16 ] ~ [ linux-4.12.14 ] ~ [ linux-4.11.12 ] ~ [ linux-4.10.17 ] ~ [ linux-4.9.225 ] ~ [ linux-4.8.17 ] ~ [ linux-4.7.10 ] ~ [ linux-4.6.7 ] ~ [ linux-4.5.7 ] ~ [ linux-4.4.225 ] ~ [ linux-4.3.6 ] ~ [ linux-4.2.8 ] ~ [ linux-4.1.52 ] ~ [ linux-4.0.9 ] ~ [ linux-3.19.8 ] ~ [ linux-3.18.140 ] ~ [ linux-3.17.8 ] ~ [ linux-3.16.84 ] ~ [ linux-3.15.10 ] ~ [ linux-3.14.79 ] ~ [ linux-3.13.11 ] ~ [ linux-3.12.74 ] ~ [ linux-3.11.10 ] ~ [ linux-3.10.108 ] ~ [ linux-2.6.32.71 ] ~ [ linux-2.6.0 ] ~ [ linux-2.4.37.11 ] ~ [ unix-v6-master ] ~ [ ccs-tools-1.8.5 ] ~ [ policy-sample ] ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  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 1: /* 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 2: /* 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 const struct net_device_ops lowpan_netdev_ops = {
1177         .ndo_start_xmit         = lowpan_xmit,
1178         .ndo_set_mac_address    = lowpan_set_address,
1179 };
1180 
1181 static struct ieee802154_mlme_ops lowpan_mlme = {
1182         .get_pan_id = lowpan_get_pan_id,
1183         .get_phy = lowpan_get_phy,
1184         .get_short_addr = lowpan_get_short_addr,
1185         .get_dsn = lowpan_get_dsn,
1186 };
1187 
1188 static void lowpan_setup(struct net_device *dev)
1189 {
1190         dev->addr_len           = IEEE802154_ADDR_LEN;
1191         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
1192         dev->type               = ARPHRD_IEEE802154;
1193         /* Frame Control + Sequence Number + Address fields + Security Header */
1194         dev->hard_header_len    = 2 + 1 + 20 + 14;
1195         dev->needed_tailroom    = 2; /* FCS */
1196         dev->mtu                = 1281;
1197         dev->tx_queue_len       = 0;
1198         dev->flags              = IFF_BROADCAST | IFF_MULTICAST;
1199         dev->watchdog_timeo     = 0;
1200 
1201         dev->netdev_ops         = &lowpan_netdev_ops;
1202         dev->header_ops         = &lowpan_header_ops;
1203         dev->ml_priv            = &lowpan_mlme;
1204         dev->destructor         = free_netdev;
1205 }
1206 
1207 static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
1208 {
1209         if (tb[IFLA_ADDRESS]) {
1210                 if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
1211                         return -EINVAL;
1212         }
1213         return 0;
1214 }
1215 
1216 static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
1217         struct packet_type *pt, struct net_device *orig_dev)
1218 {
1219         struct sk_buff *local_skb;
1220 
1221         if (!netif_running(dev))
1222                 goto drop;
1223 
1224         if (dev->type != ARPHRD_IEEE802154)
1225                 goto drop;
1226 
1227         /* check that it's our buffer */
1228         if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
1229                 /* Copy the packet so that the IPv6 header is
1230                  * properly aligned.
1231                  */
1232                 local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
1233                                             skb_tailroom(skb), GFP_ATOMIC);
1234                 if (!local_skb)
1235                         goto drop;
1236 
1237                 local_skb->protocol = htons(ETH_P_IPV6);
1238                 local_skb->pkt_type = PACKET_HOST;
1239 
1240                 /* Pull off the 1-byte of 6lowpan header. */
1241                 skb_pull(local_skb, 1);
1242                 skb_reset_network_header(local_skb);
1243                 skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
1244 
1245                 lowpan_give_skb_to_devices(local_skb);
1246 
1247                 kfree_skb(local_skb);
1248                 kfree_skb(skb);
1249         } else {
1250                 switch (skb->data[0] & 0xe0) {
1251                 case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
1252                 case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
1253                 case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
1254                         local_skb = skb_clone(skb, GFP_ATOMIC);
1255                         if (!local_skb)
1256                                 goto drop;
1257                         lowpan_process_data(local_skb);
1258 
1259                         kfree_skb(skb);
1260                         break;
1261                 default:
1262                         break;
1263                 }
1264         }
1265 
1266         return NET_RX_SUCCESS;
1267 
1268 drop:
1269         kfree_skb(skb);
1270         return NET_RX_DROP;
1271 }
1272 
1273 static int lowpan_newlink(struct net *src_net, struct net_device *dev,
1274                           struct nlattr *tb[], struct nlattr *data[])
1275 {
1276         struct net_device *real_dev;
1277         struct lowpan_dev_record *entry;
1278 
1279         pr_debug("adding new link\n");
1280 
1281         if (!tb[IFLA_LINK])
1282                 return -EINVAL;
1283         /* find and hold real wpan device */
1284         real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
1285         if (!real_dev)
1286                 return -ENODEV;
1287 
1288         lowpan_dev_info(dev)->real_dev = real_dev;
1289         lowpan_dev_info(dev)->fragment_tag = 0;
1290         mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
1291 
1292         entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
1293         if (!entry) {
1294                 dev_put(real_dev);
1295                 lowpan_dev_info(dev)->real_dev = NULL;
1296                 return -ENOMEM;
1297         }
1298 
1299         entry->ldev = dev;
1300 
1301         mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1302         INIT_LIST_HEAD(&entry->list);
1303         list_add_tail(&entry->list, &lowpan_devices);
1304         mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1305 
1306         register_netdevice(dev);
1307 
1308         return 0;
1309 }
1310 
1311 static void lowpan_dellink(struct net_device *dev, struct list_head *head)
1312 {
1313         struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
1314         struct net_device *real_dev = lowpan_dev->real_dev;
1315         struct lowpan_dev_record *entry, *tmp;
1316 
1317         ASSERT_RTNL();
1318 
1319         mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
1320         list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1321                 if (entry->ldev == dev) {
1322                         list_del(&entry->list);
1323                         kfree(entry);
1324                 }
1325         }
1326         mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
1327 
1328         mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
1329 
1330         unregister_netdevice_queue(dev, head);
1331 
1332         dev_put(real_dev);
1333 }
1334 
1335 static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
1336         .kind           = "lowpan",
1337         .priv_size      = sizeof(struct lowpan_dev_info),
1338         .setup          = lowpan_setup,
1339         .newlink        = lowpan_newlink,
1340         .dellink        = lowpan_dellink,
1341         .validate       = lowpan_validate,
1342 };
1343 
1344 static inline int __init lowpan_netlink_init(void)
1345 {
1346         return rtnl_link_register(&lowpan_link_ops);
1347 }
1348 
1349 static inline void lowpan_netlink_fini(void)
1350 {
1351         rtnl_link_unregister(&lowpan_link_ops);
1352 }
1353 
1354 static int lowpan_device_event(struct notifier_block *unused,
1355                                unsigned long event, void *ptr)
1356 {
1357         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1358         LIST_HEAD(del_list);
1359         struct lowpan_dev_record *entry, *tmp;
1360 
1361         if (dev->type != ARPHRD_IEEE802154)
1362                 goto out;
1363 
1364         if (event == NETDEV_UNREGISTER) {
1365                 list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
1366                         if (lowpan_dev_info(entry->ldev)->real_dev == dev)
1367                                 lowpan_dellink(entry->ldev, &del_list);
1368                 }
1369 
1370                 unregister_netdevice_many(&del_list);
1371         }
1372 
1373 out:
1374         return NOTIFY_DONE;
1375 }
1376 
1377 static struct notifier_block lowpan_dev_notifier = {
1378         .notifier_call = lowpan_device_event,
1379 };
1380 
1381 static struct packet_type lowpan_packet_type = {
1382         .type = __constant_htons(ETH_P_IEEE802154),
1383         .func = lowpan_rcv,
1384 };
1385 
1386 static int __init lowpan_init_module(void)
1387 {
1388         int err = 0;
1389 
1390         err = lowpan_netlink_init();
1391         if (err < 0)
1392                 goto out;
1393 
1394         dev_add_pack(&lowpan_packet_type);
1395 
1396         err = register_netdevice_notifier(&lowpan_dev_notifier);
1397         if (err < 0) {
1398                 dev_remove_pack(&lowpan_packet_type);
1399                 lowpan_netlink_fini();
1400         }
1401 out:
1402         return err;
1403 }
1404 
1405 static void __exit lowpan_cleanup_module(void)
1406 {
1407         struct lowpan_fragment *frame, *tframe;
1408 
1409         lowpan_netlink_fini();
1410 
1411         dev_remove_pack(&lowpan_packet_type);
1412 
1413         unregister_netdevice_notifier(&lowpan_dev_notifier);
1414 
1415         /* Now 6lowpan packet_type is removed, so no new fragments are
1416          * expected on RX, therefore that's the time to clean incomplete
1417          * fragments.
1418          */
1419         spin_lock_bh(&flist_lock);
1420         list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
1421                 del_timer_sync(&frame->timer);
1422                 list_del(&frame->list);
1423                 dev_kfree_skb(frame->skb);
1424                 kfree(frame);
1425         }
1426         spin_unlock_bh(&flist_lock);
1427 }
1428 
1429 module_init(lowpan_init_module);
1430 module_exit(lowpan_cleanup_module);
1431 MODULE_LICENSE("GPL");
1432 MODULE_ALIAS_RTNL_LINK("lowpan");
1433 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

kernel.org | git.kernel.org | LWN.net | Project Home | Wiki (Japanese) | Wiki (English) | SVN repository | Mail admin

Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.

osdn.jp