net/ipv6/rpl_iptunnel.c

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