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