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