Multipath support for linux kernel protocol.

sysdep/linux/netlink/netlink.c

@@ -238,37 +238,143 @@ nl_parse_attrs(struct rtattr *a, struct rtattr **k, int ksize)
     return 1;
 }
 
-static void
+void
-nl_add_attr_u32(struct nlmsghdr *h, unsigned maxsize, int code, u32 data)
+nl_add_attr(struct nlmsghdr *h, unsigned bufsize, unsigned code,
+	    void *data, unsigned dlen)
 {
-  unsigned len = RTA_LENGTH(4);
+  unsigned len = RTA_LENGTH(dlen);
+  unsigned pos = NLMSG_ALIGN(h->nlmsg_len);
   struct rtattr *a;
 
-  if (NLMSG_ALIGN(h->nlmsg_len) + len > maxsize)
+  if (pos + len > bufsize)
-    bug("nl_add_attr32: packet buffer overflow");
+    bug("nl_add_attr: packet buffer overflow");
-  a = (struct rtattr *)((char *)h + NLMSG_ALIGN(h->nlmsg_len));
+  a = (struct rtattr *)((char *)h + pos);
   a->rta_type = code;
   a->rta_len = len;
-  memcpy(RTA_DATA(a), &data, 4);
+  h->nlmsg_len = pos + len;
-  h->nlmsg_len = NLMSG_ALIGN(h->nlmsg_len) + len;
+  memcpy(RTA_DATA(a), data, dlen);
 }
 
-static void
+static inline void
-nl_add_attr_ipa(struct nlmsghdr *h, unsigned maxsize, int code, ip_addr ipa)
+nl_add_attr_u32(struct nlmsghdr *h, unsigned bufsize, int code, u32 data)
 {
-  unsigned len = RTA_LENGTH(sizeof(ipa));
+  nl_add_attr(h, bufsize, code, &data, 4);
-  struct rtattr *a;
+}
 
-  if (NLMSG_ALIGN(h->nlmsg_len) + len > maxsize)
+static inline void
-    bug("nl_add_attr_ipa: packet buffer overflow");
+nl_add_attr_ipa(struct nlmsghdr *h, unsigned bufsize, int code, ip_addr ipa)
-  a = (struct rtattr *)((char *)h + NLMSG_ALIGN(h->nlmsg_len));
+{
-  a->rta_type = code;
-  a->rta_len = len;
   ipa_hton(ipa);
-  memcpy(RTA_DATA(a), &ipa, sizeof(ipa));
+  nl_add_attr(h, bufsize, code, &ipa, sizeof(ipa));
-  h->nlmsg_len = NLMSG_ALIGN(h->nlmsg_len) + len;
 }
 
+#define RTNH_SIZE (sizeof(struct rtnexthop) + sizeof(struct rtattr) + sizeof(ip_addr))
+
+static inline void
+add_mpnexthop(char *buf, ip_addr ipa, unsigned iface, unsigned char weight)
+{
+  struct rtnexthop *nh = (void *) buf;
+  struct rtattr *rt = (void *) (buf + sizeof(*nh));
+  nh->rtnh_len = RTNH_SIZE;
+  nh->rtnh_flags = 0;
+  nh->rtnh_hops = weight;
+  nh->rtnh_ifindex = iface;
+  rt->rta_len = sizeof(*rt) + sizeof(ipa);
+  rt->rta_type = RTA_GATEWAY;
+  ipa_hton(ipa);
+  memcpy(buf + sizeof(*nh) + sizeof(*rt), &ipa, sizeof(ipa));
+}
+
+
+static void
+nl_add_multipath(struct nlmsghdr *h, unsigned bufsize, struct mpnh *nh)
+{
+  unsigned len = sizeof(struct rtattr);
+  unsigned pos = NLMSG_ALIGN(h->nlmsg_len);
+  char *buf = (char *)h + pos;
+  struct rtattr *rt = (void *) buf;
+  buf += len;
+  
+  for (; nh; nh = nh->next)
+    {
+      len += RTNH_SIZE;
+      if (pos + len > bufsize)
+	bug("nl_add_multipath: packet buffer overflow");
+
+      add_mpnexthop(buf, nh->gw, nh->iface->index, nh->weight);
+      buf += RTNH_SIZE;
+    }
+
+  rt->rta_type = RTA_MULTIPATH;
+  rt->rta_len = len;
+  h->nlmsg_len = pos + len;
+}
+
+
+static struct mpnh *
+nl_parse_multipath(struct krt_proto *p, struct rtattr *ra)
+{
+  /* Temporary buffer for multicast nexthops */
+  static struct mpnh *nh_buffer;
+  static int nh_buf_size;	/* in number of structures */
+  static int nh_buf_used;
+
+  struct rtattr *a[RTA_CACHEINFO+1];
+  struct rtnexthop *nh = RTA_DATA(ra);
+  struct mpnh *rv, *first, **last;
+  int len = RTA_PAYLOAD(ra);
+
+  first = NULL;
+  last = &first;
+  nh_buf_used = 0;
+
+  while (len)
+    {
+      /* Use RTNH_OK(nh,len) ?? */
+      if ((len < sizeof(*nh)) || (len < nh->rtnh_len))
+	return NULL;
+
+      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 mpnh));
+      }
+      *last = rv = nh_buffer + nh_buf_used++;
+      rv->next = NULL;
+      last = &(rv->next);
+
+      rv->weight = nh->rtnh_hops;
+      rv->iface = if_find_by_index(nh->rtnh_ifindex);
+      if (!rv->iface)
+	return NULL;
+
+      /* Nonexistent RTNH_PAYLOAD ?? */
+      nl_attr_len = nh->rtnh_len - RTNH_LENGTH(0);
+      nl_parse_attrs(RTNH_DATA(nh), a, sizeof(a));
+      if (a[RTA_GATEWAY])
+	{
+	  if (RTA_PAYLOAD(a[RTA_GATEWAY]) != sizeof(ip_addr))
+	    return NULL;
+
+	  memcpy(&rv->gw, RTA_DATA(a[RTA_GATEWAY]), sizeof(ip_addr));
+	  ipa_ntoh(rv->gw);
+
+	  neighbor *ng = neigh_find2(&p->p, &rv->gw, rv->iface,
+				     (nh->rtnh_flags & RTNH_F_ONLINK) ? NEF_ONLINK : 0);
+	  if (!ng || (ng->scope == SCOPE_HOST))
+	    return NULL;
+	}
+      else
+	return NULL;
+
+      len -= NLMSG_ALIGN(nh->rtnh_len);
+      nh = RTNH_NEXT(nh);
+    }
+
+  return first;
+}
+
+
 /*
  *	Scanning of interfaces
  */
@@ -490,6 +596,7 @@ krt_capable(rte *e)
     case RTD_BLACKHOLE:
     case RTD_UNREACHABLE:
     case RTD_PROHIBIT:
+    case RTD_MULTIPATH:
       break;
     default:
       return 0;
@@ -497,6 +604,15 @@ krt_capable(rte *e)
   return 1;
 }
 
+static inline int
+nh_bufsize(struct mpnh *nh)
+{
+  int rv = 0;
+  for (; nh != NULL; nh = nh->next)
+    rv += RTNH_SIZE;
+  return rv;
+}
+
 static void
 nl_send_route(struct krt_proto *p, rte *e, int new)
 {
@@ -505,7 +621,7 @@ nl_send_route(struct krt_proto *p, rte *e, int new)
   struct {
     struct nlmsghdr h;
     struct rtmsg r;
-    char buf[128];
+    char buf[64 + nh_bufsize(a->nexthops)];
   } r;
 
   DBG("nl_send_route(%I/%d,new=%d)\n", net->n.prefix, net->n.pxlen, new);
@@ -549,6 +665,10 @@ nl_send_route(struct krt_proto *p, rte *e, int new)
     case RTD_PROHIBIT:
       r.r.rtm_type = RTN_PROHIBIT;
       break;
+    case RTD_MULTIPATH:
+      r.r.rtm_type = RTN_UNICAST;
+      nl_add_multipath(&r.h, sizeof(r), a->nexthops);
+      break;
     default:
       bug("krt_capable inconsistent with nl_send_route");
     }
@@ -566,6 +686,7 @@ krt_set_notify(struct krt_proto *p, net *n UNUSED, rte *new, rte *old)
     nl_send_route(p, new, 1);
 }
 
+
 #define SKIP(ARG...) do { DBG("KRT: Ignoring route - " ARG); return; } while(0)
 
 static void
@@ -671,6 +792,21 @@ nl_parse_route(struct nlmsghdr *h, int scan)
   switch (i->rtm_type)
     {
     case RTN_UNICAST:
+
+      if (a[RTA_MULTIPATH])
+	{
+	  ra.dest = RTD_MULTIPATH;
+	  ra.nexthops = nl_parse_multipath(p, a[RTA_MULTIPATH]);
+	  if (!ra.nexthops)
+	    {
+	      log(L_ERR "KRT: Received strange multipath route %I/%d",
+		  net->n.prefix, net->n.pxlen);
+	      return;
+	    }
+	    
+	  break;
+	}
+
       ra.iface = if_find_by_index(oif);
       if (!ra.iface)
 	{