bgpd: encap: add encap SAFI (RFC5512)

Adds RFC5512 and Encapsulation Attribute.

Signed-off-by: Lou Berger <lberger@labn.net>
diff --git a/bgpd/bgp_attr.c b/bgpd/bgp_attr.c
index b019347..5dc118b 100644
--- a/bgpd/bgp_attr.c
+++ b/bgpd/bgp_attr.c
@@ -1891,9 +1891,11 @@
     uint16_t	sublength;
     struct bgp_attr_encap_subtlv *tlv;
 
-    subtype = stream_getw (BGP_INPUT (peer));
-    sublength = stream_getw (BGP_INPUT (peer));
-    length -= 4;
+    if (BGP_ATTR_ENCAP == type) {
+        subtype   = stream_getc (BGP_INPUT (peer));
+        sublength = stream_getc (BGP_INPUT (peer));
+        length   -= 2;
+    }
 
     if (sublength > length) {
       zlog (peer->log, LOG_ERR,
@@ -2423,6 +2425,10 @@
 	  stream_putl (s, 0);
 	  stream_put (s, &attr->extra->mp_nexthop_global_in, 4);
 	  break;
+	case SAFI_ENCAP:
+	  stream_putc (s, 4);
+	  stream_put (s, &attr->extra->mp_nexthop_global_in, 4);
+	  break;
 	case SAFI_UNICAST:      /* invalid for IPv4 */
 	default:
 	  break;
@@ -2465,6 +2471,11 @@
           }
         }
 	break;
+	case SAFI_ENCAP:
+          assert (attr->extra);
+          stream_putc (s, 16);
+	  stream_put (s, &attr->extra->mp_nexthop_global, 16);
+	  break;
       default:
 	break;
       }
@@ -2517,6 +2528,7 @@
     uint8_t		attrtype)
 {
     unsigned int			attrlenfield = 0;
+    unsigned int			attrhdrlen   = 0;
     struct bgp_attr_encap_subtlv	*subtlvs;
     struct bgp_attr_encap_subtlv	*st;
     const char				*attrname;
@@ -2536,6 +2548,7 @@
 	     * V = concatenated subtlvs.
 	     */
 	    attrlenfield = 2 + 2;	/* T + L */
+            attrhdrlen   = 1 + 1;	/* subTLV T + L */
 	    break;
 
 	default:
@@ -2543,15 +2556,15 @@
     }
 
 
+    /* if no tlvs, don't make attr */
+    if (subtlvs == NULL)
+	return;
+
     /* compute attr length */
     for (st = subtlvs; st; st = st->next) {
-	attrlenfield += (4 + st->length);
+	attrlenfield += (attrhdrlen + st->length);
     }
 
-    /* if no tlvs, don't make attr */
-    if (!attrlenfield)
-	return;
-
     if (attrlenfield > 0xffff) {
 	zlog (peer->log, LOG_ERR,
 	    "%s attribute is too long (length=%d), can't send it",
@@ -2581,8 +2594,10 @@
 
     /* write each sub-tlv */
     for (st = subtlvs; st; st = st->next) {
-	stream_putw (s, st->type);
-	stream_putw (s, st->length);
+        if (attrtype == BGP_ATTR_ENCAP) {
+            stream_putc (s, st->type);
+            stream_putc (s, st->length);
+        }
 	stream_put (s, st->value, st->length);
     }
 }
@@ -2944,7 +2959,14 @@
       stream_putl (s, attr->extra->aggregator_as);
       stream_put_ipv4 (s, attr->extra->aggregator_addr.s_addr);
     }
-  
+
+  if ((afi == AFI_IP || afi == AFI_IP6) &&
+      (safi == SAFI_ENCAP || safi == SAFI_MPLS_VPN))
+    {
+	/* Tunnel Encap attribute */
+	bgp_packet_mpattr_tea(bgp, peer, s, attr, BGP_ATTR_ENCAP);
+    }
+
   /* Unknown transit attribute. */
   if (attr->extra && attr->extra->transit)
     stream_put (s, attr->extra->transit->val, attr->extra->transit->length);