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         DECLARE_FLEX_ARRAY(struct ipv6_rpl_sr_hdr, srh);
  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
 194         if (skb->protocol != htons(ETH_P_IPV6))
 195                 return -EINVAL;
 196
 197         tinfo = rpl_encap_lwtunnel(dst->lwtstate);
 198
 199         return rpl_do_srh_inline(skb, rlwt, tinfo->srh);
 200 }
 201
 202 static int rpl_output(struct net *net, struct sock *sk, struct sk_buff *skb)
 203 {
 204         struct dst_entry *orig_dst = skb_dst(skb);
 205         struct dst_entry *dst = NULL;
 206         struct rpl_lwt *rlwt;
 207         int err;
 208
 209         rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
 210
 211         err = rpl_do_srh(skb, rlwt);
 212         if (unlikely(err))
 213                 goto drop;
 214
 215         local_bh_disable();
 216         dst = dst_cache_get(&rlwt->cache);
 217         local_bh_enable();
 218
 219         if (unlikely(!dst)) {
 220                 struct ipv6hdr *hdr = ipv6_hdr(skb);
 221                 struct flowi6 fl6;
 222
 223                 memset(&fl6, 0, sizeof(fl6));
 224                 fl6.daddr = hdr->daddr;
 225                 fl6.saddr = hdr->saddr;
 226                 fl6.flowlabel = ip6_flowinfo(hdr);
 227                 fl6.flowi6_mark = skb->mark;
 228                 fl6.flowi6_proto = hdr->nexthdr;
 229
 230                 dst = ip6_route_output(net, NULL, &fl6);
 231                 if (dst->error) {
 232                         err = dst->error;
 233                         dst_release(dst);
 234                         goto drop;
 235                 }
 236
 237                 local_bh_disable();
 238                 dst_cache_set_ip6(&rlwt->cache, dst, &fl6.saddr);
 239                 local_bh_enable();
 240         }
 241
 242         skb_dst_drop(skb);
 243         skb_dst_set(skb, dst);
 244
 245         err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
 246         if (unlikely(err))
 247                 goto drop;
 248
 249         return dst_output(net, sk, skb);
 250
 251 drop:
 252         kfree_skb(skb);
 253         return err;
 254 }
 255
 256 static int rpl_input(struct sk_buff *skb)
 257 {
 258         struct dst_entry *orig_dst = skb_dst(skb);
 259         struct dst_entry *dst = NULL;
 260         struct rpl_lwt *rlwt;
 261         int err;
 262
 263         rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate);
 264
 265         err = rpl_do_srh(skb, rlwt);
 266         if (unlikely(err))
 267                 goto drop;
 268
 269         local_bh_disable();
 270         dst = dst_cache_get(&rlwt->cache);
 271
 272         if (!dst) {
 273                 ip6_route_input(skb);
 274                 dst = skb_dst(skb);
 275                 if (!dst->error) {
 276                         dst_cache_set_ip6(&rlwt->cache, dst,
 277                                           &ipv6_hdr(skb)->saddr);
 278                 }
 279         } else {
 280                 skb_dst_drop(skb);
 281                 skb_dst_set(skb, dst);
 282         }
 283         local_bh_enable();
 284
 285         err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev));
 286         if (unlikely(err))
 287                 goto drop;
 288
 289         return dst_input(skb);
 290
 291 drop:
 292         kfree_skb(skb);
 293         return err;
 294 }
 295
 296 static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype,
 297                            struct rpl_iptunnel_encap *tuninfo)
 298 {
 299         struct rpl_iptunnel_encap *data;
 300         struct nlattr *nla;
 301         int len;
 302
 303         len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh);
 304
 305         nla = nla_reserve(skb, attrtype, len);
 306         if (!nla)
 307                 return -EMSGSIZE;
 308
 309         data = nla_data(nla);
 310         memcpy(data, tuninfo->srh, len);
 311
 312         return 0;
 313 }
 314
 315 static int rpl_fill_encap_info(struct sk_buff *skb,
 316                                struct lwtunnel_state *lwtstate)
 317 {
 318         struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
 319
 320         if (nla_put_rpl_srh(skb, RPL_IPTUNNEL_SRH, tuninfo))
 321                 return -EMSGSIZE;
 322
 323         return 0;
 324 }
 325
 326 static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate)
 327 {
 328         struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate);
 329
 330         return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh));
 331 }
 332
 333 static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b)
 334 {
 335         struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(a);
 336         struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(b);
 337         int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh);
 338
 339         if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh))
 340                 return 1;
 341
 342         return memcmp(a_hdr, b_hdr, len);
 343 }
 344
 345 static const struct lwtunnel_encap_ops rpl_ops = {
 346         .build_state    = rpl_build_state,
 347         .destroy_state  = rpl_destroy_state,
 348         .output         = rpl_output,
 349         .input          = rpl_input,
 350         .fill_encap     = rpl_fill_encap_info,
 351         .get_encap_size = rpl_encap_nlsize,
 352         .cmp_encap      = rpl_encap_cmp,
 353         .owner          = THIS_MODULE,
 354 };
 355
 356 int __init rpl_init(void)
 357 {
 358         int err;
 359
 360         err = lwtunnel_encap_add_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
 361         if (err)
 362                 goto out;
 363
 364         pr_info("RPL Segment Routing with IPv6\n");
 365
 366         return 0;
 367
 368 out:
 369         return err;
 370 }
 371
 372 void rpl_exit(void)
 373 {
 374         lwtunnel_encap_del_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL);
 375 }