rpl_iptunnel.c 8.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /**
  3. * Authors:
  4. * (C) 2020 Alexander Aring <alex.aring@gmail.com>
  5. */
  6. #include <linux/rpl_iptunnel.h>
  7. #include <net/dst_cache.h>
  8. #include <net/ip6_route.h>
  9. #include <net/lwtunnel.h>
  10. #include <net/ipv6.h>
  11. #include <net/rpl.h>
  12. struct rpl_iptunnel_encap {
  13. struct ipv6_rpl_sr_hdr srh[0];
  14. };
  15. struct rpl_lwt {
  16. struct dst_cache cache;
  17. struct rpl_iptunnel_encap tuninfo;
  18. };
  19. static inline struct rpl_lwt *rpl_lwt_lwtunnel(struct lwtunnel_state *lwt)
  20. {
  21. return (struct rpl_lwt *)lwt->data;
  22. }
  23. static inline struct rpl_iptunnel_encap *
  24. rpl_encap_lwtunnel(struct lwtunnel_state *lwt)
  25. {
  26. return &rpl_lwt_lwtunnel(lwt)->tuninfo;
  27. }
  28. static const struct nla_policy rpl_iptunnel_policy[RPL_IPTUNNEL_MAX + 1] = {
  29. [RPL_IPTUNNEL_SRH] = { .type = NLA_BINARY },
  30. };
  31. static bool rpl_validate_srh(struct net *net, struct ipv6_rpl_sr_hdr *srh,
  32. size_t seglen)
  33. {
  34. int err;
  35. if ((srh->hdrlen << 3) != seglen)
  36. return false;
  37. /* check at least one segment and seglen fit with segments_left */
  38. if (!srh->segments_left ||
  39. (srh->segments_left * sizeof(struct in6_addr)) != seglen)
  40. return false;
  41. if (srh->cmpri || srh->cmpre)
  42. return false;
  43. err = ipv6_chk_rpl_srh_loop(net, srh->rpl_segaddr,
  44. srh->segments_left);
  45. if (err)
  46. return false;
  47. if (ipv6_addr_type(&srh->rpl_segaddr[srh->segments_left - 1]) &
  48. IPV6_ADDR_MULTICAST)
  49. return false;
  50. return true;
  51. }
  52. static int rpl_build_state(struct net *net, struct nlattr *nla,
  53. unsigned int family, const void *cfg,
  54. struct lwtunnel_state **ts,
  55. struct netlink_ext_ack *extack)
  56. {
  57. struct nlattr *tb[RPL_IPTUNNEL_MAX + 1];
  58. struct lwtunnel_state *newts;
  59. struct ipv6_rpl_sr_hdr *srh;
  60. struct rpl_lwt *rlwt;
  61. int err, srh_len;
  62. if (family != AF_INET6)
  63. return -EINVAL;
  64. err = nla_parse_nested(tb, RPL_IPTUNNEL_MAX, nla,
  65. rpl_iptunnel_policy, extack);
  66. if (err < 0)
  67. return err;
  68. if (!tb[RPL_IPTUNNEL_SRH])
  69. return -EINVAL;
  70. srh = nla_data(tb[RPL_IPTUNNEL_SRH]);
  71. srh_len = nla_len(tb[RPL_IPTUNNEL_SRH]);
  72. if (srh_len < sizeof(*srh))
  73. return -EINVAL;
  74. /* verify that SRH is consistent */
  75. if (!rpl_validate_srh(net, srh, srh_len - sizeof(*srh)))
  76. return -EINVAL;
  77. newts = lwtunnel_state_alloc(srh_len + sizeof(*rlwt));
  78. if (!newts)
  79. return -ENOMEM;
  80. rlwt = rpl_lwt_lwtunnel(newts);
  81. err = dst_cache_init(&rlwt->cache, GFP_ATOMIC);
  82. if (err) {
  83. kfree(newts);
  84. return err;
  85. }
  86. memcpy(&rlwt->tuninfo.srh, srh, srh_len);
  87. newts->type = LWTUNNEL_ENCAP_RPL;
  88. newts->flags |= LWTUNNEL_STATE_INPUT_REDIRECT;
  89. newts->flags |= LWTUNNEL_STATE_OUTPUT_REDIRECT;
  90. *ts = newts;
  91. return 0;
  92. }
  93. static void rpl_destroy_state(struct lwtunnel_state *lwt)
  94. {
  95. dst_cache_destroy(&rpl_lwt_lwtunnel(lwt)->cache);
  96. }
  97. static int rpl_do_srh_inline(struct sk_buff *skb, const struct rpl_lwt *rlwt,
  98. const struct ipv6_rpl_sr_hdr *srh)
  99. {
  100. struct ipv6_rpl_sr_hdr *isrh, *csrh;
  101. const struct ipv6hdr *oldhdr;
  102. struct ipv6hdr *hdr;
  103. unsigned char *buf;
  104. size_t hdrlen;
  105. int err;
  106. oldhdr = ipv6_hdr(skb);
  107. buf = kcalloc(struct_size(srh, segments.addr, srh->segments_left), 2, GFP_ATOMIC);
  108. if (!buf)
  109. return -ENOMEM;
  110. isrh = (struct ipv6_rpl_sr_hdr *)buf;
  111. csrh = (struct ipv6_rpl_sr_hdr *)(buf + ((srh->hdrlen + 1) << 3));
  112. memcpy(isrh, srh, sizeof(*isrh));
  113. memcpy(isrh->rpl_segaddr, &srh->rpl_segaddr[1],
  114. (srh->segments_left - 1) * 16);
  115. isrh->rpl_segaddr[srh->segments_left - 1] = oldhdr->daddr;
  116. ipv6_rpl_srh_compress(csrh, isrh, &srh->rpl_segaddr[0],
  117. isrh->segments_left - 1);
  118. hdrlen = ((csrh->hdrlen + 1) << 3);
  119. err = skb_cow_head(skb, hdrlen + skb->mac_len);
  120. if (unlikely(err)) {
  121. kfree(buf);
  122. return err;
  123. }
  124. skb_pull(skb, sizeof(struct ipv6hdr));
  125. skb_postpull_rcsum(skb, skb_network_header(skb),
  126. sizeof(struct ipv6hdr));
  127. skb_push(skb, sizeof(struct ipv6hdr) + hdrlen);
  128. skb_reset_network_header(skb);
  129. skb_mac_header_rebuild(skb);
  130. hdr = ipv6_hdr(skb);
  131. memmove(hdr, oldhdr, sizeof(*hdr));
  132. isrh = (void *)hdr + sizeof(*hdr);
  133. memcpy(isrh, csrh, hdrlen);
  134. isrh->nexthdr = hdr->nexthdr;
  135. hdr->nexthdr = NEXTHDR_ROUTING;
  136. hdr->daddr = srh->rpl_segaddr[0];
  137. ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
  138. skb_set_transport_header(skb, sizeof(struct ipv6hdr));
  139. skb_postpush_rcsum(skb, hdr, sizeof(struct ipv6hdr) + hdrlen);
  140. kfree(buf);
  141. return 0;
  142. }
  143. static int rpl_do_srh(struct sk_buff *skb, const struct rpl_lwt *rlwt)
  144. {
  145. struct dst_entry *dst = skb_dst(skb);
  146. struct rpl_iptunnel_encap *tinfo;
  147. int err = 0;
  148. if (skb->protocol != htons(ETH_P_IPV6))
  149. return -EINVAL;
  150. tinfo = rpl_encap_lwtunnel(dst->lwtstate);
  151. err = rpl_do_srh_inline(skb, rlwt, tinfo->srh);
  152. if (err)
  153. return err;
  154. return 0;
  155. }
  156. static int rpl_output(struct net *net, struct sock *sk, struct sk_buff *skb)
  157. {
  158. struct dst_entry *orig_dst = skb_dst(skb);
  159. struct dst_entry *dst = NULL;
  160. struct rpl_lwt *rlwt;
  161. int err;
  162. rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
  163. err = rpl_do_srh(skb, rlwt);
  164. if (unlikely(err))
  165. goto drop;
  166. preempt_disable();
  167. dst = dst_cache_get(&rlwt->cache);
  168. preempt_enable();
  169. if (unlikely(!dst)) {
  170. struct ipv6hdr *hdr = ipv6_hdr(skb);
  171. struct flowi6 fl6;
  172. memset(&fl6, 0, sizeof(fl6));
  173. fl6.daddr = hdr->daddr;
  174. fl6.saddr = hdr->saddr;
  175. fl6.flowlabel = ip6_flowinfo(hdr);
  176. fl6.flowi6_mark = skb->mark;
  177. fl6.flowi6_proto = hdr->nexthdr;
  178. dst = ip6_route_output(net, NULL, &fl6);
  179. if (dst->error) {
  180. err = dst->error;
  181. dst_release(dst);
  182. goto drop;
  183. }
  184. preempt_disable();
  185. dst_cache_set_ip6(&rlwt->cache, dst, &fl6.saddr);
  186. preempt_enable();
  187. }
  188. skb_dst_drop(skb);
  189. skb_dst_set(skb, dst);
  190. err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
  191. if (unlikely(err))
  192. goto drop;
  193. return dst_output(net, sk, skb);
  194. drop:
  195. kfree_skb(skb);
  196. return err;
  197. }
  198. static int rpl_input(struct sk_buff *skb)
  199. {
  200. struct dst_entry *orig_dst = skb_dst(skb);
  201. struct dst_entry *dst = NULL;
  202. struct rpl_lwt *rlwt;
  203. int err;
  204. rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
  205. err = rpl_do_srh(skb, rlwt);
  206. if (unlikely(err)) {
  207. kfree_skb(skb);
  208. return err;
  209. }
  210. preempt_disable();
  211. dst = dst_cache_get(&rlwt->cache);
  212. preempt_enable();
  213. skb_dst_drop(skb);
  214. if (!dst) {
  215. ip6_route_input(skb);
  216. dst = skb_dst(skb);
  217. if (!dst->error) {
  218. preempt_disable();
  219. dst_cache_set_ip6(&rlwt->cache, dst,
  220. &ipv6_hdr(skb)->saddr);
  221. preempt_enable();
  222. }
  223. } else {
  224. skb_dst_set(skb, dst);
  225. }
  226. err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
  227. if (unlikely(err))
  228. return err;
  229. return dst_input(skb);
  230. }
  231. static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype,
  232. struct rpl_iptunnel_encap *tuninfo)
  233. {
  234. struct rpl_iptunnel_encap *data;
  235. struct nlattr *nla;
  236. int len;
  237. len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh);
  238. nla = nla_reserve(skb, attrtype, len);
  239. if (!nla)
  240. return -EMSGSIZE;
  241. data = nla_data(nla);
  242. memcpy(data, tuninfo->srh, len);
  243. return 0;
  244. }
  245. static int rpl_fill_encap_info(struct sk_buff *skb,
  246. struct lwtunnel_state *lwtstate)
  247. {
  248. struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
  249. if (nla_put_rpl_srh(skb, RPL_IPTUNNEL_SRH, tuninfo))
  250. return -EMSGSIZE;
  251. return 0;
  252. }
  253. static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate)
  254. {
  255. struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
  256. return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh));
  257. }
  258. static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b)
  259. {
  260. struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(a);
  261. struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(b);
  262. int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh);
  263. if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh))
  264. return 1;
  265. return memcmp(a_hdr, b_hdr, len);
  266. }
  267. static const struct lwtunnel_encap_ops rpl_ops = {
  268. .build_state = rpl_build_state,
  269. .destroy_state = rpl_destroy_state,
  270. .output = rpl_output,
  271. .input = rpl_input,
  272. .fill_encap = rpl_fill_encap_info,
  273. .get_encap_size = rpl_encap_nlsize,
  274. .cmp_encap = rpl_encap_cmp,
  275. .owner = THIS_MODULE,
  276. };
  277. int __init rpl_init(void)
  278. {
  279. int err;
  280. err = lwtunnel_encap_add_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
  281. if (err)
  282. goto out;
  283. pr_info("RPL Segment Routing with IPv6\n");
  284. return 0;
  285. out:
  286. return err;
  287. }
  288. void rpl_exit(void)
  289. {
  290. lwtunnel_encap_del_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
  291. }