1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * IPv6 IOAM Lightweight Tunnel implementation 4 * 5 * Author: 6 * Justin Iurman <justin.iurman@uliege.be> 7 */ 8 9 #include <linux/kernel.h> 10 #include <linux/skbuff.h> 11 #include <linux/net.h> 12 #include <linux/in6.h> 13 #include <linux/ioam6.h> 14 #include <linux/ioam6_iptunnel.h> 15 #include <net/dst.h> 16 #include <net/sock.h> 17 #include <net/lwtunnel.h> 18 #include <net/ioam6.h> 19 #include <net/netlink.h> 20 #include <net/ipv6.h> 21 #include <net/dst_cache.h> 22 #include <net/ip6_route.h> 23 #include <net/addrconf.h> 24 25 #define IOAM6_MASK_SHORT_FIELDS 0xff100000 26 #define IOAM6_MASK_WIDE_FIELDS 0xe00000 27 28 struct ioam6_lwt_encap { 29 struct ipv6_hopopt_hdr eh; 30 u8 pad[2]; /* 2-octet padding for 4n-alignment */ 31 struct ioam6_hdr ioamh; 32 struct ioam6_trace_hdr traceh; 33 } __packed; 34 35 struct ioam6_lwt_freq { 36 u32 k; 37 u32 n; 38 }; 39 40 struct ioam6_lwt { 41 struct dst_cache cache; 42 struct ioam6_lwt_freq freq; 43 atomic_t pkt_cnt; 44 u8 mode; 45 bool has_tunsrc; 46 struct in6_addr tunsrc; 47 struct in6_addr tundst; 48 struct ioam6_lwt_encap tuninfo; 49 }; 50 51 static const struct netlink_range_validation freq_range = { 52 .min = IOAM6_IPTUNNEL_FREQ_MIN, 53 .max = IOAM6_IPTUNNEL_FREQ_MAX, 54 }; 55 56 static struct ioam6_lwt *ioam6_lwt_state(struct lwtunnel_state *lwt) 57 { 58 return (struct ioam6_lwt *)lwt->data; 59 } 60 61 static struct ioam6_lwt_encap *ioam6_lwt_info(struct lwtunnel_state *lwt) 62 { 63 return &ioam6_lwt_state(lwt)->tuninfo; 64 } 65 66 static struct ioam6_trace_hdr *ioam6_lwt_trace(struct lwtunnel_state *lwt) 67 { 68 return &(ioam6_lwt_state(lwt)->tuninfo.traceh); 69 } 70 71 static const struct nla_policy ioam6_iptunnel_policy[IOAM6_IPTUNNEL_MAX + 1] = { 72 [IOAM6_IPTUNNEL_FREQ_K] = NLA_POLICY_FULL_RANGE(NLA_U32, &freq_range), 73 [IOAM6_IPTUNNEL_FREQ_N] = NLA_POLICY_FULL_RANGE(NLA_U32, &freq_range), 74 [IOAM6_IPTUNNEL_MODE] = NLA_POLICY_RANGE(NLA_U8, 75 IOAM6_IPTUNNEL_MODE_MIN, 76 IOAM6_IPTUNNEL_MODE_MAX), 77 [IOAM6_IPTUNNEL_SRC] = NLA_POLICY_EXACT_LEN(sizeof(struct in6_addr)), 78 [IOAM6_IPTUNNEL_DST] = NLA_POLICY_EXACT_LEN(sizeof(struct in6_addr)), 79 [IOAM6_IPTUNNEL_TRACE] = NLA_POLICY_EXACT_LEN( 80 sizeof(struct ioam6_trace_hdr)), 81 }; 82 83 static bool ioam6_validate_trace_hdr(struct ioam6_trace_hdr *trace) 84 { 85 u32 fields; 86 87 if (!trace->type_be32 || !trace->remlen || 88 trace->remlen > IOAM6_TRACE_DATA_SIZE_MAX / 4 || 89 trace->type.bit12 | trace->type.bit13 | trace->type.bit14 | 90 trace->type.bit15 | trace->type.bit16 | trace->type.bit17 | 91 trace->type.bit18 | trace->type.bit19 | trace->type.bit20 | 92 trace->type.bit21 | trace->type.bit23) 93 return false; 94 95 trace->nodelen = 0; 96 fields = be32_to_cpu(trace->type_be32); 97 98 trace->nodelen += hweight32(fields & IOAM6_MASK_SHORT_FIELDS) 99 * (sizeof(__be32) / 4); 100 trace->nodelen += hweight32(fields & IOAM6_MASK_WIDE_FIELDS) 101 * (sizeof(__be64) / 4); 102 103 return true; 104 } 105 106 static int ioam6_build_state(struct net *net, struct nlattr *nla, 107 unsigned int family, const void *cfg, 108 struct lwtunnel_state **ts, 109 struct netlink_ext_ack *extack) 110 { 111 struct nlattr *tb[IOAM6_IPTUNNEL_MAX + 1]; 112 struct ioam6_lwt_encap *tuninfo; 113 struct ioam6_trace_hdr *trace; 114 struct lwtunnel_state *lwt; 115 struct ioam6_lwt *ilwt; 116 int len_aligned, err; 117 u32 freq_k, freq_n; 118 u8 mode; 119 120 if (family != AF_INET6) 121 return -EINVAL; 122 123 err = nla_parse_nested(tb, IOAM6_IPTUNNEL_MAX, nla, 124 ioam6_iptunnel_policy, extack); 125 if (err < 0) 126 return err; 127 128 if ((!tb[IOAM6_IPTUNNEL_FREQ_K] && tb[IOAM6_IPTUNNEL_FREQ_N]) || 129 (tb[IOAM6_IPTUNNEL_FREQ_K] && !tb[IOAM6_IPTUNNEL_FREQ_N])) { 130 NL_SET_ERR_MSG(extack, "freq: missing parameter"); 131 return -EINVAL; 132 } else if (!tb[IOAM6_IPTUNNEL_FREQ_K] && !tb[IOAM6_IPTUNNEL_FREQ_N]) { 133 freq_k = IOAM6_IPTUNNEL_FREQ_MIN; 134 freq_n = IOAM6_IPTUNNEL_FREQ_MIN; 135 } else { 136 freq_k = nla_get_u32(tb[IOAM6_IPTUNNEL_FREQ_K]); 137 freq_n = nla_get_u32(tb[IOAM6_IPTUNNEL_FREQ_N]); 138 139 if (freq_k > freq_n) { 140 NL_SET_ERR_MSG(extack, "freq: k > n is forbidden"); 141 return -EINVAL; 142 } 143 } 144 145 mode = nla_get_u8_default(tb[IOAM6_IPTUNNEL_MODE], 146 IOAM6_IPTUNNEL_MODE_INLINE); 147 148 if (tb[IOAM6_IPTUNNEL_SRC] && mode == IOAM6_IPTUNNEL_MODE_INLINE) { 149 NL_SET_ERR_MSG(extack, "no tunnel src expected with this mode"); 150 return -EINVAL; 151 } 152 153 if (!tb[IOAM6_IPTUNNEL_DST] && mode != IOAM6_IPTUNNEL_MODE_INLINE) { 154 NL_SET_ERR_MSG(extack, "this mode needs a tunnel destination"); 155 return -EINVAL; 156 } 157 158 if (!tb[IOAM6_IPTUNNEL_TRACE]) { 159 NL_SET_ERR_MSG(extack, "missing trace"); 160 return -EINVAL; 161 } 162 163 trace = nla_data(tb[IOAM6_IPTUNNEL_TRACE]); 164 if (!ioam6_validate_trace_hdr(trace)) { 165 NL_SET_ERR_MSG_ATTR(extack, tb[IOAM6_IPTUNNEL_TRACE], 166 "invalid trace validation"); 167 return -EINVAL; 168 } 169 170 len_aligned = ALIGN(trace->remlen * 4, 8); 171 lwt = lwtunnel_state_alloc(sizeof(*ilwt) + len_aligned); 172 if (!lwt) 173 return -ENOMEM; 174 175 ilwt = ioam6_lwt_state(lwt); 176 err = dst_cache_init(&ilwt->cache, GFP_ATOMIC); 177 if (err) 178 goto free_lwt; 179 180 atomic_set(&ilwt->pkt_cnt, 0); 181 ilwt->freq.k = freq_k; 182 ilwt->freq.n = freq_n; 183 184 ilwt->mode = mode; 185 186 if (!tb[IOAM6_IPTUNNEL_SRC]) { 187 ilwt->has_tunsrc = false; 188 } else { 189 ilwt->has_tunsrc = true; 190 ilwt->tunsrc = nla_get_in6_addr(tb[IOAM6_IPTUNNEL_SRC]); 191 192 if (ipv6_addr_any(&ilwt->tunsrc)) { 193 NL_SET_ERR_MSG_ATTR(extack, tb[IOAM6_IPTUNNEL_SRC], 194 "invalid tunnel source address"); 195 err = -EINVAL; 196 goto free_cache; 197 } 198 } 199 200 if (tb[IOAM6_IPTUNNEL_DST]) { 201 ilwt->tundst = nla_get_in6_addr(tb[IOAM6_IPTUNNEL_DST]); 202 203 if (ipv6_addr_any(&ilwt->tundst)) { 204 NL_SET_ERR_MSG_ATTR(extack, tb[IOAM6_IPTUNNEL_DST], 205 "invalid tunnel dest address"); 206 err = -EINVAL; 207 goto free_cache; 208 } 209 } 210 211 tuninfo = ioam6_lwt_info(lwt); 212 tuninfo->eh.hdrlen = ((sizeof(*tuninfo) + len_aligned) >> 3) - 1; 213 tuninfo->pad[0] = IPV6_TLV_PADN; 214 tuninfo->ioamh.type = IOAM6_TYPE_PREALLOC; 215 tuninfo->ioamh.opt_type = IPV6_TLV_IOAM; 216 tuninfo->ioamh.opt_len = sizeof(tuninfo->ioamh) - 2 + sizeof(*trace) 217 + trace->remlen * 4; 218 219 memcpy(&tuninfo->traceh, trace, sizeof(*trace)); 220 221 if (len_aligned - trace->remlen * 4) { 222 tuninfo->traceh.data[trace->remlen * 4] = IPV6_TLV_PADN; 223 tuninfo->traceh.data[trace->remlen * 4 + 1] = 2; 224 } 225 226 lwt->type = LWTUNNEL_ENCAP_IOAM6; 227 lwt->flags |= LWTUNNEL_STATE_OUTPUT_REDIRECT; 228 229 *ts = lwt; 230 231 return 0; 232 free_cache: 233 dst_cache_destroy(&ilwt->cache); 234 free_lwt: 235 kfree(lwt); 236 return err; 237 } 238 239 static int ioam6_do_fill(struct net *net, struct sk_buff *skb) 240 { 241 struct ioam6_trace_hdr *trace; 242 struct ioam6_namespace *ns; 243 244 trace = (struct ioam6_trace_hdr *)(skb_transport_header(skb) 245 + sizeof(struct ipv6_hopopt_hdr) + 2 246 + sizeof(struct ioam6_hdr)); 247 248 ns = ioam6_namespace(net, trace->namespace_id); 249 if (ns) 250 ioam6_fill_trace_data(skb, ns, trace, false); 251 252 return 0; 253 } 254 255 static int ioam6_do_inline(struct net *net, struct sk_buff *skb, 256 struct ioam6_lwt_encap *tuninfo) 257 { 258 struct ipv6hdr *oldhdr, *hdr; 259 int hdrlen, err; 260 261 hdrlen = (tuninfo->eh.hdrlen + 1) << 3; 262 263 err = skb_cow_head(skb, hdrlen + skb->mac_len); 264 if (unlikely(err)) 265 return err; 266 267 oldhdr = ipv6_hdr(skb); 268 skb_pull(skb, sizeof(*oldhdr)); 269 skb_postpull_rcsum(skb, skb_network_header(skb), sizeof(*oldhdr)); 270 271 skb_push(skb, sizeof(*oldhdr) + hdrlen); 272 skb_reset_network_header(skb); 273 skb_mac_header_rebuild(skb); 274 275 hdr = ipv6_hdr(skb); 276 memmove(hdr, oldhdr, sizeof(*oldhdr)); 277 tuninfo->eh.nexthdr = hdr->nexthdr; 278 279 skb_set_transport_header(skb, sizeof(*hdr)); 280 skb_postpush_rcsum(skb, hdr, sizeof(*hdr) + hdrlen); 281 282 memcpy(skb_transport_header(skb), (u8 *)tuninfo, hdrlen); 283 284 hdr->nexthdr = NEXTHDR_HOP; 285 hdr->payload_len = cpu_to_be16(skb->len - sizeof(*hdr)); 286 287 return ioam6_do_fill(net, skb); 288 } 289 290 static int ioam6_do_encap(struct net *net, struct sk_buff *skb, 291 struct ioam6_lwt_encap *tuninfo, 292 bool has_tunsrc, 293 struct in6_addr *tunsrc, 294 struct in6_addr *tundst) 295 { 296 struct dst_entry *dst = skb_dst(skb); 297 struct ipv6hdr *hdr, *inner_hdr; 298 int hdrlen, len, err; 299 300 hdrlen = (tuninfo->eh.hdrlen + 1) << 3; 301 len = sizeof(*hdr) + hdrlen; 302 303 err = skb_cow_head(skb, len + skb->mac_len); 304 if (unlikely(err)) 305 return err; 306 307 inner_hdr = ipv6_hdr(skb); 308 309 skb_push(skb, len); 310 skb_reset_network_header(skb); 311 skb_mac_header_rebuild(skb); 312 skb_set_transport_header(skb, sizeof(*hdr)); 313 314 tuninfo->eh.nexthdr = NEXTHDR_IPV6; 315 memcpy(skb_transport_header(skb), (u8 *)tuninfo, hdrlen); 316 317 hdr = ipv6_hdr(skb); 318 memcpy(hdr, inner_hdr, sizeof(*hdr)); 319 320 hdr->nexthdr = NEXTHDR_HOP; 321 hdr->payload_len = cpu_to_be16(skb->len - sizeof(*hdr)); 322 hdr->daddr = *tundst; 323 324 if (has_tunsrc) 325 memcpy(&hdr->saddr, tunsrc, sizeof(*tunsrc)); 326 else 327 ipv6_dev_get_saddr(net, dst->dev, &hdr->daddr, 328 IPV6_PREFER_SRC_PUBLIC, &hdr->saddr); 329 330 skb_postpush_rcsum(skb, hdr, len); 331 332 return ioam6_do_fill(net, skb); 333 } 334 335 static int ioam6_output(struct net *net, struct sock *sk, struct sk_buff *skb) 336 { 337 struct dst_entry *dst = skb_dst(skb); 338 struct in6_addr orig_daddr; 339 struct ioam6_lwt *ilwt; 340 int err = -EINVAL; 341 u32 pkt_cnt; 342 343 if (skb->protocol != htons(ETH_P_IPV6)) 344 goto drop; 345 346 ilwt = ioam6_lwt_state(dst->lwtstate); 347 348 /* Check for insertion frequency (i.e., "k over n" insertions) */ 349 pkt_cnt = atomic_fetch_inc(&ilwt->pkt_cnt); 350 if (pkt_cnt % ilwt->freq.n >= ilwt->freq.k) 351 goto out; 352 353 orig_daddr = ipv6_hdr(skb)->daddr; 354 355 switch (ilwt->mode) { 356 case IOAM6_IPTUNNEL_MODE_INLINE: 357 do_inline: 358 /* Direct insertion - if there is no Hop-by-Hop yet */ 359 if (ipv6_hdr(skb)->nexthdr == NEXTHDR_HOP) 360 goto out; 361 362 err = ioam6_do_inline(net, skb, &ilwt->tuninfo); 363 if (unlikely(err)) 364 goto drop; 365 366 break; 367 case IOAM6_IPTUNNEL_MODE_ENCAP: 368 do_encap: 369 /* Encapsulation (ip6ip6) */ 370 err = ioam6_do_encap(net, skb, &ilwt->tuninfo, 371 ilwt->has_tunsrc, &ilwt->tunsrc, 372 &ilwt->tundst); 373 if (unlikely(err)) 374 goto drop; 375 376 break; 377 case IOAM6_IPTUNNEL_MODE_AUTO: 378 /* Automatic (RFC8200 compliant): 379 * - local packets -> INLINE mode 380 * - in-transit packets -> ENCAP mode 381 */ 382 if (!skb->dev) 383 goto do_inline; 384 385 goto do_encap; 386 default: 387 goto drop; 388 } 389 390 err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); 391 if (unlikely(err)) 392 goto drop; 393 394 if (!ipv6_addr_equal(&orig_daddr, &ipv6_hdr(skb)->daddr)) { 395 local_bh_disable(); 396 dst = dst_cache_get(&ilwt->cache); 397 local_bh_enable(); 398 399 if (unlikely(!dst)) { 400 struct ipv6hdr *hdr = ipv6_hdr(skb); 401 struct flowi6 fl6; 402 403 memset(&fl6, 0, sizeof(fl6)); 404 fl6.daddr = hdr->daddr; 405 fl6.saddr = hdr->saddr; 406 fl6.flowlabel = ip6_flowinfo(hdr); 407 fl6.flowi6_mark = skb->mark; 408 fl6.flowi6_proto = hdr->nexthdr; 409 410 dst = ip6_route_output(net, NULL, &fl6); 411 if (dst->error) { 412 err = dst->error; 413 dst_release(dst); 414 goto drop; 415 } 416 417 local_bh_disable(); 418 dst_cache_set_ip6(&ilwt->cache, dst, &fl6.saddr); 419 local_bh_enable(); 420 } 421 422 skb_dst_drop(skb); 423 skb_dst_set(skb, dst); 424 425 return dst_output(net, sk, skb); 426 } 427 out: 428 return dst->lwtstate->orig_output(net, sk, skb); 429 drop: 430 kfree_skb(skb); 431 return err; 432 } 433 434 static void ioam6_destroy_state(struct lwtunnel_state *lwt) 435 { 436 dst_cache_destroy(&ioam6_lwt_state(lwt)->cache); 437 } 438 439 static int ioam6_fill_encap_info(struct sk_buff *skb, 440 struct lwtunnel_state *lwtstate) 441 { 442 struct ioam6_lwt *ilwt = ioam6_lwt_state(lwtstate); 443 int err; 444 445 err = nla_put_u32(skb, IOAM6_IPTUNNEL_FREQ_K, ilwt->freq.k); 446 if (err) 447 goto ret; 448 449 err = nla_put_u32(skb, IOAM6_IPTUNNEL_FREQ_N, ilwt->freq.n); 450 if (err) 451 goto ret; 452 453 err = nla_put_u8(skb, IOAM6_IPTUNNEL_MODE, ilwt->mode); 454 if (err) 455 goto ret; 456 457 if (ilwt->mode != IOAM6_IPTUNNEL_MODE_INLINE) { 458 if (ilwt->has_tunsrc) { 459 err = nla_put_in6_addr(skb, IOAM6_IPTUNNEL_SRC, 460 &ilwt->tunsrc); 461 if (err) 462 goto ret; 463 } 464 465 err = nla_put_in6_addr(skb, IOAM6_IPTUNNEL_DST, &ilwt->tundst); 466 if (err) 467 goto ret; 468 } 469 470 err = nla_put(skb, IOAM6_IPTUNNEL_TRACE, sizeof(ilwt->tuninfo.traceh), 471 &ilwt->tuninfo.traceh); 472 ret: 473 return err; 474 } 475 476 static int ioam6_encap_nlsize(struct lwtunnel_state *lwtstate) 477 { 478 struct ioam6_lwt *ilwt = ioam6_lwt_state(lwtstate); 479 int nlsize; 480 481 nlsize = nla_total_size(sizeof(ilwt->freq.k)) + 482 nla_total_size(sizeof(ilwt->freq.n)) + 483 nla_total_size(sizeof(ilwt->mode)) + 484 nla_total_size(sizeof(ilwt->tuninfo.traceh)); 485 486 if (ilwt->mode != IOAM6_IPTUNNEL_MODE_INLINE) { 487 if (ilwt->has_tunsrc) 488 nlsize += nla_total_size(sizeof(ilwt->tunsrc)); 489 490 nlsize += nla_total_size(sizeof(ilwt->tundst)); 491 } 492 493 return nlsize; 494 } 495 496 static int ioam6_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) 497 { 498 struct ioam6_trace_hdr *trace_a = ioam6_lwt_trace(a); 499 struct ioam6_trace_hdr *trace_b = ioam6_lwt_trace(b); 500 struct ioam6_lwt *ilwt_a = ioam6_lwt_state(a); 501 struct ioam6_lwt *ilwt_b = ioam6_lwt_state(b); 502 503 return (ilwt_a->freq.k != ilwt_b->freq.k || 504 ilwt_a->freq.n != ilwt_b->freq.n || 505 ilwt_a->mode != ilwt_b->mode || 506 ilwt_a->has_tunsrc != ilwt_b->has_tunsrc || 507 (ilwt_a->mode != IOAM6_IPTUNNEL_MODE_INLINE && 508 !ipv6_addr_equal(&ilwt_a->tundst, &ilwt_b->tundst)) || 509 (ilwt_a->mode != IOAM6_IPTUNNEL_MODE_INLINE && 510 ilwt_a->has_tunsrc && 511 !ipv6_addr_equal(&ilwt_a->tunsrc, &ilwt_b->tunsrc)) || 512 trace_a->namespace_id != trace_b->namespace_id); 513 } 514 515 static const struct lwtunnel_encap_ops ioam6_iptun_ops = { 516 .build_state = ioam6_build_state, 517 .destroy_state = ioam6_destroy_state, 518 .output = ioam6_output, 519 .fill_encap = ioam6_fill_encap_info, 520 .get_encap_size = ioam6_encap_nlsize, 521 .cmp_encap = ioam6_encap_cmp, 522 .owner = THIS_MODULE, 523 }; 524 525 int __init ioam6_iptunnel_init(void) 526 { 527 return lwtunnel_encap_add_ops(&ioam6_iptun_ops, LWTUNNEL_ENCAP_IOAM6); 528 } 529 530 void ioam6_iptunnel_exit(void) 531 { 532 lwtunnel_encap_del_ops(&ioam6_iptun_ops, LWTUNNEL_ENCAP_IOAM6); 533 } 534