Netlink: MPLS routes in kernel

Anyway, Bird is now capable to insert both MPLS routes and MPLS encap
routes into kernel.

It was (among others) needed to define platform-specific AF_MPLS to 28
as this constant has been assigned in the linux kernel.

No support for BSD now, it may be added in the future.

3 files changed, 235 insertions, 21 deletions

diff --git a/sysdep/cf/linux.h b/sysdep/cf/linux.h
index cec9499c..3a3a15da 100644
--- a/sysdep/cf/linux.h
+++ b/sysdep/cf/linux.h
@@ -21,6 +21,10 @@
 #define CONFIG_INCLUDE_SYSPRIV_H "sysdep/linux/syspriv.h"
 
 
+#ifndef AF_MPLS
+#define AF_MPLS 28
+#endif
+
 /*
 Link: sysdep/linux
 Link: sysdep/unix
diff --git a/sysdep/linux/netlink.c b/sysdep/linux/netlink.c
index c9a30719..23431172 100644
--- a/sysdep/linux/netlink.c
+++ b/sysdep/linux/netlink.c
@@ -31,6 +31,7 @@
 
 #include <asm/types.h>
 #include <linux/if.h>
+#include <linux/lwtunnel.h>
 #include <linux/netlink.h>
 #include <linux/rtnetlink.h>
 
@@ -51,6 +52,21 @@
 #define RTA_TABLE  15
 #endif
 
+#ifndef RTA_VIA
+#define RTA_VIA	 18
+#endif
+
+#ifndef RTA_NEWDST
+#define RTA_NEWDST  19
+#endif
+
+#ifndef RTA_ENCAP_TYPE
+#define RTA_ENCAP_TYPE	21
+#endif
+
+#ifndef RTA_ENCAP
+#define RTA_ENCAP  22
+#endif
 
 #define krt_ecmp6(p) ((p)->af == AF_INET6)
 
@@ -313,10 +329,16 @@ static struct nl_want_attrs ifa_attr_want6[BIRD_IFA_MAX] = {
 };
 
 
-#define BIRD_RTA_MAX  (RTA_TABLE+1)
+#define BIRD_RTA_MAX  (RTA_ENCAP+1)
 
 static struct nl_want_attrs nexthop_attr_want4[BIRD_RTA_MAX] = {
   [RTA_GATEWAY]	  = { 1, 1, sizeof(ip4_addr) },
+  [RTA_ENCAP_TYPE]= { 1, 1, sizeof(u16) },
+  [RTA_ENCAP]	  = { 1, 0, 0 },
+};
+
+static struct nl_want_attrs encap_mpls_want[BIRD_RTA_MAX] = {
+  [RTA_DST]       = { 1, 0, 0 },
 };
 
 static struct nl_want_attrs rtm_attr_want4[BIRD_RTA_MAX] = {
@@ -329,6 +351,8 @@ static struct nl_want_attrs rtm_attr_want4[BIRD_RTA_MAX] = {
   [RTA_MULTIPATH] = { 1, 0, 0 },
   [RTA_FLOW]	  = { 1, 1, sizeof(u32) },
   [RTA_TABLE]	  = { 1, 1, sizeof(u32) },
+  [RTA_ENCAP_TYPE]= { 1, 1, sizeof(u16) },
+  [RTA_ENCAP]	  = { 1, 0, 0 },
 };
 
 static struct nl_want_attrs rtm_attr_want6[BIRD_RTA_MAX] = {
@@ -341,6 +365,20 @@ static struct nl_want_attrs rtm_attr_want6[BIRD_RTA_MAX] = {
   [RTA_METRICS]	  = { 1, 0, 0 },
   [RTA_FLOW]	  = { 1, 1, sizeof(u32) },
   [RTA_TABLE]	  = { 1, 1, sizeof(u32) },
+  [RTA_ENCAP_TYPE]= { 1, 1, sizeof(u16) },
+  [RTA_ENCAP]	  = { 1, 0, 0 },
+};
+
+static struct nl_want_attrs rtm_attr_want_mpls[BIRD_RTA_MAX] = {
+  [RTA_DST]	  = { 1, 1, sizeof(u32) },
+  [RTA_IIF]	  = { 1, 1, sizeof(u32) },
+  [RTA_OIF]	  = { 1, 1, sizeof(u32) },
+  [RTA_PRIORITY]  = { 1, 1, sizeof(u32) },
+  [RTA_METRICS]	  = { 1, 0, 0 },
+  [RTA_FLOW]	  = { 1, 1, sizeof(u32) },
+  [RTA_TABLE]	  = { 1, 1, sizeof(u32) },
+  [RTA_VIA]	  = { 1, 0, 0 },
+  [RTA_NEWDST]	  = { 1, 0, 0 },
 };
 
 
@@ -373,6 +411,9 @@ nl_parse_attrs(struct rtattr *a, struct nl_want_attrs *want, struct rtattr **k,
   return 1;
 }
 
+static inline u16 rta_get_u16(struct rtattr *a)
+{ return *(u16 *) RTA_DATA(a); }
+
 static inline u32 rta_get_u32(struct rtattr *a)
 { return *(u32 *) RTA_DATA(a); }
 
@@ -390,6 +431,25 @@ static inline ip_addr rta_get_ipa(struct rtattr *a)
     return ipa_from_ip6(rta_get_ip6(a));
 }
 
+static inline ip_addr rta_get_via(struct rtattr *a)
+{
+  struct rtvia *v = RTA_DATA(a);
+  switch(v->rtvia_family) {
+    case AF_INET:  return ipa_from_ip4(ip4_ntoh(*(ip4_addr *) v->rtvia_addr));
+    case AF_INET6: return ipa_from_ip6(ip6_ntoh(*(ip6_addr *) v->rtvia_addr));
+  }
+  return IPA_NONE;
+}
+
+static u32 rta_mpls_stack[MPLS_MAX_LABEL_STACK];
+static inline int rta_get_mpls(struct rtattr *a, u32 *stack)
+{
+  if (RTA_PAYLOAD(a) % 4)
+    log(L_WARN "KRT: Strange length of received MPLS stack: %u", RTA_PAYLOAD(a));
+
+  return mpls_get(RTA_DATA(a), RTA_PAYLOAD(a) & ~0x3, stack);
+}
+
 struct rtattr *
 nl_add_attr(struct nlmsghdr *h, uint bufsize, uint code, const void *data, uint dlen)
 {
@@ -410,6 +470,24 @@ nl_add_attr(struct nlmsghdr *h, uint bufsize, uint code, const void *data, uint
   return a;
 }
 
+static inline struct rtattr *
+nl_open_attr(struct nlmsghdr *h, uint bufsize, uint code)
+{
+  return nl_add_attr(h, bufsize, code, NULL, 0);
+}
+
+static inline void
+nl_close_attr(struct nlmsghdr *h, struct rtattr *a)
+{
+  a->rta_len = (void *)h + NLMSG_ALIGN(h->nlmsg_len) - (void *)a;
+}
+
+static inline void
+nl_add_attr_u16(struct nlmsghdr *h, uint bufsize, int code, u16 data)
+{
+  nl_add_attr(h, bufsize, code, &data, 2);
+}
+
 static inline void
 nl_add_attr_u32(struct nlmsghdr *h, uint bufsize, int code, u32 data)
 {
@@ -439,16 +517,47 @@ nl_add_attr_ipa(struct nlmsghdr *h, uint bufsize, int code, ip_addr ipa)
     nl_add_attr_ip6(h, bufsize, code, ipa_to_ip6(ipa));
 }
 
-static inline struct rtattr *
-nl_open_attr(struct nlmsghdr *h, uint bufsize, uint code)
+static inline void
+nl_add_attr_mpls(struct nlmsghdr *h, uint bufsize, int code, int len, u32 *stack)
 {
-  return nl_add_attr(h, bufsize, code, NULL, 0);
+  char buf[len*4];
+  mpls_put(buf, len, stack);
+  nl_add_attr(h, bufsize, code, buf, len*4);
 }
 
 static inline void
-nl_close_attr(struct nlmsghdr *h, struct rtattr *a)
+nl_add_attr_mpls_encap(struct nlmsghdr *h, uint bufsize, int len, u32 *stack)
 {
-  a->rta_len = (void *)h + NLMSG_ALIGN(h->nlmsg_len) - (void *)a;
+  nl_add_attr_u16(h, bufsize, RTA_ENCAP_TYPE, LWTUNNEL_ENCAP_MPLS);
+
+  struct rtattr *nest = nl_open_attr(h, bufsize, RTA_ENCAP);
+  nl_add_attr_mpls(h, bufsize, RTA_DST, len, stack);
+  nl_close_attr(h, nest);
+}
+
+static inline void
+nl_add_attr_via(struct nlmsghdr *h, uint bufsize, ip_addr ipa)
+{
+  struct rtattr *nest = nl_open_attr(h, bufsize, RTA_VIA);
+  struct rtvia *via = RTA_DATA(nest);
+
+  h->nlmsg_len += sizeof(*via);
+
+  if (ipa_is_ip4(ipa)) {
+    ip4_addr ip4 = ipa_to_ip4(ipa);
+    ip4 = ip4_hton(ip4);
+    via->rtvia_family = AF_INET;
+    memcpy(via->rtvia_addr, &ip4, sizeof(ip4));
+    h->nlmsg_len += sizeof(ip4);
+  } else {
+    ip6_addr ip6 = ipa_to_ip6(ipa);
+    ip6 = ip6_hton(ip6);
+    via->rtvia_family = AF_INET6;
+    memcpy(via->rtvia_addr, &ip6, sizeof(ip6));
+    h->nlmsg_len += sizeof(ip6);
+  }
+
+  nl_close_attr(h, nest);
 }
 
 static inline struct rtnexthop *
@@ -471,8 +580,24 @@ nl_close_nexthop(struct nlmsghdr *h, struct rtnexthop *nh)
   nh->rtnh_len = (void *)h + NLMSG_ALIGN(h->nlmsg_len) - (void *)nh;
 }
 
+static inline void
+nl_add_nexthop(struct nlmsghdr *h, uint bufsize, struct nexthop *nh, int af)
+{
+  if (nh->labels > 0)
+    if (af == AF_MPLS)
+      nl_add_attr_mpls(h, bufsize, RTA_NEWDST, nh->labels, nh->label);
+    else
+      nl_add_attr_mpls_encap(h, bufsize, nh->labels, nh->label);
+
+  if (ipa_nonzero(nh->gw))
+    if (af == AF_MPLS)
+      nl_add_attr_via(h, bufsize, nh->gw);
+    else
+      nl_add_attr_ipa(h, bufsize, RTA_GATEWAY, nh->gw);
+}
+
 static void
-nl_add_multipath(struct nlmsghdr *h, uint bufsize, struct nexthop *nh)
+nl_add_multipath(struct nlmsghdr *h, uint bufsize, struct nexthop *nh, int af)
 {
   struct rtattr *a = nl_open_attr(h, bufsize, RTA_MULTIPATH);
 
@@ -484,7 +609,7 @@ nl_add_multipath(struct nlmsghdr *h, uint bufsize, struct nexthop *nh)
     rtnh->rtnh_hops = nh->weight;
     rtnh->rtnh_ifindex = nh->iface->index;
 
-    nl_add_attr_ipa(h, bufsize, RTA_GATEWAY, nh->gw);
+    nl_add_nexthop(h, bufsize, nh, af);
 
     nl_close_nexthop(h, rtnh);
   }
@@ -518,7 +643,7 @@ nl_parse_multipath(struct krt_proto *p, struct rtattr *ra)
       if (nh_buf_used == nh_buf_size)
       {
 	nh_buf_size = nh_buf_size ? (nh_buf_size * 2) : 4;
-	nh_buffer = xrealloc(nh_buffer, nh_buf_size * sizeof(struct nexthop));
+	nh_buffer = xrealloc(nh_buffer, nh_buf_size * NEXTHOP_MAX_SIZE);
       }
       *last = rv = nh_buffer + nh_buf_used++;
       rv->next = NULL;
@@ -543,7 +668,21 @@ nl_parse_multipath(struct krt_proto *p, struct rtattr *ra)
 	    return NULL;
 	}
       else
-	return NULL;
+	rv->gw = IPA_NONE;
+      if (a[RTA_ENCAP_TYPE])
+	{
+	  if (rta_get_u16(a[RTA_ENCAP_TYPE]) != LWTUNNEL_ENCAP_MPLS) {
+	    log(L_WARN "KRT: Unknown encapsulation method %d in multipath", rta_get_u16(a[RTA_ENCAP_TYPE]));
+	    return NULL;
+	  }
+
+	  struct rtattr *enca[BIRD_RTA_MAX];
+	  nl_attr_len = RTA_PAYLOAD(a[RTA_ENCAP]);
+	  nl_parse_attrs(RTA_DATA(a[RTA_ENCAP]), encap_mpls_want, enca, sizeof(enca));
+	  rv->labels = rta_get_mpls(enca[RTA_DST], rv->label);
+	  break;
+	}
+
 
       len -= NLMSG_ALIGN(nh->rtnh_len);
       nh = RTNH_NEXT(nh);
@@ -1008,7 +1147,13 @@ nl_send_route(struct krt_proto *p, rte *e, struct ea_list *eattrs, int op, int d
   r->r.rtm_dst_len = net_pxlen(net->n.addr);
   r->r.rtm_protocol = RTPROT_BIRD;
   r->r.rtm_scope = RT_SCOPE_UNIVERSE;
-  nl_add_attr_ipa(&r->h, rsize, RTA_DST, net_prefix(net->n.addr));
+  if (p->af == AF_MPLS)
+  {
+    u32 label = net_mpls(net->n.addr);
+    nl_add_attr_mpls(&r->h, rsize, RTA_DST, 1, &label);
+  }
+  else
+    nl_add_attr_ipa(&r->h, rsize, RTA_DST, net_prefix(net->n.addr));
 
   /*
    * Strange behavior for RTM_DELROUTE:
@@ -1071,13 +1216,11 @@ dest:
     case RTD_UNICAST:
       r->r.rtm_type = RTN_UNICAST;
       if (nh->next && !krt_ecmp6(p))
-	nl_add_multipath(&r->h, rsize, nh);
+	nl_add_multipath(&r->h, rsize, nh, p->af);
       else
       {
 	nl_add_attr_u32(&r->h, rsize, RTA_OIF, nh->iface->index);
-
-	if (ipa_nonzero(nh->gw))
-	  nl_add_attr_ipa(&r->h, rsize, RTA_GATEWAY, nh->gw);
+	nl_add_nexthop(&r->h, rsize, nh, p->af);
       }
       break;
     case RTD_BLACKHOLE:
@@ -1276,6 +1419,19 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
 	net_fill_ip6(&dst, IP6_NONE, 0);
       break;
 
+    case AF_MPLS:
+      if (!nl_parse_attrs(RTM_RTA(i), rtm_attr_want_mpls, a, sizeof(a)))
+	return;
+
+      if (a[RTA_DST])
+	if (rta_get_mpls(a[RTA_DST], rta_mpls_stack) == 1)
+	  net_fill_mpls(&dst, rta_mpls_stack[0]);
+	else
+	  log(L_WARN "KRT: Got multi-label MPLS RTA_DST");
+      else
+	return; /* No support for MPLS routes without RTA_DST */
+      break;
+
     default:
       return;
     }
@@ -1338,7 +1494,7 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
   if (s->net && !nl_mergable_route(s, net, p, priority, i->rtm_type))
     nl_announce_route(s);
 
-  rta *ra = lp_allocz(s->pool, sizeof(rta)); // TODO: fix alloc
+  rta *ra = lp_allocz(s->pool, RTA_MAX_SIZE);
   ra->src = p->p.main_source;
   ra->source = RTS_INHERIT;
   ra->scope = SCOPE_UNIVERSE;
@@ -1367,9 +1523,12 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
 	  return;
 	}
 
-      if (a[RTA_GATEWAY])
+      if ((i->rtm_family != AF_MPLS) && a[RTA_GATEWAY] || (i->rtm_family == AF_MPLS) && a[RTA_VIA])
 	{
-	  ra->nh.gw = rta_get_ipa(a[RTA_GATEWAY]);
+	  if (i->rtm_family == AF_MPLS)
+	    ra->nh.gw = rta_get_via(a[RTA_VIA]);
+	  else
+	    ra->nh.gw = rta_get_ipa(a[RTA_GATEWAY]);
 
 	  /* Silently skip strange 6to4 routes */
 	  const net_addr_ip6 sit = NET_ADDR_IP6(IP6_NONE, 96);
@@ -1377,7 +1536,7 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
 	    return;
 
 	  neighbor *nbr;
-	  nbr = neigh_find2(&p->p, &ra->nh.gw, ra->nh.iface,
+	  nbr = neigh_find2(&p->p, &(ra->nh.gw), ra->nh.iface,
 			    (i->rtm_flags & RTNH_F_ONLINK) ? NEF_ONLINK : 0);
 	  if (!nbr || (nbr->scope == SCOPE_HOST))
 	    {
@@ -1403,6 +1562,44 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
       return;
     }
 
+  int labels = 0;
+  if ((i->rtm_family == AF_MPLS) && a[RTA_NEWDST] && !ra->nh.next)
+    labels = rta_get_mpls(a[RTA_NEWDST], ra->nh.label);
+
+  if (a[RTA_ENCAP] && a[RTA_ENCAP_TYPE] && !ra->nh.next)
+    {
+      switch (rta_get_u16(a[RTA_ENCAP_TYPE]))
+	{
+	  case LWTUNNEL_ENCAP_MPLS:
+	    {
+	      struct rtattr *enca[BIRD_RTA_MAX];
+	      nl_attr_len = RTA_PAYLOAD(a[RTA_ENCAP]);
+	      nl_parse_attrs(RTA_DATA(a[RTA_ENCAP]), encap_mpls_want, enca, sizeof(enca));
+	      labels = rta_get_mpls(enca[RTA_DST], ra->nh.label);
+	      break;
+	    }
+	  default:
+	    SKIP("unknown encapsulation method %d\n", rta_get_u16(a[RTA_ENCAP_TYPE]));
+	    break;
+	}
+    }
+
+  if (labels < 0)
+  {
+    log(L_WARN "KRT: Too long MPLS stack received, ignoring.");
+    ra->nh.labels = 0;
+  }
+  else
+    ra->nh.labels = labels;
+
+  rte *e = rte_get_temp(ra);
+  e->net = net;
+  e->u.krt.src = src;
+  e->u.krt.proto = i->rtm_protocol;
+  e->u.krt.seen = 0;
+  e->u.krt.best = 0;
+  e->u.krt.metric = 0;
+
   if (i->rtm_scope != def_scope)
     {
       ea_list *ea = lp_alloc(s->pool, sizeof(ea_list) + sizeof(eattr));
@@ -1416,6 +1613,9 @@ nl_parse_route(struct nl_parse_state *s, struct nlmsghdr *h)
       ea->attrs[0].u.data = i->rtm_scope;
     }
 
+  if (a[RTA_PRIORITY])
+    e->u.krt.metric = rta_get_u32(a[RTA_PRIORITY]);
+
   if (a[RTA_PREFSRC])
     {
       ip_addr ps = rta_get_ipa(a[RTA_PREFSRC]);
@@ -1527,6 +1727,15 @@ krt_do_scan(struct krt_proto *p UNUSED)	/* CONFIG_ALL_TABLES_AT_ONCE => p is NUL
     else
       log(L_DEBUG "nl_scan_fire: Unknown packet received (type=%d)", h->nlmsg_type);
   nl_parse_end(&s);
+
+  nl_parse_begin(&s, 1, 1);
+  nl_request_dump(AF_MPLS, RTM_GETROUTE);
+  while (h = nl_get_scan())
+    if (h->nlmsg_type == RTM_NEWROUTE || h->nlmsg_type == RTM_DELROUTE)
+      nl_parse_route(&s, h);
+    else
+      log(L_DEBUG "nl_scan_fire: Unknown packet received (type=%d)", h->nlmsg_type);
+  nl_parse_end(&s);
 }
 
 /*
diff --git a/sysdep/unix/krt.c b/sysdep/unix/krt.c
index 9f66d2f4..c273cb10 100644
--- a/sysdep/unix/krt.c
+++ b/sysdep/unix/krt.c
@@ -1153,7 +1153,8 @@ krt_start(struct proto *P)
   {
   case NET_IP4:	p->af = AF_INET; break;
   case NET_IP6:	p->af = AF_INET6; break;
-  default:	ASSERT(0);
+  case NET_MPLS: p->af = AF_MPLS; break;
+  default: log(L_ERR "KRT: Tried to start with strange net type: %d", p->p.net_type); return PS_START; break;
   }
 
   add_tail(&krt_proto_list, &p->krt_node);
@@ -1264,7 +1265,7 @@ struct protocol proto_unix_kernel = {
   .template =		"kernel%d",
   .attr_class =		EAP_KRT,
   .preference =		DEF_PREF_INHERITED,
-  .channel_mask =	NB_IP,
+  .channel_mask =	NB_IP | NB_MPLS,
   .proto_size =		sizeof(struct krt_proto),
   .config_size =	sizeof(struct krt_config),
   .preconfig =		krt_preconfig,