6lowpan.c 33 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284
  1. /*
  2. * Copyright 2011, Siemens AG
  3. * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  4. */
  5. /*
  6. * Based on patches from Jon Smirl <jonsmirl@gmail.com>
  7. * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
  8. *
  9. * This program is free software; you can redistribute it and/or modify
  10. * it under the terms of the GNU General Public License version 2
  11. * as published by the Free Software Foundation.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License along
  19. * with this program; if not, write to the Free Software Foundation, Inc.,
  20. * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  21. */
  22. /* Jon's code is based on 6lowpan implementation for Contiki which is:
  23. * Copyright (c) 2008, Swedish Institute of Computer Science.
  24. * All rights reserved.
  25. *
  26. * Redistribution and use in source and binary forms, with or without
  27. * modification, are permitted provided that the following conditions
  28. * are met:
  29. * 1. Redistributions of source code must retain the above copyright
  30. * notice, this list of conditions and the following disclaimer.
  31. * 2. Redistributions in binary form must reproduce the above copyright
  32. * notice, this list of conditions and the following disclaimer in the
  33. * documentation and/or other materials provided with the distribution.
  34. * 3. Neither the name of the Institute nor the names of its contributors
  35. * may be used to endorse or promote products derived from this software
  36. * without specific prior written permission.
  37. *
  38. * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
  39. * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  40. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  41. * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
  42. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  43. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  44. * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  45. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  46. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  47. * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  48. * SUCH DAMAGE.
  49. */
  50. #include <linux/bitops.h>
  51. #include <linux/if_arp.h>
  52. #include <linux/module.h>
  53. #include <linux/moduleparam.h>
  54. #include <linux/netdevice.h>
  55. #include <linux/etherdevice.h>
  56. #include <net/af_ieee802154.h>
  57. #include <net/ieee802154.h>
  58. #include <net/ieee802154_netdev.h>
  59. #include <net/ipv6.h>
  60. #include "6lowpan.h"
  61. /* TTL uncompression values */
  62. static const u8 lowpan_ttl_values[] = {0, 1, 64, 255};
  63. static LIST_HEAD(lowpan_devices);
  64. /*
  65. * Uncompression of linklocal:
  66. * 0 -> 16 bytes from packet
  67. * 1 -> 2 bytes from prefix - bunch of zeroes and 8 from packet
  68. * 2 -> 2 bytes from prefix - zeroes + 2 from packet
  69. * 3 -> 2 bytes from prefix - infer 8 bytes from lladdr
  70. *
  71. * NOTE: => the uncompress function does change 0xf to 0x10
  72. * NOTE: 0x00 => no-autoconfig => unspecified
  73. */
  74. static const u8 lowpan_unc_llconf[] = {0x0f, 0x28, 0x22, 0x20};
  75. /*
  76. * Uncompression of ctx-based:
  77. * 0 -> 0 bits from packet [unspecified / reserved]
  78. * 1 -> 8 bytes from prefix - bunch of zeroes and 8 from packet
  79. * 2 -> 8 bytes from prefix - zeroes + 2 from packet
  80. * 3 -> 8 bytes from prefix - infer 8 bytes from lladdr
  81. */
  82. static const u8 lowpan_unc_ctxconf[] = {0x00, 0x88, 0x82, 0x80};
  83. /*
  84. * Uncompression of ctx-base
  85. * 0 -> 0 bits from packet
  86. * 1 -> 2 bytes from prefix - bunch of zeroes 5 from packet
  87. * 2 -> 2 bytes from prefix - zeroes + 3 from packet
  88. * 3 -> 2 bytes from prefix - infer 1 bytes from lladdr
  89. */
  90. static const u8 lowpan_unc_mxconf[] = {0x0f, 0x25, 0x23, 0x21};
  91. /* Link local prefix */
  92. static const u8 lowpan_llprefix[] = {0xfe, 0x80};
  93. /* private device info */
  94. struct lowpan_dev_info {
  95. struct net_device *real_dev; /* real WPAN device ptr */
  96. struct mutex dev_list_mtx; /* mutex for list ops */
  97. };
  98. struct lowpan_dev_record {
  99. struct net_device *ldev;
  100. struct list_head list;
  101. };
  102. struct lowpan_fragment {
  103. struct sk_buff *skb; /* skb to be assembled */
  104. spinlock_t lock; /* concurency lock */
  105. u16 length; /* length to be assemled */
  106. u32 bytes_rcv; /* bytes received */
  107. u16 tag; /* current fragment tag */
  108. struct timer_list timer; /* assembling timer */
  109. struct list_head list; /* fragments list */
  110. };
  111. static unsigned short fragment_tag;
  112. static LIST_HEAD(lowpan_fragments);
  113. spinlock_t flist_lock;
  114. static inline struct
  115. lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
  116. {
  117. return netdev_priv(dev);
  118. }
  119. static inline void lowpan_address_flip(u8 *src, u8 *dest)
  120. {
  121. int i;
  122. for (i = 0; i < IEEE802154_ADDR_LEN; i++)
  123. (dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
  124. }
  125. /* list of all 6lowpan devices, uses for package delivering */
  126. /* print data in line */
  127. static inline void lowpan_raw_dump_inline(const char *caller, char *msg,
  128. unsigned char *buf, int len)
  129. {
  130. #ifdef DEBUG
  131. if (msg)
  132. pr_debug("(%s) %s: ", caller, msg);
  133. print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,
  134. 16, 1, buf, len, false);
  135. #endif /* DEBUG */
  136. }
  137. /*
  138. * print data in a table format:
  139. *
  140. * addr: xx xx xx xx xx xx
  141. * addr: xx xx xx xx xx xx
  142. * ...
  143. */
  144. static inline void lowpan_raw_dump_table(const char *caller, char *msg,
  145. unsigned char *buf, int len)
  146. {
  147. #ifdef DEBUG
  148. if (msg)
  149. pr_debug("(%s) %s:\n", caller, msg);
  150. print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET,
  151. 16, 1, buf, len, false);
  152. #endif /* DEBUG */
  153. }
  154. static u8
  155. lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr,
  156. const unsigned char *lladdr)
  157. {
  158. u8 val = 0;
  159. if (is_addr_mac_addr_based(ipaddr, lladdr))
  160. val = 3; /* 0-bits */
  161. else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
  162. /* compress IID to 16 bits xxxx::XXXX */
  163. memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
  164. *hc06_ptr += 2;
  165. val = 2; /* 16-bits */
  166. } else {
  167. /* do not compress IID => xxxx::IID */
  168. memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
  169. *hc06_ptr += 8;
  170. val = 1; /* 64-bits */
  171. }
  172. return rol8(val, shift);
  173. }
  174. static void
  175. lowpan_uip_ds6_set_addr_iid(struct in6_addr *ipaddr, unsigned char *lladdr)
  176. {
  177. memcpy(&ipaddr->s6_addr[8], lladdr, IEEE802154_ADDR_LEN);
  178. /* second bit-flip (Universe/Local) is done according RFC2464 */
  179. ipaddr->s6_addr[8] ^= 0x02;
  180. }
  181. /*
  182. * Uncompress addresses based on a prefix and a postfix with zeroes in
  183. * between. If the postfix is zero in length it will use the link address
  184. * to configure the IP address (autoconf style).
  185. * pref_post_count takes a byte where the first nibble specify prefix count
  186. * and the second postfix count (NOTE: 15/0xf => 16 bytes copy).
  187. */
  188. static int
  189. lowpan_uncompress_addr(struct sk_buff *skb, struct in6_addr *ipaddr,
  190. u8 const *prefix, u8 pref_post_count, unsigned char *lladdr)
  191. {
  192. u8 prefcount = pref_post_count >> 4;
  193. u8 postcount = pref_post_count & 0x0f;
  194. /* full nibble 15 => 16 */
  195. prefcount = (prefcount == 15 ? 16 : prefcount);
  196. postcount = (postcount == 15 ? 16 : postcount);
  197. if (lladdr)
  198. lowpan_raw_dump_inline(__func__, "linklocal address",
  199. lladdr, IEEE802154_ADDR_LEN);
  200. if (prefcount > 0)
  201. memcpy(ipaddr, prefix, prefcount);
  202. if (prefcount + postcount < 16)
  203. memset(&ipaddr->s6_addr[prefcount], 0,
  204. 16 - (prefcount + postcount));
  205. if (postcount > 0) {
  206. memcpy(&ipaddr->s6_addr[16 - postcount], skb->data, postcount);
  207. skb_pull(skb, postcount);
  208. } else if (prefcount > 0) {
  209. if (lladdr == NULL)
  210. return -EINVAL;
  211. /* no IID based configuration if no prefix and no data */
  212. lowpan_uip_ds6_set_addr_iid(ipaddr, lladdr);
  213. }
  214. pr_debug("(%s): uncompressing %d + %d => ", __func__, prefcount,
  215. postcount);
  216. lowpan_raw_dump_inline(NULL, NULL, ipaddr->s6_addr, 16);
  217. return 0;
  218. }
  219. static void
  220. lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
  221. {
  222. struct udphdr *uh = udp_hdr(skb);
  223. pr_debug("(%s): UDP header compression\n", __func__);
  224. if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) ==
  225. LOWPAN_NHC_UDP_4BIT_PORT) &&
  226. ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) ==
  227. LOWPAN_NHC_UDP_4BIT_PORT)) {
  228. pr_debug("(%s): both ports compression to 4 bits\n", __func__);
  229. **hc06_ptr = LOWPAN_NHC_UDP_CS_P_11;
  230. **(hc06_ptr + 1) = /* subtraction is faster */
  231. (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) +
  232. ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4));
  233. *hc06_ptr += 2;
  234. } else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) ==
  235. LOWPAN_NHC_UDP_8BIT_PORT) {
  236. pr_debug("(%s): remove 8 bits of dest\n", __func__);
  237. **hc06_ptr = LOWPAN_NHC_UDP_CS_P_01;
  238. memcpy(*hc06_ptr + 1, &uh->source, 2);
  239. **(hc06_ptr + 3) = (u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT);
  240. *hc06_ptr += 4;
  241. } else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) ==
  242. LOWPAN_NHC_UDP_8BIT_PORT) {
  243. pr_debug("(%s): remove 8 bits of source\n", __func__);
  244. **hc06_ptr = LOWPAN_NHC_UDP_CS_P_10;
  245. memcpy(*hc06_ptr + 1, &uh->dest, 2);
  246. **(hc06_ptr + 3) = (u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT);
  247. *hc06_ptr += 4;
  248. } else {
  249. pr_debug("(%s): can't compress header\n", __func__);
  250. **hc06_ptr = LOWPAN_NHC_UDP_CS_P_00;
  251. memcpy(*hc06_ptr + 1, &uh->source, 2);
  252. memcpy(*hc06_ptr + 3, &uh->dest, 2);
  253. *hc06_ptr += 5;
  254. }
  255. /* checksum is always inline */
  256. memcpy(*hc06_ptr, &uh->check, 2);
  257. *hc06_ptr += 2;
  258. }
  259. static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
  260. {
  261. if (unlikely(!pskb_may_pull(skb, 1)))
  262. return -EINVAL;
  263. *val = skb->data[0];
  264. skb_pull(skb, 1);
  265. return 0;
  266. }
  267. static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
  268. {
  269. if (unlikely(!pskb_may_pull(skb, 2)))
  270. return -EINVAL;
  271. *val = skb->data[0] | (skb->data[1] << 8);
  272. skb_pull(skb, 2);
  273. return 0;
  274. }
  275. static int
  276. lowpan_uncompress_udp_header(struct sk_buff *skb)
  277. {
  278. struct udphdr *uh = udp_hdr(skb);
  279. u8 tmp;
  280. if (lowpan_fetch_skb_u8(skb, &tmp))
  281. goto err;
  282. if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
  283. pr_debug("(%s): UDP header uncompression\n", __func__);
  284. switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
  285. case LOWPAN_NHC_UDP_CS_P_00:
  286. memcpy(&uh->source, &skb->data[0], 2);
  287. memcpy(&uh->dest, &skb->data[2], 2);
  288. skb_pull(skb, 4);
  289. break;
  290. case LOWPAN_NHC_UDP_CS_P_01:
  291. memcpy(&uh->source, &skb->data[0], 2);
  292. uh->dest =
  293. skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT;
  294. skb_pull(skb, 3);
  295. break;
  296. case LOWPAN_NHC_UDP_CS_P_10:
  297. uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT;
  298. memcpy(&uh->dest, &skb->data[1], 2);
  299. skb_pull(skb, 3);
  300. break;
  301. case LOWPAN_NHC_UDP_CS_P_11:
  302. uh->source =
  303. LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4);
  304. uh->dest =
  305. LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f);
  306. skb_pull(skb, 1);
  307. break;
  308. default:
  309. pr_debug("(%s) ERROR: unknown UDP format\n", __func__);
  310. goto err;
  311. break;
  312. }
  313. pr_debug("(%s): uncompressed UDP ports: src = %d, dst = %d\n",
  314. __func__, uh->source, uh->dest);
  315. /* copy checksum */
  316. memcpy(&uh->check, &skb->data[0], 2);
  317. skb_pull(skb, 2);
  318. } else {
  319. pr_debug("(%s): ERROR: unsupported NH format\n", __func__);
  320. goto err;
  321. }
  322. return 0;
  323. err:
  324. return -EINVAL;
  325. }
  326. static int lowpan_header_create(struct sk_buff *skb,
  327. struct net_device *dev,
  328. unsigned short type, const void *_daddr,
  329. const void *_saddr, unsigned int len)
  330. {
  331. u8 tmp, iphc0, iphc1, *hc06_ptr;
  332. struct ipv6hdr *hdr;
  333. const u8 *saddr = _saddr;
  334. const u8 *daddr = _daddr;
  335. u8 *head;
  336. struct ieee802154_addr sa, da;
  337. if (type != ETH_P_IPV6)
  338. return 0;
  339. /* TODO:
  340. * if this package isn't ipv6 one, where should it be routed?
  341. */
  342. head = kzalloc(100, GFP_KERNEL);
  343. if (head == NULL)
  344. return -ENOMEM;
  345. hdr = ipv6_hdr(skb);
  346. hc06_ptr = head + 2;
  347. pr_debug("(%s): IPv6 header dump:\n\tversion = %d\n\tlength = %d\n"
  348. "\tnexthdr = 0x%02x\n\thop_lim = %d\n", __func__,
  349. hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
  350. hdr->hop_limit);
  351. lowpan_raw_dump_table(__func__, "raw skb network header dump",
  352. skb_network_header(skb), sizeof(struct ipv6hdr));
  353. if (!saddr)
  354. saddr = dev->dev_addr;
  355. lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
  356. /*
  357. * As we copy some bit-length fields, in the IPHC encoding bytes,
  358. * we sometimes use |=
  359. * If the field is 0, and the current bit value in memory is 1,
  360. * this does not work. We therefore reset the IPHC encoding here
  361. */
  362. iphc0 = LOWPAN_DISPATCH_IPHC;
  363. iphc1 = 0;
  364. /* TODO: context lookup */
  365. lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
  366. /*
  367. * Traffic class, flow label
  368. * If flow label is 0, compress it. If traffic class is 0, compress it
  369. * We have to process both in the same time as the offset of traffic
  370. * class depends on the presence of version and flow label
  371. */
  372. /* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
  373. tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
  374. tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
  375. if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
  376. (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
  377. /* flow label can be compressed */
  378. iphc0 |= LOWPAN_IPHC_FL_C;
  379. if ((hdr->priority == 0) &&
  380. ((hdr->flow_lbl[0] & 0xF0) == 0)) {
  381. /* compress (elide) all */
  382. iphc0 |= LOWPAN_IPHC_TC_C;
  383. } else {
  384. /* compress only the flow label */
  385. *hc06_ptr = tmp;
  386. hc06_ptr += 1;
  387. }
  388. } else {
  389. /* Flow label cannot be compressed */
  390. if ((hdr->priority == 0) &&
  391. ((hdr->flow_lbl[0] & 0xF0) == 0)) {
  392. /* compress only traffic class */
  393. iphc0 |= LOWPAN_IPHC_TC_C;
  394. *hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
  395. memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
  396. hc06_ptr += 3;
  397. } else {
  398. /* compress nothing */
  399. memcpy(hc06_ptr, &hdr, 4);
  400. /* replace the top byte with new ECN | DSCP format */
  401. *hc06_ptr = tmp;
  402. hc06_ptr += 4;
  403. }
  404. }
  405. /* NOTE: payload length is always compressed */
  406. /* Next Header is compress if UDP */
  407. if (hdr->nexthdr == UIP_PROTO_UDP)
  408. iphc0 |= LOWPAN_IPHC_NH_C;
  409. if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
  410. *hc06_ptr = hdr->nexthdr;
  411. hc06_ptr += 1;
  412. }
  413. /*
  414. * Hop limit
  415. * if 1: compress, encoding is 01
  416. * if 64: compress, encoding is 10
  417. * if 255: compress, encoding is 11
  418. * else do not compress
  419. */
  420. switch (hdr->hop_limit) {
  421. case 1:
  422. iphc0 |= LOWPAN_IPHC_TTL_1;
  423. break;
  424. case 64:
  425. iphc0 |= LOWPAN_IPHC_TTL_64;
  426. break;
  427. case 255:
  428. iphc0 |= LOWPAN_IPHC_TTL_255;
  429. break;
  430. default:
  431. *hc06_ptr = hdr->hop_limit;
  432. break;
  433. }
  434. /* source address compression */
  435. if (is_addr_unspecified(&hdr->saddr)) {
  436. pr_debug("(%s): source address is unspecified, setting SAC\n",
  437. __func__);
  438. iphc1 |= LOWPAN_IPHC_SAC;
  439. /* TODO: context lookup */
  440. } else if (is_addr_link_local(&hdr->saddr)) {
  441. pr_debug("(%s): source address is link-local\n", __func__);
  442. iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
  443. LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
  444. } else {
  445. pr_debug("(%s): send the full source address\n", __func__);
  446. memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
  447. hc06_ptr += 16;
  448. }
  449. /* destination address compression */
  450. if (is_addr_mcast(&hdr->daddr)) {
  451. pr_debug("(%s): destination address is multicast", __func__);
  452. iphc1 |= LOWPAN_IPHC_M;
  453. if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
  454. pr_debug("compressed to 1 octet\n");
  455. iphc1 |= LOWPAN_IPHC_DAM_11;
  456. /* use last byte */
  457. *hc06_ptr = hdr->daddr.s6_addr[15];
  458. hc06_ptr += 1;
  459. } else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
  460. pr_debug("compressed to 4 octets\n");
  461. iphc1 |= LOWPAN_IPHC_DAM_10;
  462. /* second byte + the last three */
  463. *hc06_ptr = hdr->daddr.s6_addr[1];
  464. memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
  465. hc06_ptr += 4;
  466. } else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
  467. pr_debug("compressed to 6 octets\n");
  468. iphc1 |= LOWPAN_IPHC_DAM_01;
  469. /* second byte + the last five */
  470. *hc06_ptr = hdr->daddr.s6_addr[1];
  471. memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
  472. hc06_ptr += 6;
  473. } else {
  474. pr_debug("using full address\n");
  475. iphc1 |= LOWPAN_IPHC_DAM_00;
  476. memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
  477. hc06_ptr += 16;
  478. }
  479. } else {
  480. pr_debug("(%s): destination address is unicast: ", __func__);
  481. /* TODO: context lookup */
  482. if (is_addr_link_local(&hdr->daddr)) {
  483. pr_debug("destination address is link-local\n");
  484. iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
  485. LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
  486. } else {
  487. pr_debug("using full address\n");
  488. memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
  489. hc06_ptr += 16;
  490. }
  491. }
  492. /* UDP header compression */
  493. if (hdr->nexthdr == UIP_PROTO_UDP)
  494. lowpan_compress_udp_header(&hc06_ptr, skb);
  495. head[0] = iphc0;
  496. head[1] = iphc1;
  497. skb_pull(skb, sizeof(struct ipv6hdr));
  498. memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
  499. kfree(head);
  500. lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
  501. skb->len);
  502. /*
  503. * NOTE1: I'm still unsure about the fact that compression and WPAN
  504. * header are created here and not later in the xmit. So wait for
  505. * an opinion of net maintainers.
  506. */
  507. /*
  508. * NOTE2: to be absolutely correct, we must derive PANid information
  509. * from MAC subif of the 'dev' and 'real_dev' network devices, but
  510. * this isn't implemented in mainline yet, so currently we assign 0xff
  511. */
  512. {
  513. /* prepare wpan address data */
  514. sa.addr_type = IEEE802154_ADDR_LONG;
  515. sa.pan_id = 0xff;
  516. da.addr_type = IEEE802154_ADDR_LONG;
  517. da.pan_id = 0xff;
  518. memcpy(&(da.hwaddr), daddr, 8);
  519. memcpy(&(sa.hwaddr), saddr, 8);
  520. mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
  521. return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
  522. type, (void *)&da, (void *)&sa, skb->len);
  523. }
  524. }
  525. static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
  526. {
  527. struct sk_buff *new;
  528. struct lowpan_dev_record *entry;
  529. int stat = NET_RX_SUCCESS;
  530. new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
  531. GFP_ATOMIC);
  532. kfree_skb(skb);
  533. if (!new)
  534. return -ENOMEM;
  535. skb_push(new, sizeof(struct ipv6hdr));
  536. skb_reset_network_header(new);
  537. skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
  538. new->protocol = htons(ETH_P_IPV6);
  539. new->pkt_type = PACKET_HOST;
  540. rcu_read_lock();
  541. list_for_each_entry_rcu(entry, &lowpan_devices, list)
  542. if (lowpan_dev_info(entry->ldev)->real_dev == new->dev) {
  543. skb = skb_copy(new, GFP_ATOMIC);
  544. if (!skb) {
  545. stat = -ENOMEM;
  546. break;
  547. }
  548. skb->dev = entry->ldev;
  549. stat = netif_rx(skb);
  550. }
  551. rcu_read_unlock();
  552. kfree_skb(new);
  553. return stat;
  554. }
  555. static void lowpan_fragment_timer_expired(unsigned long entry_addr)
  556. {
  557. struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
  558. pr_debug("%s: timer expired for frame with tag %d\n", __func__,
  559. entry->tag);
  560. spin_lock(&flist_lock);
  561. list_del(&entry->list);
  562. spin_unlock(&flist_lock);
  563. dev_kfree_skb(entry->skb);
  564. kfree(entry);
  565. }
  566. static struct lowpan_fragment *
  567. lowpan_alloc_new_frame(struct sk_buff *skb, u8 iphc0, u8 len, u8 tag)
  568. {
  569. struct lowpan_fragment *frame;
  570. frame = kzalloc(sizeof(struct lowpan_fragment),
  571. GFP_ATOMIC);
  572. if (!frame)
  573. goto frame_err;
  574. INIT_LIST_HEAD(&frame->list);
  575. frame->length = (iphc0 & 7) | (len << 3);
  576. frame->tag = tag;
  577. /* allocate buffer for frame assembling */
  578. frame->skb = alloc_skb(frame->length +
  579. sizeof(struct ipv6hdr), GFP_ATOMIC);
  580. if (!frame->skb)
  581. goto skb_err;
  582. frame->skb->priority = skb->priority;
  583. frame->skb->dev = skb->dev;
  584. /* reserve headroom for uncompressed ipv6 header */
  585. skb_reserve(frame->skb, sizeof(struct ipv6hdr));
  586. skb_put(frame->skb, frame->length);
  587. init_timer(&frame->timer);
  588. /* time out is the same as for ipv6 - 60 sec */
  589. frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT;
  590. frame->timer.data = (unsigned long)frame;
  591. frame->timer.function = lowpan_fragment_timer_expired;
  592. add_timer(&frame->timer);
  593. list_add_tail(&frame->list, &lowpan_fragments);
  594. return frame;
  595. skb_err:
  596. kfree(frame);
  597. frame_err:
  598. return NULL;
  599. }
  600. static int
  601. lowpan_process_data(struct sk_buff *skb)
  602. {
  603. struct ipv6hdr hdr;
  604. u8 tmp, iphc0, iphc1, num_context = 0;
  605. u8 *_saddr, *_daddr;
  606. int err;
  607. lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
  608. skb->len);
  609. /* at least two bytes will be used for the encoding */
  610. if (skb->len < 2)
  611. goto drop;
  612. if (lowpan_fetch_skb_u8(skb, &iphc0))
  613. goto drop;
  614. /* fragments assembling */
  615. switch (iphc0 & LOWPAN_DISPATCH_MASK) {
  616. case LOWPAN_DISPATCH_FRAG1:
  617. case LOWPAN_DISPATCH_FRAGN:
  618. {
  619. struct lowpan_fragment *frame;
  620. u8 len, offset;
  621. u16 tag;
  622. bool found = false;
  623. if (lowpan_fetch_skb_u8(skb, &len) || /* frame length */
  624. lowpan_fetch_skb_u16(skb, &tag)) /* fragment tag */
  625. goto drop;
  626. /*
  627. * check if frame assembling with the same tag is
  628. * already in progress
  629. */
  630. spin_lock(&flist_lock);
  631. list_for_each_entry(frame, &lowpan_fragments, list)
  632. if (frame->tag == tag) {
  633. found = true;
  634. break;
  635. }
  636. /* alloc new frame structure */
  637. if (!found) {
  638. frame = lowpan_alloc_new_frame(skb, iphc0, len, tag);
  639. if (!frame)
  640. goto unlock_and_drop;
  641. }
  642. if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1)
  643. goto unlock_and_drop;
  644. if (lowpan_fetch_skb_u8(skb, &offset)) /* fetch offset */
  645. goto unlock_and_drop;
  646. /* if payload fits buffer, copy it */
  647. if (likely((offset * 8 + skb->len) <= frame->length))
  648. skb_copy_to_linear_data_offset(frame->skb, offset * 8,
  649. skb->data, skb->len);
  650. else
  651. goto unlock_and_drop;
  652. frame->bytes_rcv += skb->len;
  653. /* frame assembling complete */
  654. if ((frame->bytes_rcv == frame->length) &&
  655. frame->timer.expires > jiffies) {
  656. /* if timer haven't expired - first of all delete it */
  657. del_timer(&frame->timer);
  658. list_del(&frame->list);
  659. spin_unlock(&flist_lock);
  660. dev_kfree_skb(skb);
  661. skb = frame->skb;
  662. kfree(frame);
  663. if (lowpan_fetch_skb_u8(skb, &iphc0))
  664. goto unlock_and_drop;
  665. break;
  666. }
  667. spin_unlock(&flist_lock);
  668. return kfree_skb(skb), 0;
  669. }
  670. default:
  671. break;
  672. }
  673. if (lowpan_fetch_skb_u8(skb, &iphc1))
  674. goto drop;
  675. _saddr = mac_cb(skb)->sa.hwaddr;
  676. _daddr = mac_cb(skb)->da.hwaddr;
  677. pr_debug("(%s): iphc0 = %02x, iphc1 = %02x\n", __func__, iphc0, iphc1);
  678. /* another if the CID flag is set */
  679. if (iphc1 & LOWPAN_IPHC_CID) {
  680. pr_debug("(%s): CID flag is set, increase header with one\n",
  681. __func__);
  682. if (lowpan_fetch_skb_u8(skb, &num_context))
  683. goto drop;
  684. }
  685. hdr.version = 6;
  686. /* Traffic Class and Flow Label */
  687. switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
  688. /*
  689. * Traffic Class and FLow Label carried in-line
  690. * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
  691. */
  692. case 0: /* 00b */
  693. if (lowpan_fetch_skb_u8(skb, &tmp))
  694. goto drop;
  695. memcpy(&hdr.flow_lbl, &skb->data[0], 3);
  696. skb_pull(skb, 3);
  697. hdr.priority = ((tmp >> 2) & 0x0f);
  698. hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
  699. (hdr.flow_lbl[0] & 0x0f);
  700. break;
  701. /*
  702. * Traffic class carried in-line
  703. * ECN + DSCP (1 byte), Flow Label is elided
  704. */
  705. case 1: /* 10b */
  706. if (lowpan_fetch_skb_u8(skb, &tmp))
  707. goto drop;
  708. hdr.priority = ((tmp >> 2) & 0x0f);
  709. hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
  710. hdr.flow_lbl[1] = 0;
  711. hdr.flow_lbl[2] = 0;
  712. break;
  713. /*
  714. * Flow Label carried in-line
  715. * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
  716. */
  717. case 2: /* 01b */
  718. if (lowpan_fetch_skb_u8(skb, &tmp))
  719. goto drop;
  720. hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
  721. memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
  722. skb_pull(skb, 2);
  723. break;
  724. /* Traffic Class and Flow Label are elided */
  725. case 3: /* 11b */
  726. hdr.priority = 0;
  727. hdr.flow_lbl[0] = 0;
  728. hdr.flow_lbl[1] = 0;
  729. hdr.flow_lbl[2] = 0;
  730. break;
  731. default:
  732. break;
  733. }
  734. /* Next Header */
  735. if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
  736. /* Next header is carried inline */
  737. if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
  738. goto drop;
  739. pr_debug("(%s): NH flag is set, next header is carried "
  740. "inline: %02x\n", __func__, hdr.nexthdr);
  741. }
  742. /* Hop Limit */
  743. if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
  744. hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
  745. else {
  746. if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
  747. goto drop;
  748. }
  749. /* Extract SAM to the tmp variable */
  750. tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
  751. /* Source address uncompression */
  752. pr_debug("(%s): source address stateless compression\n", __func__);
  753. err = lowpan_uncompress_addr(skb, &hdr.saddr, lowpan_llprefix,
  754. lowpan_unc_llconf[tmp], skb->data);
  755. if (err)
  756. goto drop;
  757. /* Extract DAM to the tmp variable */
  758. tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
  759. /* check for Multicast Compression */
  760. if (iphc1 & LOWPAN_IPHC_M) {
  761. if (iphc1 & LOWPAN_IPHC_DAC) {
  762. pr_debug("(%s): destination address context-based "
  763. "multicast compression\n", __func__);
  764. /* TODO: implement this */
  765. } else {
  766. u8 prefix[] = {0xff, 0x02};
  767. pr_debug("(%s): destination address non-context-based"
  768. " multicast compression\n", __func__);
  769. if (0 < tmp && tmp < 3) {
  770. if (lowpan_fetch_skb_u8(skb, &prefix[1]))
  771. goto drop;
  772. }
  773. err = lowpan_uncompress_addr(skb, &hdr.daddr, prefix,
  774. lowpan_unc_mxconf[tmp], NULL);
  775. if (err)
  776. goto drop;
  777. }
  778. } else {
  779. pr_debug("(%s): destination address stateless compression\n",
  780. __func__);
  781. err = lowpan_uncompress_addr(skb, &hdr.daddr, lowpan_llprefix,
  782. lowpan_unc_llconf[tmp], skb->data);
  783. if (err)
  784. goto drop;
  785. }
  786. /* UDP data uncompression */
  787. if (iphc0 & LOWPAN_IPHC_NH_C)
  788. if (lowpan_uncompress_udp_header(skb))
  789. goto drop;
  790. /* Not fragmented package */
  791. hdr.payload_len = htons(skb->len);
  792. pr_debug("(%s): skb headroom size = %d, data length = %d\n", __func__,
  793. skb_headroom(skb), skb->len);
  794. pr_debug("(%s): IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t"
  795. "nexthdr = 0x%02x\n\thop_lim = %d\n", __func__, hdr.version,
  796. ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
  797. lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
  798. sizeof(hdr));
  799. return lowpan_skb_deliver(skb, &hdr);
  800. unlock_and_drop:
  801. spin_unlock(&flist_lock);
  802. drop:
  803. kfree_skb(skb);
  804. return -EINVAL;
  805. }
  806. static int lowpan_get_mac_header_length(struct sk_buff *skb)
  807. {
  808. /*
  809. * Currently long addressing mode is supported only, so the overall
  810. * header size is 21:
  811. * FC SeqNum DPAN DA SA Sec
  812. * 2 + 1 + 2 + 8 + 8 + 0 = 21
  813. */
  814. return 21;
  815. }
  816. static int
  817. lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
  818. int mlen, int plen, int offset)
  819. {
  820. struct sk_buff *frag;
  821. int hlen, ret;
  822. /* if payload length is zero, therefore it's a first fragment */
  823. hlen = (plen == 0 ? LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE);
  824. lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
  825. frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE);
  826. if (!frag)
  827. return -ENOMEM;
  828. frag->priority = skb->priority;
  829. frag->dev = skb->dev;
  830. /* copy header, MFR and payload */
  831. memcpy(skb_put(frag, mlen), skb->data, mlen);
  832. memcpy(skb_put(frag, hlen), head, hlen);
  833. if (plen)
  834. skb_copy_from_linear_data_offset(skb, offset + mlen,
  835. skb_put(frag, plen), plen);
  836. lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
  837. frag->len);
  838. ret = dev_queue_xmit(frag);
  839. return ret;
  840. }
  841. static int
  842. lowpan_skb_fragmentation(struct sk_buff *skb)
  843. {
  844. int err, header_length, payload_length, tag, offset = 0;
  845. u8 head[5];
  846. header_length = lowpan_get_mac_header_length(skb);
  847. payload_length = skb->len - header_length;
  848. tag = fragment_tag++;
  849. /* first fragment header */
  850. head[0] = LOWPAN_DISPATCH_FRAG1 | (payload_length & 0x7);
  851. head[1] = (payload_length >> 3) & 0xff;
  852. head[2] = tag & 0xff;
  853. head[3] = tag >> 8;
  854. err = lowpan_fragment_xmit(skb, head, header_length, 0, 0);
  855. /* next fragment header */
  856. head[0] &= ~LOWPAN_DISPATCH_FRAG1;
  857. head[0] |= LOWPAN_DISPATCH_FRAGN;
  858. while ((payload_length - offset > 0) && (err >= 0)) {
  859. int len = LOWPAN_FRAG_SIZE;
  860. head[4] = offset / 8;
  861. if (payload_length - offset < len)
  862. len = payload_length - offset;
  863. err = lowpan_fragment_xmit(skb, head, header_length,
  864. len, offset);
  865. offset += len;
  866. }
  867. return err;
  868. }
  869. static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
  870. {
  871. int err = -1;
  872. pr_debug("(%s): package xmit\n", __func__);
  873. skb->dev = lowpan_dev_info(dev)->real_dev;
  874. if (skb->dev == NULL) {
  875. pr_debug("(%s) ERROR: no real wpan device found\n", __func__);
  876. goto error;
  877. }
  878. if (skb->len <= IEEE802154_MTU) {
  879. err = dev_queue_xmit(skb);
  880. goto out;
  881. }
  882. pr_debug("(%s): frame is too big, fragmentation is needed\n",
  883. __func__);
  884. err = lowpan_skb_fragmentation(skb);
  885. error:
  886. dev_kfree_skb(skb);
  887. out:
  888. if (err < 0)
  889. pr_debug("(%s): ERROR: xmit failed\n", __func__);
  890. return (err < 0 ? NETDEV_TX_BUSY : NETDEV_TX_OK);
  891. }
  892. static void lowpan_dev_free(struct net_device *dev)
  893. {
  894. dev_put(lowpan_dev_info(dev)->real_dev);
  895. free_netdev(dev);
  896. }
  897. static struct wpan_phy *lowpan_get_phy(const struct net_device *dev)
  898. {
  899. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  900. return ieee802154_mlme_ops(real_dev)->get_phy(real_dev);
  901. }
  902. static u16 lowpan_get_pan_id(const struct net_device *dev)
  903. {
  904. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  905. return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
  906. }
  907. static u16 lowpan_get_short_addr(const struct net_device *dev)
  908. {
  909. struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
  910. return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
  911. }
  912. static struct header_ops lowpan_header_ops = {
  913. .create = lowpan_header_create,
  914. };
  915. static const struct net_device_ops lowpan_netdev_ops = {
  916. .ndo_start_xmit = lowpan_xmit,
  917. .ndo_set_mac_address = eth_mac_addr,
  918. };
  919. static struct ieee802154_mlme_ops lowpan_mlme = {
  920. .get_pan_id = lowpan_get_pan_id,
  921. .get_phy = lowpan_get_phy,
  922. .get_short_addr = lowpan_get_short_addr,
  923. };
  924. static void lowpan_setup(struct net_device *dev)
  925. {
  926. pr_debug("(%s)\n", __func__);
  927. dev->addr_len = IEEE802154_ADDR_LEN;
  928. memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
  929. dev->type = ARPHRD_IEEE802154;
  930. /* Frame Control + Sequence Number + Address fields + Security Header */
  931. dev->hard_header_len = 2 + 1 + 20 + 14;
  932. dev->needed_tailroom = 2; /* FCS */
  933. dev->mtu = 1281;
  934. dev->tx_queue_len = 0;
  935. dev->flags = IFF_BROADCAST | IFF_MULTICAST;
  936. dev->watchdog_timeo = 0;
  937. dev->netdev_ops = &lowpan_netdev_ops;
  938. dev->header_ops = &lowpan_header_ops;
  939. dev->ml_priv = &lowpan_mlme;
  940. dev->destructor = lowpan_dev_free;
  941. }
  942. static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
  943. {
  944. pr_debug("(%s)\n", __func__);
  945. if (tb[IFLA_ADDRESS]) {
  946. if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
  947. return -EINVAL;
  948. }
  949. return 0;
  950. }
  951. static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  952. struct packet_type *pt, struct net_device *orig_dev)
  953. {
  954. if (!netif_running(dev))
  955. goto drop;
  956. if (dev->type != ARPHRD_IEEE802154)
  957. goto drop;
  958. /* check that it's our buffer */
  959. switch (skb->data[0] & 0xe0) {
  960. case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
  961. case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
  962. case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
  963. lowpan_process_data(skb);
  964. break;
  965. default:
  966. break;
  967. }
  968. return NET_RX_SUCCESS;
  969. drop:
  970. kfree_skb(skb);
  971. return NET_RX_DROP;
  972. }
  973. static int lowpan_newlink(struct net *src_net, struct net_device *dev,
  974. struct nlattr *tb[], struct nlattr *data[])
  975. {
  976. struct net_device *real_dev;
  977. struct lowpan_dev_record *entry;
  978. pr_debug("(%s)\n", __func__);
  979. if (!tb[IFLA_LINK])
  980. return -EINVAL;
  981. /* find and hold real wpan device */
  982. real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
  983. if (!real_dev)
  984. return -ENODEV;
  985. lowpan_dev_info(dev)->real_dev = real_dev;
  986. mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
  987. entry = kzalloc(sizeof(struct lowpan_dev_record), GFP_KERNEL);
  988. if (!entry) {
  989. dev_put(real_dev);
  990. lowpan_dev_info(dev)->real_dev = NULL;
  991. return -ENOMEM;
  992. }
  993. entry->ldev = dev;
  994. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  995. INIT_LIST_HEAD(&entry->list);
  996. list_add_tail(&entry->list, &lowpan_devices);
  997. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  998. spin_lock_init(&flist_lock);
  999. register_netdevice(dev);
  1000. return 0;
  1001. }
  1002. static void lowpan_dellink(struct net_device *dev, struct list_head *head)
  1003. {
  1004. struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
  1005. struct net_device *real_dev = lowpan_dev->real_dev;
  1006. struct lowpan_dev_record *entry, *tmp;
  1007. struct lowpan_fragment *frame, *tframe;
  1008. ASSERT_RTNL();
  1009. spin_lock(&flist_lock);
  1010. list_for_each_entry_safe(frame, tframe, &lowpan_fragments, list) {
  1011. del_timer(&frame->timer);
  1012. list_del(&frame->list);
  1013. dev_kfree_skb(frame->skb);
  1014. kfree(frame);
  1015. }
  1016. spin_unlock(&flist_lock);
  1017. mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
  1018. list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
  1019. if (entry->ldev == dev) {
  1020. list_del(&entry->list);
  1021. kfree(entry);
  1022. }
  1023. }
  1024. mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
  1025. mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
  1026. unregister_netdevice_queue(dev, head);
  1027. dev_put(real_dev);
  1028. }
  1029. static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
  1030. .kind = "lowpan",
  1031. .priv_size = sizeof(struct lowpan_dev_info),
  1032. .setup = lowpan_setup,
  1033. .newlink = lowpan_newlink,
  1034. .dellink = lowpan_dellink,
  1035. .validate = lowpan_validate,
  1036. };
  1037. static inline int __init lowpan_netlink_init(void)
  1038. {
  1039. return rtnl_link_register(&lowpan_link_ops);
  1040. }
  1041. static inline void __init lowpan_netlink_fini(void)
  1042. {
  1043. rtnl_link_unregister(&lowpan_link_ops);
  1044. }
  1045. static struct packet_type lowpan_packet_type = {
  1046. .type = __constant_htons(ETH_P_IEEE802154),
  1047. .func = lowpan_rcv,
  1048. };
  1049. static int __init lowpan_init_module(void)
  1050. {
  1051. int err = 0;
  1052. pr_debug("(%s)\n", __func__);
  1053. err = lowpan_netlink_init();
  1054. if (err < 0)
  1055. goto out;
  1056. dev_add_pack(&lowpan_packet_type);
  1057. out:
  1058. return err;
  1059. }
  1060. static void __exit lowpan_cleanup_module(void)
  1061. {
  1062. pr_debug("(%s)\n", __func__);
  1063. lowpan_netlink_fini();
  1064. dev_remove_pack(&lowpan_packet_type);
  1065. }
  1066. module_init(lowpan_init_module);
  1067. module_exit(lowpan_cleanup_module);
  1068. MODULE_LICENSE("GPL");
  1069. MODULE_ALIAS_RTNL_LINK("lowpan");