iphc.c 37 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313
  1. /*
  2. * Copyright 2011, Siemens AG
  3. * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  4. */
  5. /* Based on patches from Jon Smirl <jonsmirl@gmail.com>
  6. * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
  7. *
  8. * This program is free software; you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License version 2
  10. * as published by the Free Software Foundation.
  11. *
  12. * This program is distributed in the hope that it will be useful,
  13. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  15. * GNU General Public License for more details.
  16. *
  17. */
  18. /* Jon's code is based on 6lowpan implementation for Contiki which is:
  19. * Copyright (c) 2008, Swedish Institute of Computer Science.
  20. * All rights reserved.
  21. *
  22. * Redistribution and use in source and binary forms, with or without
  23. * modification, are permitted provided that the following conditions
  24. * are met:
  25. * 1. Redistributions of source code must retain the above copyright
  26. * notice, this list of conditions and the following disclaimer.
  27. * 2. Redistributions in binary form must reproduce the above copyright
  28. * notice, this list of conditions and the following disclaimer in the
  29. * documentation and/or other materials provided with the distribution.
  30. * 3. Neither the name of the Institute nor the names of its contributors
  31. * may be used to endorse or promote products derived from this software
  32. * without specific prior written permission.
  33. *
  34. * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
  35. * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  36. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  37. * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
  38. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  39. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  40. * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  41. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  42. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  43. * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  44. * SUCH DAMAGE.
  45. */
  46. #include <linux/bitops.h>
  47. #include <linux/if_arp.h>
  48. #include <linux/netdevice.h>
  49. #include <net/6lowpan.h>
  50. #include <net/ipv6.h>
  51. #include "6lowpan_i.h"
  52. #include "nhc.h"
  53. /* Values of fields within the IPHC encoding first byte */
  54. #define LOWPAN_IPHC_TF_MASK 0x18
  55. #define LOWPAN_IPHC_TF_00 0x00
  56. #define LOWPAN_IPHC_TF_01 0x08
  57. #define LOWPAN_IPHC_TF_10 0x10
  58. #define LOWPAN_IPHC_TF_11 0x18
  59. #define LOWPAN_IPHC_NH 0x04
  60. #define LOWPAN_IPHC_HLIM_MASK 0x03
  61. #define LOWPAN_IPHC_HLIM_00 0x00
  62. #define LOWPAN_IPHC_HLIM_01 0x01
  63. #define LOWPAN_IPHC_HLIM_10 0x02
  64. #define LOWPAN_IPHC_HLIM_11 0x03
  65. /* Values of fields within the IPHC encoding second byte */
  66. #define LOWPAN_IPHC_CID 0x80
  67. #define LOWPAN_IPHC_SAC 0x40
  68. #define LOWPAN_IPHC_SAM_MASK 0x30
  69. #define LOWPAN_IPHC_SAM_00 0x00
  70. #define LOWPAN_IPHC_SAM_01 0x10
  71. #define LOWPAN_IPHC_SAM_10 0x20
  72. #define LOWPAN_IPHC_SAM_11 0x30
  73. #define LOWPAN_IPHC_M 0x08
  74. #define LOWPAN_IPHC_DAC 0x04
  75. #define LOWPAN_IPHC_DAM_MASK 0x03
  76. #define LOWPAN_IPHC_DAM_00 0x00
  77. #define LOWPAN_IPHC_DAM_01 0x01
  78. #define LOWPAN_IPHC_DAM_10 0x02
  79. #define LOWPAN_IPHC_DAM_11 0x03
  80. /* ipv6 address based on mac
  81. * second bit-flip (Universe/Local) is done according RFC2464
  82. */
  83. #define is_addr_mac_addr_based(a, m) \
  84. ((((a)->s6_addr[8]) == (((m)[0]) ^ 0x02)) && \
  85. (((a)->s6_addr[9]) == (m)[1]) && \
  86. (((a)->s6_addr[10]) == (m)[2]) && \
  87. (((a)->s6_addr[11]) == (m)[3]) && \
  88. (((a)->s6_addr[12]) == (m)[4]) && \
  89. (((a)->s6_addr[13]) == (m)[5]) && \
  90. (((a)->s6_addr[14]) == (m)[6]) && \
  91. (((a)->s6_addr[15]) == (m)[7]))
  92. /* check whether we can compress the IID to 16 bits,
  93. * it's possible for unicast addresses with first 49 bits are zero only.
  94. */
  95. #define lowpan_is_iid_16_bit_compressable(a) \
  96. ((((a)->s6_addr16[4]) == 0) && \
  97. (((a)->s6_addr[10]) == 0) && \
  98. (((a)->s6_addr[11]) == 0xff) && \
  99. (((a)->s6_addr[12]) == 0xfe) && \
  100. (((a)->s6_addr[13]) == 0))
  101. /* check whether the 112-bit gid of the multicast address is mappable to: */
  102. /* 48 bits, FFXX::00XX:XXXX:XXXX */
  103. #define lowpan_is_mcast_addr_compressable48(a) \
  104. ((((a)->s6_addr16[1]) == 0) && \
  105. (((a)->s6_addr16[2]) == 0) && \
  106. (((a)->s6_addr16[3]) == 0) && \
  107. (((a)->s6_addr16[4]) == 0) && \
  108. (((a)->s6_addr[10]) == 0))
  109. /* 32 bits, FFXX::00XX:XXXX */
  110. #define lowpan_is_mcast_addr_compressable32(a) \
  111. ((((a)->s6_addr16[1]) == 0) && \
  112. (((a)->s6_addr16[2]) == 0) && \
  113. (((a)->s6_addr16[3]) == 0) && \
  114. (((a)->s6_addr16[4]) == 0) && \
  115. (((a)->s6_addr16[5]) == 0) && \
  116. (((a)->s6_addr[12]) == 0))
  117. /* 8 bits, FF02::00XX */
  118. #define lowpan_is_mcast_addr_compressable8(a) \
  119. ((((a)->s6_addr[1]) == 2) && \
  120. (((a)->s6_addr16[1]) == 0) && \
  121. (((a)->s6_addr16[2]) == 0) && \
  122. (((a)->s6_addr16[3]) == 0) && \
  123. (((a)->s6_addr16[4]) == 0) && \
  124. (((a)->s6_addr16[5]) == 0) && \
  125. (((a)->s6_addr16[6]) == 0) && \
  126. (((a)->s6_addr[14]) == 0))
  127. #define lowpan_is_linklocal_zero_padded(a) \
  128. (!(hdr->saddr.s6_addr[1] & 0x3f) && \
  129. !hdr->saddr.s6_addr16[1] && \
  130. !hdr->saddr.s6_addr32[1])
  131. #define LOWPAN_IPHC_CID_DCI(cid) (cid & 0x0f)
  132. #define LOWPAN_IPHC_CID_SCI(cid) ((cid & 0xf0) >> 4)
  133. static inline void
  134. lowpan_iphc_uncompress_802154_lladdr(struct in6_addr *ipaddr,
  135. const void *lladdr)
  136. {
  137. const struct ieee802154_addr *addr = lladdr;
  138. u8 eui64[EUI64_ADDR_LEN];
  139. switch (addr->mode) {
  140. case IEEE802154_ADDR_LONG:
  141. ieee802154_le64_to_be64(eui64, &addr->extended_addr);
  142. lowpan_iphc_uncompress_eui64_lladdr(ipaddr, eui64);
  143. break;
  144. case IEEE802154_ADDR_SHORT:
  145. /* fe:80::ff:fe00:XXXX
  146. * \__/
  147. * short_addr
  148. *
  149. * Universe/Local bit is zero.
  150. */
  151. ipaddr->s6_addr[0] = 0xFE;
  152. ipaddr->s6_addr[1] = 0x80;
  153. ipaddr->s6_addr[11] = 0xFF;
  154. ipaddr->s6_addr[12] = 0xFE;
  155. ieee802154_le16_to_be16(&ipaddr->s6_addr16[7],
  156. &addr->short_addr);
  157. break;
  158. default:
  159. /* should never handled and filtered by 802154 6lowpan */
  160. WARN_ON_ONCE(1);
  161. break;
  162. }
  163. }
  164. static struct lowpan_iphc_ctx *
  165. lowpan_iphc_ctx_get_by_id(const struct net_device *dev, u8 id)
  166. {
  167. struct lowpan_iphc_ctx *ret = &lowpan_dev(dev)->ctx.table[id];
  168. if (!lowpan_iphc_ctx_is_active(ret))
  169. return NULL;
  170. return ret;
  171. }
  172. static struct lowpan_iphc_ctx *
  173. lowpan_iphc_ctx_get_by_addr(const struct net_device *dev,
  174. const struct in6_addr *addr)
  175. {
  176. struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table;
  177. struct lowpan_iphc_ctx *ret = NULL;
  178. struct in6_addr addr_pfx;
  179. u8 addr_plen;
  180. int i;
  181. for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
  182. /* Check if context is valid. A context that is not valid
  183. * MUST NOT be used for compression.
  184. */
  185. if (!lowpan_iphc_ctx_is_active(&table[i]) ||
  186. !lowpan_iphc_ctx_is_compression(&table[i]))
  187. continue;
  188. ipv6_addr_prefix(&addr_pfx, addr, table[i].plen);
  189. /* if prefix len < 64, the remaining bits until 64th bit is
  190. * zero. Otherwise we use table[i]->plen.
  191. */
  192. if (table[i].plen < 64)
  193. addr_plen = 64;
  194. else
  195. addr_plen = table[i].plen;
  196. if (ipv6_prefix_equal(&addr_pfx, &table[i].pfx, addr_plen)) {
  197. /* remember first match */
  198. if (!ret) {
  199. ret = &table[i];
  200. continue;
  201. }
  202. /* get the context with longest prefix len */
  203. if (table[i].plen > ret->plen)
  204. ret = &table[i];
  205. }
  206. }
  207. return ret;
  208. }
  209. static struct lowpan_iphc_ctx *
  210. lowpan_iphc_ctx_get_by_mcast_addr(const struct net_device *dev,
  211. const struct in6_addr *addr)
  212. {
  213. struct lowpan_iphc_ctx *table = lowpan_dev(dev)->ctx.table;
  214. struct lowpan_iphc_ctx *ret = NULL;
  215. struct in6_addr addr_mcast, network_pfx = {};
  216. int i;
  217. /* init mcast address with */
  218. memcpy(&addr_mcast, addr, sizeof(*addr));
  219. for (i = 0; i < LOWPAN_IPHC_CTX_TABLE_SIZE; i++) {
  220. /* Check if context is valid. A context that is not valid
  221. * MUST NOT be used for compression.
  222. */
  223. if (!lowpan_iphc_ctx_is_active(&table[i]) ||
  224. !lowpan_iphc_ctx_is_compression(&table[i]))
  225. continue;
  226. /* setting plen */
  227. addr_mcast.s6_addr[3] = table[i].plen;
  228. /* get network prefix to copy into multicast address */
  229. ipv6_addr_prefix(&network_pfx, &table[i].pfx,
  230. table[i].plen);
  231. /* setting network prefix */
  232. memcpy(&addr_mcast.s6_addr[4], &network_pfx, 8);
  233. if (ipv6_addr_equal(addr, &addr_mcast)) {
  234. ret = &table[i];
  235. break;
  236. }
  237. }
  238. return ret;
  239. }
  240. static void lowpan_iphc_uncompress_lladdr(const struct net_device *dev,
  241. struct in6_addr *ipaddr,
  242. const void *lladdr)
  243. {
  244. switch (dev->addr_len) {
  245. case ETH_ALEN:
  246. lowpan_iphc_uncompress_eui48_lladdr(ipaddr, lladdr);
  247. break;
  248. case EUI64_ADDR_LEN:
  249. lowpan_iphc_uncompress_eui64_lladdr(ipaddr, lladdr);
  250. break;
  251. default:
  252. WARN_ON_ONCE(1);
  253. break;
  254. }
  255. }
  256. /* Uncompress address function for source and
  257. * destination address(non-multicast).
  258. *
  259. * address_mode is the masked value for sam or dam value
  260. */
  261. static int lowpan_iphc_uncompress_addr(struct sk_buff *skb,
  262. const struct net_device *dev,
  263. struct in6_addr *ipaddr,
  264. u8 address_mode, const void *lladdr)
  265. {
  266. bool fail;
  267. switch (address_mode) {
  268. /* SAM and DAM are the same here */
  269. case LOWPAN_IPHC_DAM_00:
  270. /* for global link addresses */
  271. fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
  272. break;
  273. case LOWPAN_IPHC_SAM_01:
  274. case LOWPAN_IPHC_DAM_01:
  275. /* fe:80::XXXX:XXXX:XXXX:XXXX */
  276. ipaddr->s6_addr[0] = 0xFE;
  277. ipaddr->s6_addr[1] = 0x80;
  278. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
  279. break;
  280. case LOWPAN_IPHC_SAM_10:
  281. case LOWPAN_IPHC_DAM_10:
  282. /* fe:80::ff:fe00:XXXX */
  283. ipaddr->s6_addr[0] = 0xFE;
  284. ipaddr->s6_addr[1] = 0x80;
  285. ipaddr->s6_addr[11] = 0xFF;
  286. ipaddr->s6_addr[12] = 0xFE;
  287. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
  288. break;
  289. case LOWPAN_IPHC_SAM_11:
  290. case LOWPAN_IPHC_DAM_11:
  291. fail = false;
  292. switch (lowpan_dev(dev)->lltype) {
  293. case LOWPAN_LLTYPE_IEEE802154:
  294. lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr);
  295. break;
  296. default:
  297. lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr);
  298. break;
  299. }
  300. break;
  301. default:
  302. pr_debug("Invalid address mode value: 0x%x\n", address_mode);
  303. return -EINVAL;
  304. }
  305. if (fail) {
  306. pr_debug("Failed to fetch skb data\n");
  307. return -EIO;
  308. }
  309. raw_dump_inline(NULL, "Reconstructed ipv6 addr is",
  310. ipaddr->s6_addr, 16);
  311. return 0;
  312. }
  313. /* Uncompress address function for source context
  314. * based address(non-multicast).
  315. */
  316. static int lowpan_iphc_uncompress_ctx_addr(struct sk_buff *skb,
  317. const struct net_device *dev,
  318. const struct lowpan_iphc_ctx *ctx,
  319. struct in6_addr *ipaddr,
  320. u8 address_mode, const void *lladdr)
  321. {
  322. bool fail;
  323. switch (address_mode) {
  324. /* SAM and DAM are the same here */
  325. case LOWPAN_IPHC_DAM_00:
  326. fail = false;
  327. /* SAM_00 -> unspec address ::
  328. * Do nothing, address is already ::
  329. *
  330. * DAM 00 -> reserved should never occur.
  331. */
  332. break;
  333. case LOWPAN_IPHC_SAM_01:
  334. case LOWPAN_IPHC_DAM_01:
  335. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
  336. ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
  337. break;
  338. case LOWPAN_IPHC_SAM_10:
  339. case LOWPAN_IPHC_DAM_10:
  340. ipaddr->s6_addr[11] = 0xFF;
  341. ipaddr->s6_addr[12] = 0xFE;
  342. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
  343. ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
  344. break;
  345. case LOWPAN_IPHC_SAM_11:
  346. case LOWPAN_IPHC_DAM_11:
  347. fail = false;
  348. switch (lowpan_dev(dev)->lltype) {
  349. case LOWPAN_LLTYPE_IEEE802154:
  350. lowpan_iphc_uncompress_802154_lladdr(ipaddr, lladdr);
  351. break;
  352. default:
  353. lowpan_iphc_uncompress_lladdr(dev, ipaddr, lladdr);
  354. break;
  355. }
  356. ipv6_addr_prefix_copy(ipaddr, &ctx->pfx, ctx->plen);
  357. break;
  358. default:
  359. pr_debug("Invalid sam value: 0x%x\n", address_mode);
  360. return -EINVAL;
  361. }
  362. if (fail) {
  363. pr_debug("Failed to fetch skb data\n");
  364. return -EIO;
  365. }
  366. raw_dump_inline(NULL,
  367. "Reconstructed context based ipv6 src addr is",
  368. ipaddr->s6_addr, 16);
  369. return 0;
  370. }
  371. /* Uncompress function for multicast destination address,
  372. * when M bit is set.
  373. */
  374. static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
  375. struct in6_addr *ipaddr,
  376. u8 address_mode)
  377. {
  378. bool fail;
  379. switch (address_mode) {
  380. case LOWPAN_IPHC_DAM_00:
  381. /* 00: 128 bits. The full address
  382. * is carried in-line.
  383. */
  384. fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
  385. break;
  386. case LOWPAN_IPHC_DAM_01:
  387. /* 01: 48 bits. The address takes
  388. * the form ffXX::00XX:XXXX:XXXX.
  389. */
  390. ipaddr->s6_addr[0] = 0xFF;
  391. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
  392. fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
  393. break;
  394. case LOWPAN_IPHC_DAM_10:
  395. /* 10: 32 bits. The address takes
  396. * the form ffXX::00XX:XXXX.
  397. */
  398. ipaddr->s6_addr[0] = 0xFF;
  399. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
  400. fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
  401. break;
  402. case LOWPAN_IPHC_DAM_11:
  403. /* 11: 8 bits. The address takes
  404. * the form ff02::00XX.
  405. */
  406. ipaddr->s6_addr[0] = 0xFF;
  407. ipaddr->s6_addr[1] = 0x02;
  408. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
  409. break;
  410. default:
  411. pr_debug("DAM value has a wrong value: 0x%x\n", address_mode);
  412. return -EINVAL;
  413. }
  414. if (fail) {
  415. pr_debug("Failed to fetch skb data\n");
  416. return -EIO;
  417. }
  418. raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is",
  419. ipaddr->s6_addr, 16);
  420. return 0;
  421. }
  422. static int lowpan_uncompress_multicast_ctx_daddr(struct sk_buff *skb,
  423. struct lowpan_iphc_ctx *ctx,
  424. struct in6_addr *ipaddr,
  425. u8 address_mode)
  426. {
  427. struct in6_addr network_pfx = {};
  428. bool fail;
  429. ipaddr->s6_addr[0] = 0xFF;
  430. fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 2);
  431. fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[12], 4);
  432. if (fail)
  433. return -EIO;
  434. /* take prefix_len and network prefix from the context */
  435. ipaddr->s6_addr[3] = ctx->plen;
  436. /* get network prefix to copy into multicast address */
  437. ipv6_addr_prefix(&network_pfx, &ctx->pfx, ctx->plen);
  438. /* setting network prefix */
  439. memcpy(&ipaddr->s6_addr[4], &network_pfx, 8);
  440. return 0;
  441. }
  442. /* get the ecn values from iphc tf format and set it to ipv6hdr */
  443. static inline void lowpan_iphc_tf_set_ecn(struct ipv6hdr *hdr, const u8 *tf)
  444. {
  445. /* get the two higher bits which is ecn */
  446. u8 ecn = tf[0] & 0xc0;
  447. /* ECN takes 0x30 in hdr->flow_lbl[0] */
  448. hdr->flow_lbl[0] |= (ecn >> 2);
  449. }
  450. /* get the dscp values from iphc tf format and set it to ipv6hdr */
  451. static inline void lowpan_iphc_tf_set_dscp(struct ipv6hdr *hdr, const u8 *tf)
  452. {
  453. /* DSCP is at place after ECN */
  454. u8 dscp = tf[0] & 0x3f;
  455. /* The four highest bits need to be set at hdr->priority */
  456. hdr->priority |= ((dscp & 0x3c) >> 2);
  457. /* The two lower bits is part of hdr->flow_lbl[0] */
  458. hdr->flow_lbl[0] |= ((dscp & 0x03) << 6);
  459. }
  460. /* get the flow label values from iphc tf format and set it to ipv6hdr */
  461. static inline void lowpan_iphc_tf_set_lbl(struct ipv6hdr *hdr, const u8 *lbl)
  462. {
  463. /* flow label is always some array started with lower nibble of
  464. * flow_lbl[0] and followed with two bytes afterwards. Inside inline
  465. * data the flow_lbl position can be different, which will be handled
  466. * by lbl pointer. E.g. case "01" vs "00" the traffic class is 8 bit
  467. * shifted, the different lbl pointer will handle that.
  468. *
  469. * The flow label will started at lower nibble of flow_lbl[0], the
  470. * higher nibbles are part of DSCP + ECN.
  471. */
  472. hdr->flow_lbl[0] |= lbl[0] & 0x0f;
  473. memcpy(&hdr->flow_lbl[1], &lbl[1], 2);
  474. }
  475. /* lowpan_iphc_tf_decompress - decompress the traffic class.
  476. * This function will return zero on success, a value lower than zero if
  477. * failed.
  478. */
  479. static int lowpan_iphc_tf_decompress(struct sk_buff *skb, struct ipv6hdr *hdr,
  480. u8 val)
  481. {
  482. u8 tf[4];
  483. /* Traffic Class and Flow Label */
  484. switch (val) {
  485. case LOWPAN_IPHC_TF_00:
  486. /* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */
  487. if (lowpan_fetch_skb(skb, tf, 4))
  488. return -EINVAL;
  489. /* 1 2 3
  490. * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
  491. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  492. * |ECN| DSCP | rsv | Flow Label |
  493. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  494. */
  495. lowpan_iphc_tf_set_ecn(hdr, tf);
  496. lowpan_iphc_tf_set_dscp(hdr, tf);
  497. lowpan_iphc_tf_set_lbl(hdr, &tf[1]);
  498. break;
  499. case LOWPAN_IPHC_TF_01:
  500. /* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided. */
  501. if (lowpan_fetch_skb(skb, tf, 3))
  502. return -EINVAL;
  503. /* 1 2
  504. * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
  505. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  506. * |ECN|rsv| Flow Label |
  507. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  508. */
  509. lowpan_iphc_tf_set_ecn(hdr, tf);
  510. lowpan_iphc_tf_set_lbl(hdr, &tf[0]);
  511. break;
  512. case LOWPAN_IPHC_TF_10:
  513. /* ECN + DSCP (1 byte), Flow Label is elided. */
  514. if (lowpan_fetch_skb(skb, tf, 1))
  515. return -EINVAL;
  516. /* 0 1 2 3 4 5 6 7
  517. * +-+-+-+-+-+-+-+-+
  518. * |ECN| DSCP |
  519. * +-+-+-+-+-+-+-+-+
  520. */
  521. lowpan_iphc_tf_set_ecn(hdr, tf);
  522. lowpan_iphc_tf_set_dscp(hdr, tf);
  523. break;
  524. case LOWPAN_IPHC_TF_11:
  525. /* Traffic Class and Flow Label are elided */
  526. break;
  527. default:
  528. WARN_ON_ONCE(1);
  529. return -EINVAL;
  530. }
  531. return 0;
  532. }
  533. /* TTL uncompression values */
  534. static const u8 lowpan_ttl_values[] = {
  535. [LOWPAN_IPHC_HLIM_01] = 1,
  536. [LOWPAN_IPHC_HLIM_10] = 64,
  537. [LOWPAN_IPHC_HLIM_11] = 255,
  538. };
  539. int lowpan_header_decompress(struct sk_buff *skb, const struct net_device *dev,
  540. const void *daddr, const void *saddr)
  541. {
  542. struct ipv6hdr hdr = {};
  543. struct lowpan_iphc_ctx *ci;
  544. u8 iphc0, iphc1, cid = 0;
  545. int err;
  546. raw_dump_table(__func__, "raw skb data dump uncompressed",
  547. skb->data, skb->len);
  548. if (lowpan_fetch_skb(skb, &iphc0, sizeof(iphc0)) ||
  549. lowpan_fetch_skb(skb, &iphc1, sizeof(iphc1)))
  550. return -EINVAL;
  551. hdr.version = 6;
  552. /* default CID = 0, another if the CID flag is set */
  553. if (iphc1 & LOWPAN_IPHC_CID) {
  554. if (lowpan_fetch_skb(skb, &cid, sizeof(cid)))
  555. return -EINVAL;
  556. }
  557. err = lowpan_iphc_tf_decompress(skb, &hdr,
  558. iphc0 & LOWPAN_IPHC_TF_MASK);
  559. if (err < 0)
  560. return err;
  561. /* Next Header */
  562. if (!(iphc0 & LOWPAN_IPHC_NH)) {
  563. /* Next header is carried inline */
  564. if (lowpan_fetch_skb(skb, &hdr.nexthdr, sizeof(hdr.nexthdr)))
  565. return -EINVAL;
  566. pr_debug("NH flag is set, next header carried inline: %02x\n",
  567. hdr.nexthdr);
  568. }
  569. /* Hop Limit */
  570. if ((iphc0 & LOWPAN_IPHC_HLIM_MASK) != LOWPAN_IPHC_HLIM_00) {
  571. hdr.hop_limit = lowpan_ttl_values[iphc0 & LOWPAN_IPHC_HLIM_MASK];
  572. } else {
  573. if (lowpan_fetch_skb(skb, &hdr.hop_limit,
  574. sizeof(hdr.hop_limit)))
  575. return -EINVAL;
  576. }
  577. if (iphc1 & LOWPAN_IPHC_SAC) {
  578. spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
  579. ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_SCI(cid));
  580. if (!ci) {
  581. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  582. return -EINVAL;
  583. }
  584. pr_debug("SAC bit is set. Handle context based source address.\n");
  585. err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.saddr,
  586. iphc1 & LOWPAN_IPHC_SAM_MASK,
  587. saddr);
  588. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  589. } else {
  590. /* Source address uncompression */
  591. pr_debug("source address stateless compression\n");
  592. err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.saddr,
  593. iphc1 & LOWPAN_IPHC_SAM_MASK,
  594. saddr);
  595. }
  596. /* Check on error of previous branch */
  597. if (err)
  598. return -EINVAL;
  599. switch (iphc1 & (LOWPAN_IPHC_M | LOWPAN_IPHC_DAC)) {
  600. case LOWPAN_IPHC_M | LOWPAN_IPHC_DAC:
  601. skb->pkt_type = PACKET_BROADCAST;
  602. spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
  603. ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
  604. if (!ci) {
  605. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  606. return -EINVAL;
  607. }
  608. /* multicast with context */
  609. pr_debug("dest: context-based mcast compression\n");
  610. err = lowpan_uncompress_multicast_ctx_daddr(skb, ci,
  611. &hdr.daddr,
  612. iphc1 & LOWPAN_IPHC_DAM_MASK);
  613. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  614. break;
  615. case LOWPAN_IPHC_M:
  616. skb->pkt_type = PACKET_BROADCAST;
  617. /* multicast */
  618. err = lowpan_uncompress_multicast_daddr(skb, &hdr.daddr,
  619. iphc1 & LOWPAN_IPHC_DAM_MASK);
  620. break;
  621. case LOWPAN_IPHC_DAC:
  622. skb->pkt_type = PACKET_HOST;
  623. spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
  624. ci = lowpan_iphc_ctx_get_by_id(dev, LOWPAN_IPHC_CID_DCI(cid));
  625. if (!ci) {
  626. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  627. return -EINVAL;
  628. }
  629. /* Destination address context based uncompression */
  630. pr_debug("DAC bit is set. Handle context based destination address.\n");
  631. err = lowpan_iphc_uncompress_ctx_addr(skb, dev, ci, &hdr.daddr,
  632. iphc1 & LOWPAN_IPHC_DAM_MASK,
  633. daddr);
  634. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  635. break;
  636. default:
  637. skb->pkt_type = PACKET_HOST;
  638. err = lowpan_iphc_uncompress_addr(skb, dev, &hdr.daddr,
  639. iphc1 & LOWPAN_IPHC_DAM_MASK,
  640. daddr);
  641. pr_debug("dest: stateless compression mode %d dest %pI6c\n",
  642. iphc1 & LOWPAN_IPHC_DAM_MASK, &hdr.daddr);
  643. break;
  644. }
  645. if (err)
  646. return -EINVAL;
  647. /* Next header data uncompression */
  648. if (iphc0 & LOWPAN_IPHC_NH) {
  649. err = lowpan_nhc_do_uncompression(skb, dev, &hdr);
  650. if (err < 0)
  651. return err;
  652. } else {
  653. err = skb_cow(skb, sizeof(hdr));
  654. if (unlikely(err))
  655. return err;
  656. }
  657. switch (lowpan_dev(dev)->lltype) {
  658. case LOWPAN_LLTYPE_IEEE802154:
  659. if (lowpan_802154_cb(skb)->d_size)
  660. hdr.payload_len = htons(lowpan_802154_cb(skb)->d_size -
  661. sizeof(struct ipv6hdr));
  662. else
  663. hdr.payload_len = htons(skb->len);
  664. break;
  665. default:
  666. hdr.payload_len = htons(skb->len);
  667. break;
  668. }
  669. pr_debug("skb headroom size = %d, data length = %d\n",
  670. skb_headroom(skb), skb->len);
  671. pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n\t"
  672. "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n",
  673. hdr.version, ntohs(hdr.payload_len), hdr.nexthdr,
  674. hdr.hop_limit, &hdr.daddr);
  675. skb_push(skb, sizeof(hdr));
  676. skb_reset_mac_header(skb);
  677. skb_reset_network_header(skb);
  678. skb_copy_to_linear_data(skb, &hdr, sizeof(hdr));
  679. raw_dump_table(__func__, "raw header dump", (u8 *)&hdr, sizeof(hdr));
  680. return 0;
  681. }
  682. EXPORT_SYMBOL_GPL(lowpan_header_decompress);
  683. static const u8 lowpan_iphc_dam_to_sam_value[] = {
  684. [LOWPAN_IPHC_DAM_00] = LOWPAN_IPHC_SAM_00,
  685. [LOWPAN_IPHC_DAM_01] = LOWPAN_IPHC_SAM_01,
  686. [LOWPAN_IPHC_DAM_10] = LOWPAN_IPHC_SAM_10,
  687. [LOWPAN_IPHC_DAM_11] = LOWPAN_IPHC_SAM_11,
  688. };
  689. static inline bool
  690. lowpan_iphc_compress_ctx_802154_lladdr(const struct in6_addr *ipaddr,
  691. const struct lowpan_iphc_ctx *ctx,
  692. const void *lladdr)
  693. {
  694. const struct ieee802154_addr *addr = lladdr;
  695. unsigned char extended_addr[EUI64_ADDR_LEN];
  696. bool lladdr_compress = false;
  697. struct in6_addr tmp = {};
  698. switch (addr->mode) {
  699. case IEEE802154_ADDR_LONG:
  700. ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
  701. /* check for SAM/DAM = 11 */
  702. memcpy(&tmp.s6_addr[8], &extended_addr, EUI64_ADDR_LEN);
  703. /* second bit-flip (Universe/Local) is done according RFC2464 */
  704. tmp.s6_addr[8] ^= 0x02;
  705. /* context information are always used */
  706. ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
  707. if (ipv6_addr_equal(&tmp, ipaddr))
  708. lladdr_compress = true;
  709. break;
  710. case IEEE802154_ADDR_SHORT:
  711. tmp.s6_addr[11] = 0xFF;
  712. tmp.s6_addr[12] = 0xFE;
  713. ieee802154_le16_to_be16(&tmp.s6_addr16[7],
  714. &addr->short_addr);
  715. /* context information are always used */
  716. ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
  717. if (ipv6_addr_equal(&tmp, ipaddr))
  718. lladdr_compress = true;
  719. break;
  720. default:
  721. /* should never handled and filtered by 802154 6lowpan */
  722. WARN_ON_ONCE(1);
  723. break;
  724. }
  725. return lladdr_compress;
  726. }
  727. static bool lowpan_iphc_addr_equal(const struct net_device *dev,
  728. const struct lowpan_iphc_ctx *ctx,
  729. const struct in6_addr *ipaddr,
  730. const void *lladdr)
  731. {
  732. struct in6_addr tmp = {};
  733. lowpan_iphc_uncompress_lladdr(dev, &tmp, lladdr);
  734. if (ctx)
  735. ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
  736. return ipv6_addr_equal(&tmp, ipaddr);
  737. }
  738. static u8 lowpan_compress_ctx_addr(u8 **hc_ptr, const struct net_device *dev,
  739. const struct in6_addr *ipaddr,
  740. const struct lowpan_iphc_ctx *ctx,
  741. const unsigned char *lladdr, bool sam)
  742. {
  743. struct in6_addr tmp = {};
  744. u8 dam;
  745. switch (lowpan_dev(dev)->lltype) {
  746. case LOWPAN_LLTYPE_IEEE802154:
  747. if (lowpan_iphc_compress_ctx_802154_lladdr(ipaddr, ctx,
  748. lladdr)) {
  749. dam = LOWPAN_IPHC_DAM_11;
  750. goto out;
  751. }
  752. break;
  753. default:
  754. if (lowpan_iphc_addr_equal(dev, ctx, ipaddr, lladdr)) {
  755. dam = LOWPAN_IPHC_DAM_11;
  756. goto out;
  757. }
  758. break;
  759. }
  760. memset(&tmp, 0, sizeof(tmp));
  761. /* check for SAM/DAM = 10 */
  762. tmp.s6_addr[11] = 0xFF;
  763. tmp.s6_addr[12] = 0xFE;
  764. memcpy(&tmp.s6_addr[14], &ipaddr->s6_addr[14], 2);
  765. /* context information are always used */
  766. ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
  767. if (ipv6_addr_equal(&tmp, ipaddr)) {
  768. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[14], 2);
  769. dam = LOWPAN_IPHC_DAM_10;
  770. goto out;
  771. }
  772. memset(&tmp, 0, sizeof(tmp));
  773. /* check for SAM/DAM = 01, should always match */
  774. memcpy(&tmp.s6_addr[8], &ipaddr->s6_addr[8], 8);
  775. /* context information are always used */
  776. ipv6_addr_prefix_copy(&tmp, &ctx->pfx, ctx->plen);
  777. if (ipv6_addr_equal(&tmp, ipaddr)) {
  778. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[8], 8);
  779. dam = LOWPAN_IPHC_DAM_01;
  780. goto out;
  781. }
  782. WARN_ONCE(1, "context found but no address mode matched\n");
  783. return LOWPAN_IPHC_DAM_00;
  784. out:
  785. if (sam)
  786. return lowpan_iphc_dam_to_sam_value[dam];
  787. else
  788. return dam;
  789. }
  790. static inline bool
  791. lowpan_iphc_compress_802154_lladdr(const struct in6_addr *ipaddr,
  792. const void *lladdr)
  793. {
  794. const struct ieee802154_addr *addr = lladdr;
  795. unsigned char extended_addr[EUI64_ADDR_LEN];
  796. bool lladdr_compress = false;
  797. struct in6_addr tmp = {};
  798. switch (addr->mode) {
  799. case IEEE802154_ADDR_LONG:
  800. ieee802154_le64_to_be64(&extended_addr, &addr->extended_addr);
  801. if (is_addr_mac_addr_based(ipaddr, extended_addr))
  802. lladdr_compress = true;
  803. break;
  804. case IEEE802154_ADDR_SHORT:
  805. /* fe:80::ff:fe00:XXXX
  806. * \__/
  807. * short_addr
  808. *
  809. * Universe/Local bit is zero.
  810. */
  811. tmp.s6_addr[0] = 0xFE;
  812. tmp.s6_addr[1] = 0x80;
  813. tmp.s6_addr[11] = 0xFF;
  814. tmp.s6_addr[12] = 0xFE;
  815. ieee802154_le16_to_be16(&tmp.s6_addr16[7],
  816. &addr->short_addr);
  817. if (ipv6_addr_equal(&tmp, ipaddr))
  818. lladdr_compress = true;
  819. break;
  820. default:
  821. /* should never handled and filtered by 802154 6lowpan */
  822. WARN_ON_ONCE(1);
  823. break;
  824. }
  825. return lladdr_compress;
  826. }
  827. static u8 lowpan_compress_addr_64(u8 **hc_ptr, const struct net_device *dev,
  828. const struct in6_addr *ipaddr,
  829. const unsigned char *lladdr, bool sam)
  830. {
  831. u8 dam = LOWPAN_IPHC_DAM_01;
  832. switch (lowpan_dev(dev)->lltype) {
  833. case LOWPAN_LLTYPE_IEEE802154:
  834. if (lowpan_iphc_compress_802154_lladdr(ipaddr, lladdr)) {
  835. dam = LOWPAN_IPHC_DAM_11; /* 0-bits */
  836. pr_debug("address compression 0 bits\n");
  837. goto out;
  838. }
  839. break;
  840. default:
  841. if (lowpan_iphc_addr_equal(dev, NULL, ipaddr, lladdr)) {
  842. dam = LOWPAN_IPHC_DAM_11;
  843. pr_debug("address compression 0 bits\n");
  844. goto out;
  845. }
  846. break;
  847. }
  848. if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
  849. /* compress IID to 16 bits xxxx::XXXX */
  850. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[7], 2);
  851. dam = LOWPAN_IPHC_DAM_10; /* 16-bits */
  852. raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
  853. *hc_ptr - 2, 2);
  854. goto out;
  855. }
  856. /* do not compress IID => xxxx::IID */
  857. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr16[4], 8);
  858. raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
  859. *hc_ptr - 8, 8);
  860. out:
  861. if (sam)
  862. return lowpan_iphc_dam_to_sam_value[dam];
  863. else
  864. return dam;
  865. }
  866. /* lowpan_iphc_get_tc - get the ECN + DCSP fields in hc format */
  867. static inline u8 lowpan_iphc_get_tc(const struct ipv6hdr *hdr)
  868. {
  869. u8 dscp, ecn;
  870. /* hdr->priority contains the higher bits of dscp, lower are part of
  871. * flow_lbl[0]. Note ECN, DCSP is swapped in ipv6 hdr.
  872. */
  873. dscp = (hdr->priority << 2) | ((hdr->flow_lbl[0] & 0xc0) >> 6);
  874. /* ECN is at the two lower bits from first nibble of flow_lbl[0] */
  875. ecn = (hdr->flow_lbl[0] & 0x30);
  876. /* for pretty debug output, also shift ecn to get the ecn value */
  877. pr_debug("ecn 0x%02x dscp 0x%02x\n", ecn >> 4, dscp);
  878. /* ECN is at 0x30 now, shift it to have ECN + DCSP */
  879. return (ecn << 2) | dscp;
  880. }
  881. /* lowpan_iphc_is_flow_lbl_zero - check if flow label is zero */
  882. static inline bool lowpan_iphc_is_flow_lbl_zero(const struct ipv6hdr *hdr)
  883. {
  884. return ((!(hdr->flow_lbl[0] & 0x0f)) &&
  885. !hdr->flow_lbl[1] && !hdr->flow_lbl[2]);
  886. }
  887. /* lowpan_iphc_tf_compress - compress the traffic class which is set by
  888. * ipv6hdr. Return the corresponding format identifier which is used.
  889. */
  890. static u8 lowpan_iphc_tf_compress(u8 **hc_ptr, const struct ipv6hdr *hdr)
  891. {
  892. /* get ecn dscp data in a byteformat as: ECN(hi) + DSCP(lo) */
  893. u8 tc = lowpan_iphc_get_tc(hdr), tf[4], val;
  894. /* printout the traffic class in hc format */
  895. pr_debug("tc 0x%02x\n", tc);
  896. if (lowpan_iphc_is_flow_lbl_zero(hdr)) {
  897. if (!tc) {
  898. /* 11: Traffic Class and Flow Label are elided. */
  899. val = LOWPAN_IPHC_TF_11;
  900. } else {
  901. /* 10: ECN + DSCP (1 byte), Flow Label is elided.
  902. *
  903. * 0 1 2 3 4 5 6 7
  904. * +-+-+-+-+-+-+-+-+
  905. * |ECN| DSCP |
  906. * +-+-+-+-+-+-+-+-+
  907. */
  908. lowpan_push_hc_data(hc_ptr, &tc, sizeof(tc));
  909. val = LOWPAN_IPHC_TF_10;
  910. }
  911. } else {
  912. /* check if dscp is zero, it's after the first two bit */
  913. if (!(tc & 0x3f)) {
  914. /* 01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
  915. *
  916. * 1 2
  917. * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
  918. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  919. * |ECN|rsv| Flow Label |
  920. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  921. */
  922. memcpy(&tf[0], &hdr->flow_lbl[0], 3);
  923. /* zero the highest 4-bits, contains DCSP + ECN */
  924. tf[0] &= ~0xf0;
  925. /* set ECN */
  926. tf[0] |= (tc & 0xc0);
  927. lowpan_push_hc_data(hc_ptr, tf, 3);
  928. val = LOWPAN_IPHC_TF_01;
  929. } else {
  930. /* 00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
  931. *
  932. * 1 2 3
  933. * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
  934. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  935. * |ECN| DSCP | rsv | Flow Label |
  936. * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  937. */
  938. memcpy(&tf[0], &tc, sizeof(tc));
  939. /* highest nibble of flow_lbl[0] is part of DSCP + ECN
  940. * which will be the 4-bit pad and will be filled with
  941. * zeros afterwards.
  942. */
  943. memcpy(&tf[1], &hdr->flow_lbl[0], 3);
  944. /* zero the 4-bit pad, which is reserved */
  945. tf[1] &= ~0xf0;
  946. lowpan_push_hc_data(hc_ptr, tf, 4);
  947. val = LOWPAN_IPHC_TF_00;
  948. }
  949. }
  950. return val;
  951. }
  952. static u8 lowpan_iphc_mcast_ctx_addr_compress(u8 **hc_ptr,
  953. const struct lowpan_iphc_ctx *ctx,
  954. const struct in6_addr *ipaddr)
  955. {
  956. u8 data[6];
  957. /* flags/scope, reserved (RIID) */
  958. memcpy(data, &ipaddr->s6_addr[1], 2);
  959. /* group ID */
  960. memcpy(&data[1], &ipaddr->s6_addr[11], 4);
  961. lowpan_push_hc_data(hc_ptr, data, 6);
  962. return LOWPAN_IPHC_DAM_00;
  963. }
  964. static u8 lowpan_iphc_mcast_addr_compress(u8 **hc_ptr,
  965. const struct in6_addr *ipaddr)
  966. {
  967. u8 val;
  968. if (lowpan_is_mcast_addr_compressable8(ipaddr)) {
  969. pr_debug("compressed to 1 octet\n");
  970. /* use last byte */
  971. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[15], 1);
  972. val = LOWPAN_IPHC_DAM_11;
  973. } else if (lowpan_is_mcast_addr_compressable32(ipaddr)) {
  974. pr_debug("compressed to 4 octets\n");
  975. /* second byte + the last three */
  976. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
  977. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[13], 3);
  978. val = LOWPAN_IPHC_DAM_10;
  979. } else if (lowpan_is_mcast_addr_compressable48(ipaddr)) {
  980. pr_debug("compressed to 6 octets\n");
  981. /* second byte + the last five */
  982. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[1], 1);
  983. lowpan_push_hc_data(hc_ptr, &ipaddr->s6_addr[11], 5);
  984. val = LOWPAN_IPHC_DAM_01;
  985. } else {
  986. pr_debug("using full address\n");
  987. lowpan_push_hc_data(hc_ptr, ipaddr->s6_addr, 16);
  988. val = LOWPAN_IPHC_DAM_00;
  989. }
  990. return val;
  991. }
  992. int lowpan_header_compress(struct sk_buff *skb, const struct net_device *dev,
  993. const void *daddr, const void *saddr)
  994. {
  995. u8 iphc0, iphc1, *hc_ptr, cid = 0;
  996. struct ipv6hdr *hdr;
  997. u8 head[LOWPAN_IPHC_MAX_HC_BUF_LEN] = {};
  998. struct lowpan_iphc_ctx *dci, *sci, dci_entry, sci_entry;
  999. int ret, ipv6_daddr_type, ipv6_saddr_type;
  1000. if (skb->protocol != htons(ETH_P_IPV6))
  1001. return -EINVAL;
  1002. hdr = ipv6_hdr(skb);
  1003. hc_ptr = head + 2;
  1004. pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength = %d\n"
  1005. "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest = %pI6c\n",
  1006. hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
  1007. hdr->hop_limit, &hdr->daddr);
  1008. raw_dump_table(__func__, "raw skb network header dump",
  1009. skb_network_header(skb), sizeof(struct ipv6hdr));
  1010. /* As we copy some bit-length fields, in the IPHC encoding bytes,
  1011. * we sometimes use |=
  1012. * If the field is 0, and the current bit value in memory is 1,
  1013. * this does not work. We therefore reset the IPHC encoding here
  1014. */
  1015. iphc0 = LOWPAN_DISPATCH_IPHC;
  1016. iphc1 = 0;
  1017. raw_dump_table(__func__, "sending raw skb network uncompressed packet",
  1018. skb->data, skb->len);
  1019. ipv6_daddr_type = ipv6_addr_type(&hdr->daddr);
  1020. spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
  1021. if (ipv6_daddr_type & IPV6_ADDR_MULTICAST)
  1022. dci = lowpan_iphc_ctx_get_by_mcast_addr(dev, &hdr->daddr);
  1023. else
  1024. dci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->daddr);
  1025. if (dci) {
  1026. memcpy(&dci_entry, dci, sizeof(*dci));
  1027. cid |= dci->id;
  1028. }
  1029. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  1030. spin_lock_bh(&lowpan_dev(dev)->ctx.lock);
  1031. sci = lowpan_iphc_ctx_get_by_addr(dev, &hdr->saddr);
  1032. if (sci) {
  1033. memcpy(&sci_entry, sci, sizeof(*sci));
  1034. cid |= (sci->id << 4);
  1035. }
  1036. spin_unlock_bh(&lowpan_dev(dev)->ctx.lock);
  1037. /* if cid is zero it will be compressed */
  1038. if (cid) {
  1039. iphc1 |= LOWPAN_IPHC_CID;
  1040. lowpan_push_hc_data(&hc_ptr, &cid, sizeof(cid));
  1041. }
  1042. /* Traffic Class, Flow Label compression */
  1043. iphc0 |= lowpan_iphc_tf_compress(&hc_ptr, hdr);
  1044. /* NOTE: payload length is always compressed */
  1045. /* Check if we provide the nhc format for nexthdr and compression
  1046. * functionality. If not nexthdr is handled inline and not compressed.
  1047. */
  1048. ret = lowpan_nhc_check_compression(skb, hdr, &hc_ptr);
  1049. if (ret == -ENOENT)
  1050. lowpan_push_hc_data(&hc_ptr, &hdr->nexthdr,
  1051. sizeof(hdr->nexthdr));
  1052. else
  1053. iphc0 |= LOWPAN_IPHC_NH;
  1054. /* Hop limit
  1055. * if 1: compress, encoding is 01
  1056. * if 64: compress, encoding is 10
  1057. * if 255: compress, encoding is 11
  1058. * else do not compress
  1059. */
  1060. switch (hdr->hop_limit) {
  1061. case 1:
  1062. iphc0 |= LOWPAN_IPHC_HLIM_01;
  1063. break;
  1064. case 64:
  1065. iphc0 |= LOWPAN_IPHC_HLIM_10;
  1066. break;
  1067. case 255:
  1068. iphc0 |= LOWPAN_IPHC_HLIM_11;
  1069. break;
  1070. default:
  1071. lowpan_push_hc_data(&hc_ptr, &hdr->hop_limit,
  1072. sizeof(hdr->hop_limit));
  1073. }
  1074. ipv6_saddr_type = ipv6_addr_type(&hdr->saddr);
  1075. /* source address compression */
  1076. if (ipv6_saddr_type == IPV6_ADDR_ANY) {
  1077. pr_debug("source address is unspecified, setting SAC\n");
  1078. iphc1 |= LOWPAN_IPHC_SAC;
  1079. } else {
  1080. if (sci) {
  1081. iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev,
  1082. &hdr->saddr,
  1083. &sci_entry, saddr,
  1084. true);
  1085. iphc1 |= LOWPAN_IPHC_SAC;
  1086. } else {
  1087. if (ipv6_saddr_type & IPV6_ADDR_LINKLOCAL &&
  1088. lowpan_is_linklocal_zero_padded(hdr->saddr)) {
  1089. iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev,
  1090. &hdr->saddr,
  1091. saddr, true);
  1092. pr_debug("source address unicast link-local %pI6c iphc1 0x%02x\n",
  1093. &hdr->saddr, iphc1);
  1094. } else {
  1095. pr_debug("send the full source address\n");
  1096. lowpan_push_hc_data(&hc_ptr,
  1097. hdr->saddr.s6_addr, 16);
  1098. }
  1099. }
  1100. }
  1101. /* destination address compression */
  1102. if (ipv6_daddr_type & IPV6_ADDR_MULTICAST) {
  1103. pr_debug("destination address is multicast: ");
  1104. iphc1 |= LOWPAN_IPHC_M;
  1105. if (dci) {
  1106. iphc1 |= lowpan_iphc_mcast_ctx_addr_compress(&hc_ptr,
  1107. &dci_entry,
  1108. &hdr->daddr);
  1109. iphc1 |= LOWPAN_IPHC_DAC;
  1110. } else {
  1111. iphc1 |= lowpan_iphc_mcast_addr_compress(&hc_ptr,
  1112. &hdr->daddr);
  1113. }
  1114. } else {
  1115. if (dci) {
  1116. iphc1 |= lowpan_compress_ctx_addr(&hc_ptr, dev,
  1117. &hdr->daddr,
  1118. &dci_entry, daddr,
  1119. false);
  1120. iphc1 |= LOWPAN_IPHC_DAC;
  1121. } else {
  1122. if (ipv6_daddr_type & IPV6_ADDR_LINKLOCAL &&
  1123. lowpan_is_linklocal_zero_padded(hdr->daddr)) {
  1124. iphc1 |= lowpan_compress_addr_64(&hc_ptr, dev,
  1125. &hdr->daddr,
  1126. daddr, false);
  1127. pr_debug("dest address unicast link-local %pI6c iphc1 0x%02x\n",
  1128. &hdr->daddr, iphc1);
  1129. } else {
  1130. pr_debug("dest address unicast %pI6c\n",
  1131. &hdr->daddr);
  1132. lowpan_push_hc_data(&hc_ptr,
  1133. hdr->daddr.s6_addr, 16);
  1134. }
  1135. }
  1136. }
  1137. /* next header compression */
  1138. if (iphc0 & LOWPAN_IPHC_NH) {
  1139. ret = lowpan_nhc_do_compression(skb, hdr, &hc_ptr);
  1140. if (ret < 0)
  1141. return ret;
  1142. }
  1143. head[0] = iphc0;
  1144. head[1] = iphc1;
  1145. skb_pull(skb, sizeof(struct ipv6hdr));
  1146. skb_reset_transport_header(skb);
  1147. memcpy(skb_push(skb, hc_ptr - head), head, hc_ptr - head);
  1148. skb_reset_network_header(skb);
  1149. pr_debug("header len %d skb %u\n", (int)(hc_ptr - head), skb->len);
  1150. raw_dump_table(__func__, "raw skb data dump compressed",
  1151. skb->data, skb->len);
  1152. return 0;
  1153. }
  1154. EXPORT_SYMBOL_GPL(lowpan_header_compress);