zebra: Add support for static encap mpls labels

We were ignoring mpls labels encapped with static routes.
Added support for single and multipath labels.

Signed-off-by: Stephen Worley <sworley@cumulusnetworks.com>
Signed-off-by: Donald Sharp <sharpd@cumulusnetworks.com>

3 files changed, 73 insertions, 5 deletions

diff --git a/zebra/kernel_netlink.c b/zebra/kernel_netlink.c
index ef7c26c9d..545b4d9d6 100644
--- a/zebra/kernel_netlink.c
+++ b/zebra/kernel_netlink.c
@@ -467,6 +467,18 @@ void netlink_parse_rtattr(struct rtattr **tb, int max, struct rtattr *rta,
 	}
 }
 
+/**
+ * netlink_parse_rtattr_nested() - Parses a nested route attribute
+ * @tb:         Pointer to array for storing rtattr in.
+ * @max:        Max number to store.
+ * @rta:        Pointer to rtattr to look for nested items in.
+ */
+void netlink_parse_rtattr_nested(struct rtattr **tb, int max,
+				 struct rtattr *rta)
+{
+	netlink_parse_rtattr(tb, max, RTA_DATA(rta), RTA_PAYLOAD(rta));
+}
+
 int addattr_l(struct nlmsghdr *n, unsigned int maxlen, int type,
 	      const void *data, unsigned int alen)
 {
diff --git a/zebra/kernel_netlink.h b/zebra/kernel_netlink.h
index 65df15599..af0cc83f4 100644
--- a/zebra/kernel_netlink.h
+++ b/zebra/kernel_netlink.h
@@ -28,6 +28,8 @@
 
 extern void netlink_parse_rtattr(struct rtattr **tb, int max,
 				 struct rtattr *rta, int len);
+extern void netlink_parse_rtattr_nested(struct rtattr **tb, int max,
+					struct rtattr *rta);
 extern int addattr_l(struct nlmsghdr *n, unsigned int maxlen, int type,
 		     const void *data, unsigned int alen);
 extern int rta_addattr_l(struct rtattr *rta, unsigned int maxlen, int type,
diff --git a/zebra/rt_netlink.c b/zebra/rt_netlink.c
index 6a258e685..cab3f43b1 100644
--- a/zebra/rt_netlink.c
+++ b/zebra/rt_netlink.c
@@ -246,6 +246,33 @@ static vrf_id_t vrf_lookup_by_table(uint32_t table_id, ns_id_t ns_id)
 	return VRF_DEFAULT;
 }
 
+/**
+ * @parse_encap_mpls() - Parses encapsulated mpls attributes
+ * @tb:         Pointer to rtattr to look for nested items in.
+ * @labels:     Pointer to store labels in.
+ *
+ * Return:      Number of mpls labels found.
+ */
+static int parse_encap_mpls(struct rtattr *tb, mpls_label_t *labels)
+{
+	struct rtattr *tb_encap[MPLS_IPTUNNEL_MAX + 1] = {0};
+	mpls_lse_t *lses = NULL;
+	int num_labels = 0;
+	uint32_t ttl = 0;
+	uint32_t bos = 0;
+	uint32_t exp = 0;
+	mpls_label_t label = 0;
+
+	netlink_parse_rtattr_nested(tb_encap, MPLS_IPTUNNEL_MAX, tb);
+	lses = (mpls_lse_t *)RTA_DATA(tb_encap[MPLS_IPTUNNEL_DST]);
+	while (!bos && num_labels < MPLS_MAX_LABELS) {
+		mpls_lse_decode(lses[num_labels], &label, &ttl, &exp, &bos);
+		labels[num_labels++] = label;
+	}
+
+	return num_labels;
+}
+
 /* Looking up routing table by netlink interface. */
 static int netlink_route_change_read_unicast(struct nlmsghdr *h, ns_id_t ns_id,
 					     int startup)
@@ -274,6 +301,10 @@ static int netlink_route_change_read_unicast(struct nlmsghdr *h, ns_id_t ns_id,
 	void *src = NULL;     /* IPv6 srcdest   source prefix */
 	enum blackhole_type bh_type = BLACKHOLE_UNSPEC;
 
+	/* MPLS labels */
+	mpls_label_t labels[MPLS_MAX_LABELS] = {0};
+	int num_labels = 0;
+
 	rtm = NLMSG_DATA(h);
 
 	if (startup && h->nlmsg_type != RTM_NEWROUTE)
@@ -508,6 +539,17 @@ static int netlink_route_change_read_unicast(struct nlmsghdr *h, ns_id_t ns_id,
 			}
 			nh.vrf_id = nh_vrf_id;
 
+			if (tb[RTA_ENCAP] && tb[RTA_ENCAP_TYPE]
+			    && *(uint16_t *)RTA_DATA(tb[RTA_ENCAP_TYPE])
+				       == LWTUNNEL_ENCAP_MPLS) {
+				num_labels =
+					parse_encap_mpls(tb[RTA_ENCAP], labels);
+			}
+
+			if (num_labels)
+				nexthop_add_labels(&nh, ZEBRA_LSP_STATIC,
+						   num_labels, labels);
+
 			rib_add(afi, SAFI_UNICAST, vrf_id, proto, 0, flags, &p,
 				&src_p, &nh, table, metric, mtu, distance, tag);
 		} else {
@@ -532,6 +574,7 @@ static int netlink_route_change_read_unicast(struct nlmsghdr *h, ns_id_t ns_id,
 			re->tag = tag;
 
 			for (;;) {
+				struct nexthop *nh = NULL;
 				vrf_id_t nh_vrf_id;
 				if (len < (int)sizeof(*rtnh)
 				    || rtnh->rtnh_len > len)
@@ -569,35 +612,46 @@ static int netlink_route_change_read_unicast(struct nlmsghdr *h, ns_id_t ns_id,
 					if (tb[RTA_GATEWAY])
 						gate = RTA_DATA(
 							tb[RTA_GATEWAY]);
+					if (tb[RTA_ENCAP] && tb[RTA_ENCAP_TYPE]
+					    && *(uint16_t *)RTA_DATA(
+						       tb[RTA_ENCAP_TYPE])
+						       == LWTUNNEL_ENCAP_MPLS) {
+						num_labels = parse_encap_mpls(
+							tb[RTA_ENCAP], labels);
+					}
 				}
 
 				if (gate) {
 					if (rtm->rtm_family == AF_INET) {
 						if (index)
-							route_entry_nexthop_ipv4_ifindex_add(
+							nh = route_entry_nexthop_ipv4_ifindex_add(
 								re, gate,
 								prefsrc, index,
 								nh_vrf_id);
 						else
-							route_entry_nexthop_ipv4_add(
+							nh = route_entry_nexthop_ipv4_add(
 								re, gate,
 								prefsrc,
 								nh_vrf_id);
 					} else if (rtm->rtm_family
 						   == AF_INET6) {
 						if (index)
-							route_entry_nexthop_ipv6_ifindex_add(
+							nh = route_entry_nexthop_ipv6_ifindex_add(
 								re, gate, index,
 								nh_vrf_id);
 						else
-							route_entry_nexthop_ipv6_add(
+							nh = route_entry_nexthop_ipv6_add(
 								re, gate,
 								nh_vrf_id);
 					}
 				} else
-					route_entry_nexthop_ifindex_add(
+					nh = route_entry_nexthop_ifindex_add(
 						re, index, nh_vrf_id);
 
+				if (nh && num_labels)
+					nexthop_add_labels(nh, ZEBRA_LSP_STATIC,
+							   num_labels, labels);
+
 				if (rtnh->rtnh_len == 0)
 					break;