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 } 376