From d47c3d64b2733baea756f1bb37ef09f10d7f9644 Mon Sep 17 00:00:00 2001
From: Jan Moskyto Matejka <mq@ucw.cz>
Date: Tue, 9 Aug 2016 14:47:51 +0200
Subject: [PATCH] MPLS: Label stack concatenation for recursive routes

---
 nest/route.h          |  3 ++-
 nest/rt-table.c       | 49 ++++++++++++++++++++++++++++++++++---------
 proto/static/config.Y |  7 ++++++-
 proto/static/static.c |  6 +++++-
 4 files changed, 52 insertions(+), 13 deletions(-)

diff --git a/nest/route.h b/nest/route.h
index a2fadf62..eb98b609 100644
--- a/nest/route.h
+++ b/nest/route.h
@@ -338,7 +338,8 @@ struct nexthop {
   struct iface *iface;			/* Outgoing interface */
   struct nexthop *next;
   byte weight;
-  byte labels;				/* Number of labels appended */
+  byte labels_append;			/* Number of labels before hostentry was applied */
+  byte labels;				/* Number of labels prepended */
   u32 label[0];
 };
 
diff --git a/nest/rt-table.c b/nest/rt-table.c
index 73b838be..ef402f28 100644
--- a/nest/rt-table.c
+++ b/nest/rt-table.c
@@ -1768,27 +1768,56 @@ static inline void
 rta_apply_hostentry(rta *a, struct hostentry *he)
 {
   a->hostentry = he;
-
-  a->nh.gw = ipa_nonzero(he->nh->gw) ? he->nh->gw : he->link;
-  a->nh.iface = he->nh->iface;
-  a->nh.weight = he->nh->weight;
-  a->nh.next = he->nh->next;
   
   a->dest = he->dest;
   a->igp_metric = he->igp_metric;
+
+  if (a->nh.labels_append == 0)
+  {
+    a->nh = *(he->nh);
+    a->nh.labels_append = 0;
+    return;
+  }
+
+  int labels_append = a->nh.labels_append;
+  u32 label_stack[MPLS_MAX_LABEL_STACK];
+  memcpy(label_stack, a->nh.label, labels_append * sizeof(u32));
+
+  struct nexthop *nhp = NULL;
+  for (struct nexthop *nh = he->nh; nh; nh = nh->next)
+  {
+    nhp = nhp ? (nhp->next = lp_alloc(rte_update_pool, NEXTHOP_MAX_SIZE)) : &(a->nh);
+    nhp->gw = ipa_nonzero(nh->gw) ? nh->gw : he->link;
+    nhp->iface = nh->iface; /* FIXME: This is at least strange, if not utter nonsense. */
+    nhp->weight = nh->weight;
+    nhp->labels = nh->labels + labels_append;
+    nhp->labels_append = labels_append;
+    if (nhp->labels <= MPLS_MAX_LABEL_STACK)
+    {
+      memcpy(nhp->label, nh->label, nh->labels * sizeof(u32));
+      memcpy(&(nhp->label[nh->labels]), label_stack, labels_append * sizeof(u32));
+    }
+    else
+    {
+      log(L_WARN "Sum of label stack sizes %d + %d = %d exceedes allowed maximum (%d)",
+	  nh->labels, labels_append, nhp->labels, MPLS_MAX_LABEL_STACK);
+      a->dest = RTD_UNREACHABLE;
+      break;
+    }
+  }
 }
 
 static inline rte *
 rt_next_hop_update_rte(rtable *tab UNUSED, rte *old)
 {
-  rta a;
-  memcpy(&a, old->attrs, rta_size(old->attrs));
-  rta_apply_hostentry(&a, old->attrs->hostentry);
-  a.aflags = 0;
+  rta *ap = alloca(RTA_MAX_SIZE);
+  memcpy(ap, old->attrs, rta_size(old->attrs));
+  rta_apply_hostentry(ap, old->attrs->hostentry);
+  ap->aflags = 0;
 
   rte *e = sl_alloc(rte_slab);
   memcpy(e, old, sizeof(rte));
-  e->attrs = rta_lookup(&a);
+  e->attrs = rta_lookup(ap);
 
   return e;
 }
diff --git a/proto/static/config.Y b/proto/static/config.Y
index a461bd27..2fb54448 100644
--- a/proto/static/config.Y
+++ b/proto/static/config.Y
@@ -105,7 +105,12 @@ stat_route:
       this_srt->dest = RTDX_RECURSIVE;
       this_srt->via = $3;
    }
-
+ | stat_route0 RECURSIVE ipa MPLS label_stack {
+      this_srt->dest = RTDX_RECURSIVE;
+      this_srt->via = $3;
+      this_srt->label_count = $5[0];
+      this_srt->label_stack = &($5[1]);
+   }
  | stat_route0 DROP		{ this_srt->dest = RTD_BLACKHOLE; }
  | stat_route0 REJECT		{ this_srt->dest = RTD_UNREACHABLE; }
  | stat_route0 BLACKHOLE	{ this_srt->dest = RTD_BLACKHOLE; }
diff --git a/proto/static/static.c b/proto/static/static.c
index 878248c1..3e03708c 100644
--- a/proto/static/static.c
+++ b/proto/static/static.c
@@ -128,7 +128,11 @@ drop:
     r->state |= STS_INSTALLED;
   
   if (r->dest == RTDX_RECURSIVE)
-    rta_set_recursive_next_hop(p->main_channel->table, ap, p_igp_table(p), r->via, IPA_NONE);
+    {
+      ap->nh.labels_append = ap->nh.labels = r->label_count;
+      memcpy(ap->nh.label, r->label_stack, r->label_count * sizeof(u32));
+      rta_set_recursive_next_hop(p->main_channel->table, ap, p_igp_table(p), r->via, IPA_NONE);
+    }
 
   /* We skip rta_lookup() here */