Kernel

Kernel Git Source Tree

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

Download this file

Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master

Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9

Kernel

Kernel Git Source Tree

Root/net/ieee802154/6lowpan.c

interactive