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) 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 if (!tb[IOAM6_IPTUNNEL_MODE]) 146 mode = IOAM6_IPTUNNEL_MODE_INLINE; 147 else 148 mode = nla_get_u8(tb[IOAM6_IPTUNNEL_MODE]); 149 150 if (tb[IOAM6_IPTUNNEL_SRC] && mode == IOAM6_IPTUNNEL_MODE_INLINE) { 151 NL_SET_ERR_MSG(extack, "no tunnel src expected with this mode"); 152 return -EINVAL; 153 } 154 155 if (!tb[IOAM6_IPTUNNEL_DST] && mode != IOAM6_IPTUNNEL_MODE_INLINE) { 156 NL_SET_ERR_MSG(extack, "this mode needs a tunnel destination"); 157 return -EINVAL; 158 } 159 160 if (!tb[IOAM6_IPTUNNEL_TRACE]) { 161 NL_SET_ERR_MSG(extack, "missing trace"); 162 return -EINVAL; 163 } 164 165 trace = nla_data(tb[IOAM6_IPTUNNEL_TRACE]); 166 if (!ioam6_validate_trace_hdr(trace)) { 167 NL_SET_ERR_MSG_ATTR(extack, tb[IOAM6_IPTUNNEL_TRACE], 168 "invalid trace validation"); 169 return -EINVAL; 170 } 171 172 len_aligned = ALIGN(trace->remlen * 4, 8); 173 lwt = lwtunnel_state_alloc(sizeof(*ilwt) + len_aligned); 174 if (!lwt) 175 return -ENOMEM; 176 177 ilwt = ioam6_lwt_state(lwt); 178 err = dst_cache_init(&ilwt->cache, GFP_ATOMIC); 179 if (err) 180 goto free_lwt; 181 182 atomic_set(&ilwt->pkt_cnt, 0); 183 ilwt->freq.k = freq_k; 184 ilwt->freq.n = freq_n; 185 186 ilwt->mode = mode; 187 188 if (!tb[IOAM6_IPTUNNEL_SRC]) { 189 ilwt->has_tunsrc = false; 190 } else { 191 ilwt->has_tunsrc = true; 192 ilwt->tunsrc = nla_get_in6_addr(tb[IOAM6_IPTUNNEL_SRC]); 193 194 if (ipv6_addr_any(&ilwt->tunsrc)) { 195 NL_SET_ERR_MSG_ATTR(extack, tb[IOAM6_IPTUNNEL_SRC], 196 "invalid tunnel source address"); 197 err = -EINVAL; 198 goto free_cache; 199 } 200 } 201 202 if (tb[IOAM6_IPTUNNEL_DST]) 203 ilwt->tundst = nla_get_in6_addr(tb[IOAM6_IPTUNNEL_DST]); 204 205 tuninfo = ioam6_lwt_info(lwt); 206 tuninfo->eh.hdrlen = ((sizeof(*tuninfo) + len_aligned) >> 3) - 1; 207 tuninfo->pad[0] = IPV6_TLV_PADN; 208 tuninfo->ioamh.type = IOAM6_TYPE_PREALLOC; 209 tuninfo->ioamh.opt_type = IPV6_TLV_IOAM; 210 tuninfo->ioamh.opt_len = sizeof(tuninfo->ioamh) - 2 + sizeof(*trace) 211 + trace->remlen * 4; 212 213 memcpy(&tuninfo->traceh, trace, sizeof(*trace)); 214 215 if (len_aligned - trace->remlen * 4) { 216 tuninfo->traceh.data[trace->remlen * 4] = IPV6_TLV_PADN; 217 tuninfo->traceh.data[trace->remlen * 4 + 1] = 2; 218 } 219 220 lwt->type = LWTUNNEL_ENCAP_IOAM6; 221 lwt->flags |= LWTUNNEL_STATE_OUTPUT_REDIRECT; 222 223 *ts = lwt; 224 225 return 0; 226 free_cache: 227 dst_cache_destroy(&ilwt->cache); 228 free_lwt: 229 kfree(lwt); 230 return err; 231 } 232 233 static int ioam6_do_fill(struct net *net, struct sk_buff *skb) 234 { 235 struct ioam6_trace_hdr *trace; 236 struct ioam6_namespace *ns; 237 238 trace = (struct ioam6_trace_hdr *)(skb_transport_header(skb) 239 + sizeof(struct ipv6_hopopt_hdr) + 2 240 + sizeof(struct ioam6_hdr)); 241 242 ns = ioam6_namespace(net, trace->namespace_id); 243 if (ns) 244 ioam6_fill_trace_data(skb, ns, trace, false); 245 246 return 0; 247 } 248 249 static int ioam6_do_inline(struct net *net, struct sk_buff *skb, 250 struct ioam6_lwt_encap *tuninfo) 251 { 252 struct ipv6hdr *oldhdr, *hdr; 253 int hdrlen, err; 254 255 hdrlen = (tuninfo->eh.hdrlen + 1) << 3; 256 257 err = skb_cow_head(skb, hdrlen + skb->mac_len); 258 if (unlikely(err)) 259 return err; 260 261 oldhdr = ipv6_hdr(skb); 262 skb_pull(skb, sizeof(*oldhdr)); 263 skb_postpull_rcsum(skb, skb_network_header(skb), sizeof(*oldhdr)); 264 265 skb_push(skb, sizeof(*oldhdr) + hdrlen); 266 skb_reset_network_header(skb); 267 skb_mac_header_rebuild(skb); 268 269 hdr = ipv6_hdr(skb); 270 memmove(hdr, oldhdr, sizeof(*oldhdr)); 271 tuninfo->eh.nexthdr = hdr->nexthdr; 272 273 skb_set_transport_header(skb, sizeof(*hdr)); 274 skb_postpush_rcsum(skb, hdr, sizeof(*hdr) + hdrlen); 275 276 memcpy(skb_transport_header(skb), (u8 *)tuninfo, hdrlen); 277 278 hdr->nexthdr = NEXTHDR_HOP; 279 hdr->payload_len = cpu_to_be16(skb->len - sizeof(*hdr)); 280 281 return ioam6_do_fill(net, skb); 282 } 283 284 static int ioam6_do_encap(struct net *net, struct sk_buff *skb, 285 struct ioam6_lwt_encap *tuninfo, 286 bool has_tunsrc, 287 struct in6_addr *tunsrc, 288 struct in6_addr *tundst) 289 { 290 struct dst_entry *dst = skb_dst(skb); 291 struct ipv6hdr *hdr, *inner_hdr; 292 int hdrlen, len, err; 293 294 hdrlen = (tuninfo->eh.hdrlen + 1) << 3; 295 len = sizeof(*hdr) + hdrlen; 296 297 err = skb_cow_head(skb, len + skb->mac_len); 298 if (unlikely(err)) 299 return err; 300 301 inner_hdr = ipv6_hdr(skb); 302 303 skb_push(skb, len); 304 skb_reset_network_header(skb); 305 skb_mac_header_rebuild(skb); 306 skb_set_transport_header(skb, sizeof(*hdr)); 307 308 tuninfo->eh.nexthdr = NEXTHDR_IPV6; 309 memcpy(skb_transport_header(skb), (u8 *)tuninfo, hdrlen); 310 311 hdr = ipv6_hdr(skb); 312 memcpy(hdr, inner_hdr, sizeof(*hdr)); 313 314 hdr->nexthdr = NEXTHDR_HOP; 315 hdr->payload_len = cpu_to_be16(skb->len - sizeof(*hdr)); 316 hdr->daddr = *tundst; 317 318 if (has_tunsrc) 319 memcpy(&hdr->saddr, tunsrc, sizeof(*tunsrc)); 320 else 321 ipv6_dev_get_saddr(net, dst->dev, &hdr->daddr, 322 IPV6_PREFER_SRC_PUBLIC, &hdr->saddr); 323 324 skb_postpush_rcsum(skb, hdr, len); 325 326 return ioam6_do_fill(net, skb); 327 } 328 329 static int ioam6_output(struct net *net, struct sock *sk, struct sk_buff *skb) 330 { 331 struct dst_entry *dst = skb_dst(skb); 332 struct in6_addr orig_daddr; 333 struct ioam6_lwt *ilwt; 334 int err = -EINVAL; 335 u32 pkt_cnt; 336 337 if (skb->protocol != htons(ETH_P_IPV6)) 338 goto drop; 339 340 ilwt = ioam6_lwt_state(dst->lwtstate); 341 342 /* Check for insertion frequency (i.e., "k over n" insertions) */ 343 pkt_cnt = atomic_fetch_inc(&ilwt->pkt_cnt); 344 if (pkt_cnt % ilwt->freq.n >= ilwt->freq.k) 345 goto out; 346 347 orig_daddr = ipv6_hdr(skb)->daddr; 348 349 switch (ilwt->mode) { 350 case IOAM6_IPTUNNEL_MODE_INLINE: 351 do_inline: 352 /* Direct insertion - if there is no Hop-by-Hop yet */ 353 if (ipv6_hdr(skb)->nexthdr == NEXTHDR_HOP) 354 goto out; 355 356 err = ioam6_do_inline(net, skb, &ilwt->tuninfo); 357 if (unlikely(err)) 358 goto drop; 359 360 break; 361 case IOAM6_IPTUNNEL_MODE_ENCAP: 362 do_encap: 363 /* Encapsulation (ip6ip6) */ 364 err = ioam6_do_encap(net, skb, &ilwt->tuninfo, 365 ilwt->has_tunsrc, &ilwt->tunsrc, 366 &ilwt->tundst); 367 if (unlikely(err)) 368 goto drop; 369 370 break; 371 case IOAM6_IPTUNNEL_MODE_AUTO: 372 /* Automatic (RFC8200 compliant): 373 * - local packets -> INLINE mode 374 * - in-transit packets -> ENCAP mode 375 */ 376 if (!skb->dev) 377 goto do_inline; 378 379 goto do_encap; 380 default: 381 goto drop; 382 } 383 384 err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); 385 if (unlikely(err)) 386 goto drop; 387 388 if (!ipv6_addr_equal(&orig_daddr, &ipv6_hdr(skb)->daddr)) { 389 local_bh_disable(); 390 dst = dst_cache_get(&ilwt->cache); 391 local_bh_enable(); 392 393 if (unlikely(!dst)) { 394 struct ipv6hdr *hdr = ipv6_hdr(skb); 395 struct flowi6 fl6; 396 397 memset(&fl6, 0, sizeof(fl6)); 398 fl6.daddr = hdr->daddr; 399 fl6.saddr = hdr->saddr; 400 fl6.flowlabel = ip6_flowinfo(hdr); 401 fl6.flowi6_mark = skb->mark; 402 fl6.flowi6_proto = hdr->nexthdr; 403 404 dst = ip6_route_output(net, NULL, &fl6); 405 if (dst->error) { 406 err = dst->error; 407 dst_release(dst); 408 goto drop; 409 } 410 411 local_bh_disable(); 412 dst_cache_set_ip6(&ilwt->cache, dst, &fl6.saddr); 413 local_bh_enable(); 414 } 415 416 skb_dst_drop(skb); 417 skb_dst_set(skb, dst); 418 419 return dst_output(net, sk, skb); 420 } 421 out: 422 return dst->lwtstate->orig_output(net, sk, skb); 423 drop: 424 kfree_skb(skb); 425 return err; 426 } 427 428 static void ioam6_destroy_state(struct lwtunnel_state *lwt) 429 { 430 dst_cache_destroy(&ioam6_lwt_state(lwt)->cache); 431 } 432 433 static int ioam6_fill_encap_info(struct sk_buff *skb, 434 struct lwtunnel_state *lwtstate) 435 { 436 struct ioam6_lwt *ilwt = ioam6_lwt_state(lwtstate); 437 int err; 438 439 err = nla_put_u32(skb, IOAM6_IPTUNNEL_FREQ_K, ilwt->freq.k); 440 if (err) 441 goto ret; 442 443 err = nla_put_u32(skb, IOAM6_IPTUNNEL_FREQ_N, ilwt->freq.n); 444 if (err) 445 goto ret; 446 447 err = nla_put_u8(skb, IOAM6_IPTUNNEL_MODE, ilwt->mode); 448 if (err) 449 goto ret; 450 451 if (ilwt->mode != IOAM6_IPTUNNEL_MODE_INLINE) { 452 if (ilwt->has_tunsrc) { 453 err = nla_put_in6_addr(skb, IOAM6_IPTUNNEL_SRC, 454 &ilwt->tunsrc); 455 if (err) 456 goto ret; 457 } 458 459 err = nla_put_in6_addr(skb, IOAM6_IPTUNNEL_DST, &ilwt->tundst); 460 if (err) 461 goto ret; 462 } 463 464 err = nla_put(skb, IOAM6_IPTUNNEL_TRACE, sizeof(ilwt->tuninfo.traceh), 465 &ilwt->tuninfo.traceh); 466 ret: 467 return err; 468 } 469 470 static int ioam6_encap_nlsize(struct lwtunnel_state *lwtstate) 471 { 472 struct ioam6_lwt *ilwt = ioam6_lwt_state(lwtstate); 473 int nlsize; 474 475 nlsize = nla_total_size(sizeof(ilwt->freq.k)) + 476 nla_total_size(sizeof(ilwt->freq.n)) + 477 nla_total_size(sizeof(ilwt->mode)) + 478 nla_total_size(sizeof(ilwt->tuninfo.traceh)); 479 480 if (ilwt->mode != IOAM6_IPTUNNEL_MODE_INLINE) { 481 if (ilwt->has_tunsrc) 482 nlsize += nla_total_size(sizeof(ilwt->tunsrc)); 483 484 nlsize += nla_total_size(sizeof(ilwt->tundst)); 485 } 486 487 return nlsize; 488 } 489 490 static int ioam6_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) 491 { 492 struct ioam6_trace_hdr *trace_a = ioam6_lwt_trace(a); 493 struct ioam6_trace_hdr *trace_b = ioam6_lwt_trace(b); 494 struct ioam6_lwt *ilwt_a = ioam6_lwt_state(a); 495 struct ioam6_lwt *ilwt_b = ioam6_lwt_state(b); 496 497 return (ilwt_a->freq.k != ilwt_b->freq.k || 498 ilwt_a->freq.n != ilwt_b->freq.n || 499 ilwt_a->mode != ilwt_b->mode || 500 ilwt_a->has_tunsrc != ilwt_b->has_tunsrc || 501 (ilwt_a->mode != IOAM6_IPTUNNEL_MODE_INLINE && 502 !ipv6_addr_equal(&ilwt_a->tundst, &ilwt_b->tundst)) || 503 (ilwt_a->mode != IOAM6_IPTUNNEL_MODE_INLINE && 504 ilwt_a->has_tunsrc && 505 !ipv6_addr_equal(&ilwt_a->tunsrc, &ilwt_b->tunsrc)) || 506 trace_a->namespace_id != trace_b->namespace_id); 507 } 508 509 static const struct lwtunnel_encap_ops ioam6_iptun_ops = { 510 .build_state = ioam6_build_state, 511 .destroy_state = ioam6_destroy_state, 512 .output = ioam6_output, 513 .fill_encap = ioam6_fill_encap_info, 514 .get_encap_size = ioam6_encap_nlsize, 515 .cmp_encap = ioam6_encap_cmp, 516 .owner = THIS_MODULE, 517 }; 518 519 int __init ioam6_iptunnel_init(void) 520 { 521 return lwtunnel_encap_add_ops(&ioam6_iptun_ops, LWTUNNEL_ENCAP_IOAM6); 522 } 523 524 void ioam6_iptunnel_exit(void) 525 { 526 lwtunnel_encap_del_ops(&ioam6_iptun_ops, LWTUNNEL_ENCAP_IOAM6); 527 } 528