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 = kzalloc(ipv6_rpl_srh_alloc_size(srh->segments_left - 1) * 2,
 140                       GFP_ATOMIC);
 141         if (!buf)
 142                 return -ENOMEM;
 143
 144         isrh = (struct ipv6_rpl_sr_hdr *)buf;
 145         csrh = (struct ipv6_rpl_sr_hdr *)(buf + ((srh->hdrlen + 1) << 3));
 146
 147         memcpy(isrh, srh, sizeof(*isrh));
 148         memcpy(isrh->rpl_segaddr, &srh->rpl_segaddr[1],
 149                (srh->segments_left - 1) * 16);
 150         isrh->rpl_segaddr[srh->segments_left - 1] = oldhdr->daddr;
 151
 152         ipv6_rpl_srh_compress(csrh, isrh, &srh->rpl_segaddr[0],
 153                               isrh->segments_left - 1);
 154
 155         hdrlen = ((csrh->hdrlen + 1) << 3);
 156
 157         err = skb_cow_head(skb, hdrlen + skb->mac_len);
 158         if (unlikely(err)) {
 159                 kfree(buf);
 160                 return err;
 161         }
 162
 163         skb_pull(skb, sizeof(struct ipv6hdr));
 164         skb_postpull_rcsum(skb, skb_network_header(skb),
 165                            sizeof(struct ipv6hdr));
 166
 167         skb_push(skb, sizeof(struct ipv6hdr) + hdrlen);
 168         skb_reset_network_header(skb);
 169         skb_mac_header_rebuild(skb);
 170
 171         hdr = ipv6_hdr(skb);
 172         memmove(hdr, oldhdr, sizeof(*hdr));
 173         isrh = (void *)hdr + sizeof(*hdr);
 174         memcpy(isrh, csrh, hdrlen);
 175
 176         isrh->nexthdr = hdr->nexthdr;
 177         hdr->nexthdr = NEXTHDR_ROUTING;
 178         hdr->daddr = srh->rpl_segaddr[0];
 179
 180         ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
 181         skb_set_transport_header(skb, sizeof(struct ipv6hdr));
 182
 183         skb_postpush_rcsum(skb, hdr, sizeof(struct ipv6hdr) + hdrlen);
 184
 185         kfree(buf);
 186
 187         return 0;
 188 }
 189
 190 static int rpl_do_srh(struct sk_buff *skb, const struct rpl_lwt *rlwt)
 191 {
 192         struct dst_entry *dst = skb_dst(skb);
 193         struct rpl_iptunnel_encap *tinfo;
 194         int err = 0;
 195
 196         if (skb->protocol != htons(ETH_P_IPV6))
 197                 return -EINVAL;
 198
 199         tinfo = rpl_encap_lwtunnel(dst->lwtstate);
 200
 201         err = rpl_do_srh_inline(skb, rlwt, tinfo->srh);
 202         if (err)
 203                 return err;
 204
 205         return 0;
 206 }
 207
 208 static int rpl_output(struct net *net, struct sock *sk, struct sk_buff *skb)
 209 {
 210         struct dst_entry *orig_dst = skb_dst(skb);
 211         struct dst_entry *dst = NULL;
 212         struct rpl_lwt *rlwt;
 213         int err;
 214
 215         rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
 216
 217         err = rpl_do_srh(skb, rlwt);
 218         if (unlikely(err))
 219                 goto drop;
 220
 221         preempt_disable();
 222         dst = dst_cache_get(&rlwt->cache);
 223         preempt_enable();
 224
 225         if (unlikely(!dst)) {
 226                 struct ipv6hdr *hdr = ipv6_hdr(skb);
 227                 struct flowi6 fl6;
 228
 229                 memset(&fl6, 0, sizeof(fl6));
 230                 fl6.daddr = hdr->daddr;
 231                 fl6.saddr = hdr->saddr;
 232                 fl6.flowlabel = ip6_flowinfo(hdr);
 233                 fl6.flowi6_mark = skb->mark;
 234                 fl6.flowi6_proto = hdr->nexthdr;
 235
 236                 dst = ip6_route_output(net, NULL, &fl6);
 237                 if (dst->error) {
 238                         err = dst->error;
 239                         dst_release(dst);
 240                         goto drop;
 241                 }
 242
 243                 preempt_disable();
 244                 dst_cache_set_ip6(&rlwt->cache, dst, &fl6.saddr);
 245                 preempt_enable();
 246         }
 247
 248         skb_dst_drop(skb);
 249         skb_dst_set(skb, dst);
 250
 251         err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
 252         if (unlikely(err))
 253                 goto drop;
 254
 255         return dst_output(net, sk, skb);
 256
 257 drop:
 258         kfree_skb(skb);
 259         return err;
 260 }
 261
 262 static int rpl_input(struct sk_buff *skb)
 263 {
 264         struct dst_entry *orig_dst = skb_dst(skb);
 265         struct dst_entry *dst = NULL;
 266         struct rpl_lwt *rlwt;
 267         int err;
 268
 269         rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
 270
 271         err = rpl_do_srh(skb, rlwt);
 272         if (unlikely(err)) {
 273                 kfree_skb(skb);
 274                 return err;
 275         }
 276
 277         preempt_disable();
 278         dst = dst_cache_get(&rlwt->cache);
 279         preempt_enable();
 280
 281         skb_dst_drop(skb);
 282
 283         if (!dst) {
 284                 ip6_route_input(skb);
 285                 dst = skb_dst(skb);
 286                 if (!dst->error) {
 287                         preempt_disable();
 288                         dst_cache_set_ip6(&rlwt->cache, dst,
 289                                           &ipv6_hdr(skb)->saddr);
 290                         preempt_enable();
 291                 }
 292         } else {
 293                 skb_dst_set(skb, dst);
 294         }
 295
 296         err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
 297         if (unlikely(err))
 298                 return err;
 299
 300         return dst_input(skb);
 301 }
 302
 303 static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype,
 304                            struct rpl_iptunnel_encap *tuninfo)
 305 {
 306         struct rpl_iptunnel_encap *data;
 307         struct nlattr *nla;
 308         int len;
 309
 310         len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh);
 311
 312         nla = nla_reserve(skb, attrtype, len);
 313         if (!nla)
 314                 return -EMSGSIZE;
 315
 316         data = nla_data(nla);
 317         memcpy(data, tuninfo->srh, len);
 318
 319         return 0;
 320 }
 321
 322 static int rpl_fill_encap_info(struct sk_buff *skb,
 323                                struct lwtunnel_state *lwtstate)
 324 {
 325         struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
 326
 327         if (nla_put_rpl_srh(skb, RPL_IPTUNNEL_SRH, tuninfo))
 328                 return -EMSGSIZE;
 329
 330         return 0;
 331 }
 332
 333 static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate)
 334 {
 335         struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
 336
 337         return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh));
 338 }
 339
 340 static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b)
 341 {
 342         struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(a);
 343         struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(b);
 344         int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh);
 345
 346         if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh))
 347                 return 1;
 348
 349         return memcmp(a_hdr, b_hdr, len);
 350 }
 351
 352 static const struct lwtunnel_encap_ops rpl_ops = {
 353         .build_state    = rpl_build_state,
 354         .destroy_state  = rpl_destroy_state,
 355         .output         = rpl_output,
 356         .input          = rpl_input,
 357         .fill_encap     = rpl_fill_encap_info,
 358         .get_encap_size = rpl_encap_nlsize,
 359         .cmp_encap      = rpl_encap_cmp,
 360         .owner          = THIS_MODULE,
 361 };
 362
 363 int __init rpl_init(void)
 364 {
 365         int err;
 366
 367         err = lwtunnel_encap_add_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
 368         if (err)
 369                 goto out;
 370
 371         pr_info("RPL Segment Routing with IPv6\n");
 372
 373         return 0;
 374
 375 out:
 376         return err;
 377 }
 378
 379 void rpl_exit(void)
 380 {
 381         lwtunnel_encap_del_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
 382 }