]> git.proxmox.com Git - mirror_frr.git/blobdiff - bgpd/bgp_open.c
Added json formating support to show-...-neighbors-... bgp commands.
[mirror_frr.git] / bgpd / bgp_open.c
index a3e86b06c8f2691c9ce91294be1acdd815262fce..a530542a49719d49a839b9a70d2a1c6345b61872 100644 (file)
@@ -26,14 +26,19 @@ Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 #include "thread.h"
 #include "log.h"
 #include "command.h"
+#include "memory.h"
+#include "queue.h"
 
+#include "lib/json.h"
 #include "bgpd/bgpd.h"
 #include "bgpd/bgp_attr.h"
 #include "bgpd/bgp_debug.h"
 #include "bgpd/bgp_fsm.h"
 #include "bgpd/bgp_packet.h"
 #include "bgpd/bgp_open.h"
-\f
+#include "bgpd/bgp_aspath.h"
+#include "bgpd/bgp_vty.h"
+
 /* BGP-4 Multiprotocol Extentions lead us to the complex world. We can
    negotiate remote peer supports extentions or not. But if
    remote-peer doesn't supports negotiation process itself.  We would
@@ -45,242 +50,322 @@ Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
    inforation at each peer. */
 
 void
-bgp_capability_vty_out (struct vty *vty, struct peer *peer)
+bgp_capability_vty_out (struct vty *vty, struct peer *peer, u_char use_json, json_object *json_neigh)
 {
-  u_char *pnt;
-  u_char *end;
-  struct capability cap;
+  char *pnt;
+  char *end;
+  struct capability_mp_data mpc;
+  struct capability_header *hdr;
+  json_object *json_cap = NULL;
+
+  if (use_json)
+    json_cap = json_object_new_object();
 
   pnt = peer->notify.data;
   end = pnt + peer->notify.length;
-
+  
   while (pnt < end)
     {
-      memcpy(&cap, pnt, sizeof(struct capability));
-
-      if (pnt + 2 > end)
+      if (pnt + sizeof (struct capability_mp_data) + 2 > end)
        return;
-      if (pnt + (cap.length + 2) > end)
+      
+      hdr = (struct capability_header *)pnt;
+      if (pnt + hdr->length + 2 > end)
        return;
 
-      if (cap.code == CAPABILITY_CODE_MP)
-       {
-         vty_out (vty, "  Capability error for: Multi protocol ");
+      memcpy (&mpc, pnt + 2, sizeof(struct capability_mp_data));
 
-         switch (ntohs (cap.mpc.afi))
-           {
-           case AFI_IP:
-             vty_out (vty, "AFI IPv4, ");
-             break;
-           case AFI_IP6:
-             vty_out (vty, "AFI IPv6, ");
-             break;
-           default:
-             vty_out (vty, "AFI Unknown %d, ", ntohs (cap.mpc.afi));
-             break;
-           }
-         switch (cap.mpc.safi)
-           {
-           case SAFI_UNICAST:
-             vty_out (vty, "SAFI Unicast");
-             break;
-           case SAFI_MULTICAST:
-             vty_out (vty, "SAFI Multicast");
-             break;
-           case SAFI_UNICAST_MULTICAST:
-             vty_out (vty, "SAFI Unicast Multicast");
-             break;
-           case BGP_SAFI_VPNV4:
-             vty_out (vty, "SAFI MPLS-VPN");
-             break;
-           default:
-             vty_out (vty, "SAFI Unknown %d ", cap.mpc.safi);
-             break;
-           }
-         vty_out (vty, "%s", VTY_NEWLINE);
-       }
-      else if (cap.code >= 128)
-       vty_out (vty, "  Capability error: vendor specific capability code %d",
-                cap.code);
+      if (hdr->code == CAPABILITY_CODE_MP)
+       {
+          if (use_json)
+            {
+              switch (ntohs (mpc.afi))
+                {
+                  case AFI_IP:
+                    json_object_string_add(json_cap, "capabilityErrorMultiProtocolAfi", "IPv4");
+                  break;
+                  case AFI_IP6:
+                    json_object_string_add(json_cap, "capabilityErrorMultiProtocolAfi", "IPv6");
+                  break;
+                  default:
+                    json_object_int_add(json_cap, "capabilityErrorMultiProtocolAfiUnknown", ntohs (mpc.afi));
+                  break;
+                }
+              switch (mpc.safi)
+                {
+                  case SAFI_UNICAST:
+                    json_object_string_add(json_cap, "capabilityErrorMultiProtocolSafi", "unicast");
+                  break;
+                  case SAFI_MULTICAST:
+                    json_object_string_add(json_cap, "capabilityErrorMultiProtocolSafi", "multicast");
+                  break;
+                  case SAFI_MPLS_LABELED_VPN:
+                    json_object_string_add(json_cap, "capabilityErrorMultiProtocolSafi", "MPLS-labeled VPN");
+                  break;
+                  default:
+                    json_object_int_add(json_cap, "capabilityErrorMultiProtocolSafiUnknown", mpc.safi);
+                  break;
+                }
+            }
+          else
+            {
+              vty_out (vty, "  Capability error for: Multi protocol ");
+              switch (ntohs (mpc.afi))
+                {
+                  case AFI_IP:
+                    vty_out (vty, "AFI IPv4, ");
+                  break;
+                  case AFI_IP6:
+                    vty_out (vty, "AFI IPv6, ");
+                  break;
+                  default:
+                    vty_out (vty, "AFI Unknown %d, ", ntohs (mpc.afi));
+                  break;
+                }
+              switch (mpc.safi)
+                {
+                  case SAFI_UNICAST:
+                    vty_out (vty, "SAFI Unicast");
+                  break;
+                  case SAFI_MULTICAST:
+                    vty_out (vty, "SAFI Multicast");
+                  break;
+                 case SAFI_MPLS_LABELED_VPN:
+                    vty_out (vty, "SAFI MPLS-labeled VPN");
+                  break;
+                  default:
+                    vty_out (vty, "SAFI Unknown %d ", mpc.safi);
+                  break;
+                }
+              vty_out (vty, "%s", VTY_NEWLINE);
+            }
+        }
+      else if (hdr->code >= 128)
+        {
+          if (use_json)
+            json_object_int_add(json_cap, "capabilityErrorVendorSpecificCapabilityCode", hdr->code);
+          else
+            vty_out (vty, "  Capability error: vendor specific capability code %d",
+                     hdr->code);
+        }
       else
-       vty_out (vty, "  Capability error: unknown capability code %d", 
-                cap.code);
-
-      pnt += cap.length + 2;
+        {
+          if (use_json)
+            json_object_int_add(json_cap, "capabilityErrorUnknownCapabilityCode", hdr->code);
+          else
+            vty_out (vty, "  Capability error: unknown capability code %d",
+                     hdr->code);
+        }
+      pnt += hdr->length + 2;
     }
+  if (use_json)
+    json_object_object_add(json_neigh, "capabilityErrors", json_cap);
+}
+
+static void 
+bgp_capability_mp_data (struct stream *s, struct capability_mp_data *mpc)
+{
+  mpc->afi = stream_getw (s);
+  mpc->reserved = stream_getc (s);
+  mpc->safi = stream_getc (s);
 }
 
-/* Set negotiated capability value. */
 int
-bgp_capability_mp (struct peer *peer, struct capability *cap)
+bgp_afi_safi_valid_indices (afi_t afi, safi_t *safi)
 {
-  if (ntohs (cap->mpc.afi) == AFI_IP)
+  switch (afi)
     {
-      if (cap->mpc.safi == SAFI_UNICAST)
-       {
-         peer->afc_recv[AFI_IP][SAFI_UNICAST] = 1;
-
-         if (peer->afc[AFI_IP][SAFI_UNICAST])
-           peer->afc_nego[AFI_IP][SAFI_UNICAST] = 1;
-         else
-           return -1;
-       }
-      else if (cap->mpc.safi == SAFI_MULTICAST) 
-       {
-         peer->afc_recv[AFI_IP][SAFI_MULTICAST] = 1;
-
-         if (peer->afc[AFI_IP][SAFI_MULTICAST])
-           peer->afc_nego[AFI_IP][SAFI_MULTICAST] = 1;
-         else
-           return -1;
-       }
-      else if (cap->mpc.safi == BGP_SAFI_VPNV4)
-       {
-         peer->afc_recv[AFI_IP][SAFI_MPLS_VPN] = 1;
-
-         if (peer->afc[AFI_IP][SAFI_MPLS_VPN])
-           peer->afc_nego[AFI_IP][SAFI_MPLS_VPN] = 1;
-         else
-           return -1;
-       }
-      else
-       return -1;
-    }
+      case AFI_IP:
 #ifdef HAVE_IPV6
-  else if (ntohs (cap->mpc.afi) == AFI_IP6)
-    {
-      if (cap->mpc.safi == SAFI_UNICAST)
-       {
-         peer->afc_recv[AFI_IP6][SAFI_UNICAST] = 1;
-
-         if (peer->afc[AFI_IP6][SAFI_UNICAST])
-           peer->afc_nego[AFI_IP6][SAFI_UNICAST] = 1;
-         else
-           return -1;
-       }
-      else if (cap->mpc.safi == SAFI_MULTICAST)
-       {
-         peer->afc_recv[AFI_IP6][SAFI_MULTICAST] = 1;
-
-         if (peer->afc[AFI_IP6][SAFI_MULTICAST])
-           peer->afc_nego[AFI_IP6][SAFI_MULTICAST] = 1;
-         else
-           return -1;
-       }
-      else
-       return -1;
+      case AFI_IP6:
+#endif
+        switch (*safi)
+          {
+            /* BGP MPLS-labeled VPN SAFI isn't contigious with others, remap */
+            case SAFI_MPLS_LABELED_VPN:
+              *safi = SAFI_MPLS_VPN;
+            case SAFI_UNICAST:
+            case SAFI_MULTICAST:
+            case SAFI_MPLS_VPN:
+              return 1;
+          }
     }
-#endif /* HAVE_IPV6 */
-  else
+  zlog_debug ("unknown afi/safi (%u/%u)", afi, *safi);
+  
+  return 0;
+}
+
+/* Set negotiated capability value. */
+static int
+bgp_capability_mp (struct peer *peer, struct capability_header *hdr)
+{
+  struct capability_mp_data mpc;
+  struct stream *s = BGP_INPUT (peer);
+  
+  /* Verify length is 4 */
+  if (hdr->length != 4)
     {
-      /* Unknown Address Family. */
+      zlog_warn("MP Cap: Received invalid length %d, non-multiple of 4",
+               hdr->length);
       return -1;
     }
 
+  bgp_capability_mp_data (s, &mpc);
+  
+  if (bgp_debug_neighbor_events(peer))
+    zlog_debug ("%s OPEN has MP_EXT CAP for afi/safi: %u/%u",
+               peer->host, mpc.afi, mpc.safi);
+  
+  if (!bgp_afi_safi_valid_indices (mpc.afi, &mpc.safi))
+    return -1;
+   
+  /* Now safi remapped, and afi/safi are valid array indices */
+  peer->afc_recv[mpc.afi][mpc.safi] = 1;
+  
+  if (peer->afc[mpc.afi][mpc.safi])
+    peer->afc_nego[mpc.afi][mpc.safi] = 1;
+  else 
+    return -1;
+
   return 0;
 }
 
-void
+static void
 bgp_capability_orf_not_support (struct peer *peer, afi_t afi, safi_t safi,
                                u_char type, u_char mode)
 {
-  if (BGP_DEBUG (normal, NORMAL))
-    zlog_info ("%s Addr-family %d/%d has ORF type/mode %d/%d not supported",
+  if (bgp_debug_neighbor_events(peer))
+    zlog_debug ("%s Addr-family %d/%d has ORF type/mode %d/%d not supported",
               peer->host, afi, safi, type, mode);
 }
 
-int
-bgp_capability_orf (struct peer *peer, struct capability *cap,
-                   u_char *pnt)
+static const struct message orf_type_str[] =
 {
-  afi_t afi = ntohs(cap->mpc.afi);
-  safi_t safi = cap->mpc.safi;
-  u_char number_of_orfs;
+  { ORF_TYPE_PREFIX,           "Prefixlist"            },
+  { ORF_TYPE_PREFIX_OLD,       "Prefixlist (old)"      },
+};
+static const int orf_type_str_max = array_size(orf_type_str);
+
+static const struct message orf_mode_str[] =
+{
+  { ORF_MODE_RECEIVE,  "Receive"       },
+  { ORF_MODE_SEND,     "Send"          },
+  { ORF_MODE_BOTH,     "Both"          },
+};
+static const int orf_mode_str_max = array_size(orf_mode_str);
+
+static int
+bgp_capability_orf_entry (struct peer *peer, struct capability_header *hdr)
+{
+  struct stream *s = BGP_INPUT (peer);
+  struct capability_orf_entry entry;
+  afi_t afi;
+  safi_t safi;
   u_char type;
   u_char mode;
   u_int16_t sm_cap = 0; /* capability send-mode receive */
   u_int16_t rm_cap = 0; /* capability receive-mode receive */ 
   int i;
 
-  /* Check length. */
-  if (cap->length < 7)
-    {
-      zlog_info ("%s ORF Capability length error %d",
-                peer->host, cap->length);
-                bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
-      return -1;
-    }
-
-  if (BGP_DEBUG (normal, NORMAL))
-    zlog_info ("%s OPEN has ORF CAP(%s) for afi/safi: %u/%u",
-              peer->host, (cap->code == CAPABILITY_CODE_ORF ?
-                       "new" : "old"), afi, safi);
+  /* ORF Entry header */
+  bgp_capability_mp_data (s, &entry.mpc);
+  entry.num = stream_getc (s);
+  afi = entry.mpc.afi;
+  safi = entry.mpc.safi;
+  
+  if (bgp_debug_neighbor_events(peer))
+    zlog_debug ("%s ORF Cap entry for afi/safi: %u/%u",
+               peer->host, entry.mpc.afi, entry.mpc.safi);
 
   /* Check AFI and SAFI. */
-  if ((afi != AFI_IP && afi != AFI_IP6)
-      || (safi != SAFI_UNICAST && safi != SAFI_MULTICAST
-         && safi != BGP_SAFI_VPNV4))
+  if (!bgp_afi_safi_valid_indices (entry.mpc.afi, &safi))
+    {
+      zlog_info ("%s Addr-family %d/%d not supported."
+                 " Ignoring the ORF capability",
+                 peer->host, entry.mpc.afi, entry.mpc.safi);
+      return 0;
+    }
+  
+  /* validate number field */
+  if (sizeof (struct capability_orf_entry) + (entry.num * 2) > hdr->length)
     {
-      zlog_info ("%s Addr-family %d/%d not supported. Ignoring the ORF capability",
-                 peer->host, afi, safi);
+      zlog_info ("%s ORF Capability entry length error,"
+                 " Cap length %u, num %u",
+                 peer->host, hdr->length, entry.num);
+      bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
       return -1;
     }
 
-  number_of_orfs = *pnt++;
-
-  for (i = 0 ; i < number_of_orfs ; i++)
+  for (i = 0 ; i < entry.num ; i++)
     {
-      type = *pnt++;
-      mode = *pnt++;
-
+      type = stream_getc(s);
+      mode = stream_getc(s);
+      
       /* ORF Mode error check */
-      if (mode != ORF_MODE_BOTH && mode != ORF_MODE_SEND
-         && mode != ORF_MODE_RECEIVE)
-       {
-         bgp_capability_orf_not_support (peer, afi, safi, type, mode);
-         continue;
+      switch (mode)
+        {
+          case ORF_MODE_BOTH:
+          case ORF_MODE_SEND:
+          case ORF_MODE_RECEIVE:
+            break;
+          default:
+           bgp_capability_orf_not_support (peer, afi, safi, type, mode);
+           continue;
        }
-
-      /* ORF Type and afi/safi error check */
-      if (cap->code == CAPABILITY_CODE_ORF)
+      /* ORF Type and afi/safi error checks */
+      /* capcode versus type */
+      switch (hdr->code)
+        {
+          case CAPABILITY_CODE_ORF:
+            switch (type)
+              {
+                case ORF_TYPE_PREFIX:
+                  break;
+                default:
+                  bgp_capability_orf_not_support (peer, afi, safi, type, mode);
+                  continue;
+              }
+            break;
+          case CAPABILITY_CODE_ORF_OLD:
+            switch (type)
+              {
+                case ORF_TYPE_PREFIX_OLD:
+                  break;
+                default:
+                  bgp_capability_orf_not_support (peer, afi, safi, type, mode);
+                  continue;
+              }
+            break;
+          default:
+            bgp_capability_orf_not_support (peer, afi, safi, type, mode);
+            continue;
+        }
+                
+      /* AFI vs SAFI */
+      if (!((afi == AFI_IP && safi == SAFI_UNICAST)
+            || (afi == AFI_IP && safi == SAFI_MULTICAST)
+            || (afi == AFI_IP6 && safi == SAFI_UNICAST)))
+        {
+          bgp_capability_orf_not_support (peer, afi, safi, type, mode);
+          continue;
+        }
+      
+      if (bgp_debug_neighbor_events(peer))
+        zlog_debug ("%s OPEN has %s ORF capability"
+                    " as %s for afi/safi: %d/%d",
+                    peer->host, LOOKUP (orf_type_str, type),
+                    LOOKUP (orf_mode_str, mode),
+                    entry.mpc.afi, safi);
+
+      if (hdr->code == CAPABILITY_CODE_ORF)
        {
-         if (type == ORF_TYPE_PREFIX &&
-             ((afi == AFI_IP && safi == SAFI_UNICAST)
-               || (afi == AFI_IP && safi == SAFI_MULTICAST)
-               || (afi == AFI_IP6 && safi == SAFI_UNICAST)))
-           {
-             sm_cap = PEER_CAP_ORF_PREFIX_SM_RCV;
-             rm_cap = PEER_CAP_ORF_PREFIX_RM_RCV;
-             if (BGP_DEBUG (normal, NORMAL))
-               zlog_info ("%s OPEN has Prefixlist ORF(%d) capability as %s for afi/safi: %d/%d",
-                          peer->host, ORF_TYPE_PREFIX, (mode == ORF_MODE_SEND ? "SEND" :
-                          mode == ORF_MODE_RECEIVE ? "RECEIVE" : "BOTH") , afi, safi);
-           }
-         else
-           {
-             bgp_capability_orf_not_support (peer, afi, safi, type, mode);
-             continue;
-           }
+          sm_cap = PEER_CAP_ORF_PREFIX_SM_RCV;
+          rm_cap = PEER_CAP_ORF_PREFIX_RM_RCV;
        }
-      else if (cap->code == CAPABILITY_CODE_ORF_OLD)
+      else if (hdr->code == CAPABILITY_CODE_ORF_OLD)
        {
-         if (type == ORF_TYPE_PREFIX_OLD &&
-             ((afi == AFI_IP && safi == SAFI_UNICAST)
-               || (afi == AFI_IP && safi == SAFI_MULTICAST)
-               || (afi == AFI_IP6 && safi == SAFI_UNICAST)))
-           {
-             sm_cap = PEER_CAP_ORF_PREFIX_SM_OLD_RCV;
-             rm_cap = PEER_CAP_ORF_PREFIX_RM_OLD_RCV;
-             if (BGP_DEBUG (normal, NORMAL))
-               zlog_info ("%s OPEN has Prefixlist ORF(%d) capability as %s for afi/safi: %d/%d",
-                          peer->host, ORF_TYPE_PREFIX_OLD, (mode == ORF_MODE_SEND ? "SEND" :
-                          mode == ORF_MODE_RECEIVE ? "RECEIVE" : "BOTH") , afi, safi);
-           }
-         else
-           {
-             bgp_capability_orf_not_support (peer, afi, safi, type, mode);
-             continue;
-           }
+          sm_cap = PEER_CAP_ORF_PREFIX_SM_OLD_RCV;
+          rm_cap = PEER_CAP_ORF_PREFIX_RM_OLD_RCV;
        }
       else
        {
@@ -305,135 +390,411 @@ bgp_capability_orf (struct peer *peer, struct capability *cap,
   return 0;
 }
 
-/* Parse given capability. */
-int
-bgp_capability_parse (struct peer *peer, u_char *pnt, u_char length,
-                     u_char **error)
+static int
+bgp_capability_restart (struct peer *peer, struct capability_header *caphdr)
 {
-  int ret;
-  u_char *end;
-  struct capability cap;
+  struct stream *s = BGP_INPUT (peer);
+  u_int16_t restart_flag_time;
+  int restart_bit = 0;
+  size_t end = stream_get_getp (s) + caphdr->length;
 
-  end = pnt + length;
+  /* Verify length is a multiple of 4 */
+  if ((caphdr->length-2) % 4)
+    {
+      zlog_warn("Restart Cap: Received invalid length %d, non-multiple of 4",
+               caphdr->length);
+      return -1;
+    }
 
-  while (pnt < end)
+  SET_FLAG (peer->cap, PEER_CAP_RESTART_RCV);
+  restart_flag_time = stream_getw(s);
+  if (CHECK_FLAG (restart_flag_time, RESTART_R_BIT))
     {
-      afi_t afi;
-      safi_t safi;
+      SET_FLAG (peer->cap, PEER_CAP_RESTART_BIT_RCV);
+      restart_bit = 1;
+    }
+  UNSET_FLAG (restart_flag_time, 0xF000);
+  peer->v_gr_restart = restart_flag_time;
 
-      /* Fetch structure to the byte stream. */
-      memcpy (&cap, pnt, sizeof (struct capability));
+  if (bgp_debug_neighbor_events(peer))
+    {
+      zlog_debug ("%s OPEN has Graceful Restart capability", peer->host);
+      zlog_debug ("%s Peer has%srestarted. Restart Time : %d",
+                  peer->host, restart_bit ? " " : " not ",
+                  peer->v_gr_restart);
+    }
 
-      afi = ntohs(cap.mpc.afi);
-      safi = cap.mpc.safi;
+  while (stream_get_getp (s) + 4 <= end)
+    {
+      afi_t afi = stream_getw (s);
+      safi_t safi = stream_getc (s);
+      u_char flag = stream_getc (s);
+      
+      if (!bgp_afi_safi_valid_indices (afi, &safi))
+        {
+          if (bgp_debug_neighbor_events(peer))
+            zlog_debug ("%s Addr-family %d/%d(afi/safi) not supported."
+                        " Ignore the Graceful Restart capability for this AFI/SAFI",
+                        peer->host, afi, safi);
+        }
+      else if (!peer->afc[afi][safi])
+        {
+          if (bgp_debug_neighbor_events(peer))
+            zlog_debug ("%s Addr-family %d/%d(afi/safi) not enabled."
+                        " Ignore the Graceful Restart capability",
+                        peer->host, afi, safi);
+        }
+      else
+        {
+          if (bgp_debug_neighbor_events(peer))
+            zlog_debug ("%s Address family %s is%spreserved", peer->host,
+                        afi_safi_print (afi, safi),
+                        CHECK_FLAG (peer->af_cap[afi][safi],
+                                    PEER_CAP_RESTART_AF_PRESERVE_RCV)
+                        ? " " : " not ");
+
+          SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_RESTART_AF_RCV);
+          if (CHECK_FLAG (flag, RESTART_F_BIT))
+            SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_RESTART_AF_PRESERVE_RCV);
+          
+        }
+    }
+  return 0;
+}
 
-      if (BGP_DEBUG (normal, NORMAL))
-       zlog_info ("%s OPEN has CAPABILITY code: %d, length %d",
-                  peer->host, cap.code, cap.length);
+/* Unlike other capability parsing routines, this one returns 0 on error */
+static as_t
+bgp_capability_as4 (struct peer *peer, struct capability_header *hdr)
+{
+  SET_FLAG (peer->cap, PEER_CAP_AS4_RCV);
+  
+  if (hdr->length != CAPABILITY_CODE_AS4_LEN)
+    {
+      zlog_err ("%s AS4 capability has incorrect data length %d",
+                peer->host, hdr->length);
+      return 0;
+    }
+  
+  as_t as4 = stream_getl (BGP_INPUT(peer));
+  
+  if (BGP_DEBUG (as4, AS4))
+    zlog_debug ("%s [AS4] about to set cap PEER_CAP_AS4_RCV, got as4 %u",
+                peer->host, as4);
+  return as4;
+}
 
-      /* We need at least capability code and capability length. */
-      if (pnt + 2 > end)
-       {
-         zlog_info ("%s Capability length error", peer->host);
-         bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
-         return -1;
-       }
+static int
+bgp_capability_addpath (struct peer *peer, struct capability_header *hdr)
+{
+  struct stream *s = BGP_INPUT (peer);
+  size_t end = stream_get_getp (s) + hdr->length;
 
-      /* Capability length check. */
-      if (pnt + (cap.length + 2) > end)
-       {
-         zlog_info ("%s Capability length error", peer->host);
-         bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
-         return -1;
-       }
+  SET_FLAG (peer->cap, PEER_CAP_ADDPATH_RCV);
 
-      /* We know MP Capability Code. */
-      if (cap.code == CAPABILITY_CODE_MP)
-       {
-         if (BGP_DEBUG (normal, NORMAL))
-           zlog_info ("%s OPEN has MP_EXT CAP for afi/safi: %u/%u",
-                      peer->host, afi, safi);
+  /* Verify length is a multiple of 4 */
+  if (hdr->length % 4)
+    {
+      zlog_warn("Add Path: Received invalid length %d, non-multiple of 4",
+               hdr->length);
+      return -1;
+    }
 
-         /* Ignore capability when override-capability is set. */
-         if (! CHECK_FLAG (peer->flags, PEER_FLAG_OVERRIDE_CAPABILITY))
-           {
-             /* Set negotiated value. */
-             ret = bgp_capability_mp (peer, &cap);
-
-             /* Unsupported Capability. */
-             if (ret < 0)
-               {
-                 /* Store return data. */
-                 memcpy (*error, &cap, cap.length + 2);
-                 *error += cap.length + 2;
-               }
-           }
-       }
-      else if (cap.code == CAPABILITY_CODE_REFRESH
-              || cap.code == CAPABILITY_CODE_REFRESH_OLD)
-       {
-         /* Check length. */
-         if (cap.length != 0)
-           {
-             zlog_info ("%s Route Refresh Capability length error %d",
-                        peer->host, cap.length);
-             bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
-             return -1;
-           }
+  while (stream_get_getp (s) + 4 <= end)
+    {
+      afi_t afi = stream_getw (s);
+      safi_t safi = stream_getc (s);
+      u_char send_receive = stream_getc (s);
+
+      if (bgp_debug_neighbor_events(peer))
+        zlog_debug ("%s OPEN has AddPath CAP for afi/safi: %u/%u%s%s",
+                    peer->host, afi, safi,
+                    (send_receive & BGP_ADDPATH_RX) ? ", receive" : "",
+                    (send_receive & BGP_ADDPATH_TX) ? ", transmit" : "");
+
+      if (!bgp_afi_safi_valid_indices (afi, &safi))
+        {
+          if (bgp_debug_neighbor_events(peer))
+            zlog_debug ("%s Addr-family %d/%d(afi/safi) not supported."
+                        " Ignore the Addpath Attribute for this AFI/SAFI",
+                        peer->host, afi, safi);
+         continue;
+        }
+      else if (!peer->afc[afi][safi])
+        {
+          if (bgp_debug_neighbor_events(peer))
+            zlog_debug ("%s Addr-family %d/%d(afi/safi) not enabled."
+                        " Ignore the AddPath capability for this AFI/SAFI",
+                        peer->host, afi, safi);
+         continue;
+        }
 
-         if (BGP_DEBUG (normal, NORMAL))
-           zlog_info ("%s OPEN has ROUTE-REFRESH capability(%s) for all address-families",
-                      peer->host,
-                      cap.code == CAPABILITY_CODE_REFRESH_OLD ? "old" : "new");
+      if (send_receive & BGP_ADDPATH_RX)
+        SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_ADDPATH_AF_RX_RCV);
 
-         /* BGP refresh capability */
-         if (cap.code == CAPABILITY_CODE_REFRESH_OLD)
-           SET_FLAG (peer->cap, PEER_CAP_REFRESH_OLD_RCV);
-         else
-           SET_FLAG (peer->cap, PEER_CAP_REFRESH_NEW_RCV);
-       }
-      else if (cap.code == CAPABILITY_CODE_ORF
-              || cap.code == CAPABILITY_CODE_ORF_OLD)
-       bgp_capability_orf (peer, &cap, pnt + sizeof (struct capability));
-      else if (cap.code == CAPABILITY_CODE_DYNAMIC)
-       {
-         /* Check length. */
-         if (cap.length != 0)
-           {
-             zlog_info ("%s Dynamic Capability length error %d",
-                        peer->host, cap.length);
-             bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
-             return -1;
-           }
+      if (send_receive & BGP_ADDPATH_TX)
+        SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_ADDPATH_AF_TX_RCV);
+    }
 
-         if (BGP_DEBUG (normal, NORMAL))
-           zlog_info ("%s OPEN has DYNAMIC capability", peer->host);
+  return 0;
+}
 
-         SET_FLAG (peer->cap, PEER_CAP_DYNAMIC_RCV);
-       }
-      else if (cap.code > 128)
+static int
+bgp_capability_enhe (struct peer *peer, struct capability_header *hdr)
+{
+  struct stream *s = BGP_INPUT (peer);
+  size_t end = stream_get_getp (s) + hdr->length;
+
+  /* Verify length is a multiple of 4 */
+  if (hdr->length % 6)
+    {
+      zlog_warn("Extended NH: Received invalid length %d, non-multiple of 6",
+               hdr->length);
+      return -1;
+    }
+
+  while (stream_get_getp (s) + 6 <= end)
+    {
+      afi_t afi = stream_getw (s);
+      safi_t safi = stream_getw (s);
+      afi_t nh_afi = stream_getw (s);
+
+      if (bgp_debug_neighbor_events(peer))
+        zlog_debug ("%s   Received with value triple (afi/safi/next-hop afi): %u/%u/%u",
+                    peer->host, afi, safi, nh_afi);
+
+      if (!bgp_afi_safi_valid_indices (afi, &safi))
+        return -1;
+
+      if (afi != AFI_IP || nh_afi != AFI_IP6)
+        {
+          zlog_warn ("%s Extended Next-hop capability, wrong afi/next-hop afi: %u/%u",
+                     peer->host, afi, nh_afi);
+          return -1;
+        }
+
+      /* Until SAFIs other than SAFI_UNICAST are supported */
+      if (safi != SAFI_UNICAST)
+        zlog_warn ("%s Extended Next-hop capability came with unsupported SAFI: %u",
+                   peer->host, safi);
+
+      SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_RCV);
+
+      if (CHECK_FLAG (peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_ADV))
+        SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_NEGO);
+    }
+
+  SET_FLAG (peer->cap, PEER_CAP_ENHE_RCV);
+
+  return 0;
+}
+
+  static const struct message capcode_str[] =
+{
+  { CAPABILITY_CODE_MP,                        "MultiProtocol Extensions"      },
+  { CAPABILITY_CODE_REFRESH,           "Route Refresh"                 },
+  { CAPABILITY_CODE_ORF,               "Cooperative Route Filtering"   },
+  { CAPABILITY_CODE_RESTART,           "Graceful Restart"              },
+  { CAPABILITY_CODE_AS4,               "4-octet AS number"             },
+  { CAPABILITY_CODE_ADDPATH,            "AddPath"                       },
+  { CAPABILITY_CODE_DYNAMIC,           "Dynamic"                       },
+  { CAPABILITY_CODE_ENHE,               "Extended Next Hop Encoding"    },
+  { CAPABILITY_CODE_DYNAMIC_OLD,       "Dynamic (Old)"                 },
+  { CAPABILITY_CODE_REFRESH_OLD,       "Route Refresh (Old)"           },
+  { CAPABILITY_CODE_ORF_OLD,           "ORF (Old)"                     },
+};
+static const int capcode_str_max = array_size(capcode_str);
+
+/* Minimum sizes for length field of each cap (so not inc. the header) */
+static const size_t cap_minsizes[] = 
+{
+  [CAPABILITY_CODE_MP]         = sizeof (struct capability_mp_data),
+  [CAPABILITY_CODE_REFRESH]    = CAPABILITY_CODE_REFRESH_LEN,
+  [CAPABILITY_CODE_ORF]                = sizeof (struct capability_orf_entry),
+  [CAPABILITY_CODE_RESTART]    = sizeof (struct capability_gr),
+  [CAPABILITY_CODE_AS4]                = CAPABILITY_CODE_AS4_LEN,
+  [CAPABILITY_CODE_ADDPATH]     = CAPABILITY_CODE_ADDPATH_LEN,
+  [CAPABILITY_CODE_DYNAMIC]    = CAPABILITY_CODE_DYNAMIC_LEN,
+  [CAPABILITY_CODE_DYNAMIC_OLD]        = CAPABILITY_CODE_DYNAMIC_LEN,
+  [CAPABILITY_CODE_ENHE]        = CAPABILITY_CODE_ENHE_LEN,
+  [CAPABILITY_CODE_REFRESH_OLD]        = CAPABILITY_CODE_REFRESH_LEN,
+  [CAPABILITY_CODE_ORF_OLD]    = sizeof (struct capability_orf_entry),
+};
+
+/**
+ * Parse given capability.
+ * XXX: This is reading into a stream, but not using stream API
+ *
+ * @param[out] mp_capability Set to 1 on return iff one or more Multiprotocol
+ *                           capabilities were encountered.
+ */
+static int
+bgp_capability_parse (struct peer *peer, size_t length, int *mp_capability,
+                     u_char **error)
+{
+  int ret;
+  struct stream *s = BGP_INPUT (peer);
+  size_t end = stream_get_getp (s) + length;
+  
+  assert (STREAM_READABLE (s) >= length);
+  
+  while (stream_get_getp (s) < end)
+    {
+      size_t start;
+      u_char *sp = stream_pnt (s);
+      struct capability_header caphdr;
+
+      ret = 0;
+      /* We need at least capability code and capability length. */
+      if (stream_get_getp(s) + 2 > end)
        {
-         /* We don't send Notification for unknown vendor specific
-            capabilities.  It seems reasonable for now...  */
-         zlog_warn ("%s Vendor specific capability %d",
-                    peer->host, cap.code);
+         zlog_info ("%s Capability length error (< header)", peer->host);
+         bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
+         return -1;
        }
-      else
+      
+      caphdr.code = stream_getc (s);
+      caphdr.length = stream_getc (s);
+      start = stream_get_getp (s);
+      
+      /* Capability length check sanity check. */
+      if (start + caphdr.length > end)
        {
-         zlog_warn ("%s unrecognized capability code: %d - ignored",
-                    peer->host, cap.code);
-         memcpy (*error, &cap, cap.length + 2);
-         *error += cap.length + 2;
+         zlog_info ("%s Capability length error (< length)", peer->host);
+         bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
+         return -1;
        }
+      
+      if (bgp_debug_neighbor_events(peer))
+       zlog_debug ("%s OPEN has %s capability (%u), length %u",
+                  peer->host,
+                  LOOKUP (capcode_str, caphdr.code),
+                  caphdr.code, caphdr.length);
+      
+      /* Length sanity check, type-specific, for known capabilities */
+      switch (caphdr.code)
+        {
+          case CAPABILITY_CODE_MP:
+          case CAPABILITY_CODE_REFRESH:
+          case CAPABILITY_CODE_REFRESH_OLD:
+          case CAPABILITY_CODE_ORF:
+          case CAPABILITY_CODE_ORF_OLD:
+          case CAPABILITY_CODE_RESTART:
+          case CAPABILITY_CODE_AS4:
+          case CAPABILITY_CODE_ADDPATH:
+          case CAPABILITY_CODE_DYNAMIC:
+          case CAPABILITY_CODE_DYNAMIC_OLD:
+          case CAPABILITY_CODE_ENHE:
+              /* Check length. */
+              if (caphdr.length < cap_minsizes[caphdr.code])
+                {
+                  zlog_info ("%s %s Capability length error: got %u,"
+                             " expected at least %u",
+                             peer->host, 
+                             LOOKUP (capcode_str, caphdr.code),
+                             caphdr.length, 
+                            (unsigned) cap_minsizes[caphdr.code]);
+                  bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
+                  return -1;
+                }
+          /* we deliberately ignore unknown codes, see below */
+          default:
+            break;
+        }
+      
+      switch (caphdr.code)
+        {
+          case CAPABILITY_CODE_MP:
+            {
+             *mp_capability = 1;
+
+              /* Ignore capability when override-capability is set. */
+              if (! CHECK_FLAG (peer->flags, PEER_FLAG_OVERRIDE_CAPABILITY))
+                {
+                  /* Set negotiated value. */
+                  ret = bgp_capability_mp (peer, &caphdr);
+
+                  /* Unsupported Capability. */
+                  if (ret < 0)
+                    {
+                      /* Store return data. */
+                      memcpy (*error, sp, caphdr.length + 2);
+                      *error += caphdr.length + 2;
+                    }
+                 ret = 0;      /* Don't return error for this */
+                }
+            }
+            break;
+          case CAPABILITY_CODE_REFRESH:
+          case CAPABILITY_CODE_REFRESH_OLD:
+            {
+              /* BGP refresh capability */
+              if (caphdr.code == CAPABILITY_CODE_REFRESH_OLD)
+                SET_FLAG (peer->cap, PEER_CAP_REFRESH_OLD_RCV);
+              else
+                SET_FLAG (peer->cap, PEER_CAP_REFRESH_NEW_RCV);
+            }
+            break;
+          case CAPABILITY_CODE_ORF:
+          case CAPABILITY_CODE_ORF_OLD:
+            ret = bgp_capability_orf_entry (peer, &caphdr);
+            break;
+          case CAPABILITY_CODE_RESTART:
+            ret = bgp_capability_restart (peer, &caphdr);
+            break;
+          case CAPABILITY_CODE_DYNAMIC:
+         case CAPABILITY_CODE_DYNAMIC_OLD:
+            SET_FLAG (peer->cap, PEER_CAP_DYNAMIC_RCV);
+            break;
+          case CAPABILITY_CODE_AS4:
+              /* Already handled as a special-case parsing of the capabilities
+               * at the beginning of OPEN processing. So we care not a jot
+               * for the value really, only error case.
+               */
+              if (!bgp_capability_as4 (peer, &caphdr))
+                ret = -1;
+              break;            
+          case CAPABILITY_CODE_ADDPATH:
+            ret = bgp_capability_addpath (peer, &caphdr);
+            break;
+          case CAPABILITY_CODE_ENHE:
+            ret = bgp_capability_enhe (peer, &caphdr);
+            break;
+          default:
+            if (caphdr.code > 128)
+              {
+                /* We don't send Notification for unknown vendor specific
+                   capabilities.  It seems reasonable for now...  */
+                zlog_warn ("%s Vendor specific capability %d",
+                           peer->host, caphdr.code);
+              }
+            else
+              {
+                zlog_warn ("%s unrecognized capability code: %d - ignored",
+                           peer->host, caphdr.code);
+                memcpy (*error, sp, caphdr.length + 2);
+                *error += caphdr.length + 2;
+              }
+          }
 
-      pnt += cap.length + 2;
+      if (ret < 0)
+       {
+         bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
+         return -1;
+       }
+      if (stream_get_getp(s) != (start + caphdr.length))
+        {
+          if (stream_get_getp(s) > (start + caphdr.length))
+            zlog_warn ("%s Cap-parser for %s read past cap-length, %u!",
+                       peer->host, LOOKUP (capcode_str, caphdr.code),
+                       caphdr.length);
+          stream_set_getp (s, start + caphdr.length);
+        }
     }
   return 0;
 }
 
-int
-bgp_auth_parse (struct peer *peer, u_char *pnt, size_t length)
+static int
+bgp_auth_parse (struct peer *peer, size_t length)
 {
   bgp_notify_send (peer, 
                   BGP_NOTIFY_OPEN_ERR, 
@@ -441,7 +802,7 @@ bgp_auth_parse (struct peer *peer, u_char *pnt, size_t length)
   return -1;
 }
 
-int
+static int
 strict_capability_same (struct peer *peer)
 {
   int i, j;
@@ -453,55 +814,131 @@ strict_capability_same (struct peer *peer)
   return 1;
 }
 
-/* Parse open option */
+/* peek into option, stores ASN to *as4 if the AS4 capability was found.
+ * Returns  0 if no as4 found, as4cap value otherwise.
+ */
+as_t
+peek_for_as4_capability (struct peer *peer, u_char length)
+{
+  struct stream *s = BGP_INPUT (peer);
+  size_t orig_getp = stream_get_getp (s);
+  size_t end = orig_getp + length;
+  as_t as4 = 0;
+  
+  /* The full capability parser will better flag the error.. */
+  if (STREAM_READABLE(s) < length)
+    return 0;
+
+  if (BGP_DEBUG (as4, AS4))
+    zlog_info ("%s [AS4] rcv OPEN w/ OPTION parameter len: %u,"
+                " peeking for as4",
+               peer->host, length);
+  /* the error cases we DONT handle, we ONLY try to read as4 out of
+   * correctly formatted options.
+   */
+  while (stream_get_getp(s) < end) 
+    {
+      u_char opt_type;
+      u_char opt_length;
+      
+      /* Check the length. */
+      if (stream_get_getp (s) + 2 > end)
+        goto end;
+      
+      /* Fetch option type and length. */
+      opt_type = stream_getc (s);
+      opt_length = stream_getc (s);
+      
+      /* Option length check. */
+      if (stream_get_getp (s) + opt_length > end)
+        goto end;
+      
+      if (opt_type == BGP_OPEN_OPT_CAP)
+        {
+          unsigned long capd_start = stream_get_getp (s);
+          unsigned long capd_end = capd_start + opt_length;
+          
+          assert (capd_end <= end);
+          
+         while (stream_get_getp (s) < capd_end)
+           {
+             struct capability_header hdr;
+             
+             if (stream_get_getp (s) + 2 > capd_end)
+                goto end;
+              
+              hdr.code = stream_getc (s);
+              hdr.length = stream_getc (s);
+              
+             if ((stream_get_getp(s) +  hdr.length) > capd_end)
+               goto end;
+
+             if (hdr.code == CAPABILITY_CODE_AS4)
+               {
+                 if (BGP_DEBUG (as4, AS4))
+                   zlog_info ("[AS4] found AS4 capability, about to parse");
+                 as4 = bgp_capability_as4 (peer, &hdr);
+                 
+                 goto end;
+                }
+              stream_forward_getp (s, hdr.length);
+           }
+       }
+    }
+
+end:
+  stream_set_getp (s, orig_getp);
+  return as4;
+}
+
+/**
+ * Parse open option.
+ *
+ * @param[out] mp_capability @see bgp_capability_parse() for semantics.
+ */
 int
-bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
+bgp_open_option_parse (struct peer *peer, u_char length, int *mp_capability)
 {
   int ret;
-  u_char *end;
-  u_char opt_type;
-  u_char opt_length;
-  u_char *pnt;
   u_char *error;
   u_char error_data[BGP_MAX_PACKET_SIZE];
-
-  /* Fetch pointer. */
-  pnt = stream_pnt (peer->ibuf);
+  struct stream *s = BGP_INPUT(peer);
+  size_t end = stream_get_getp (s) + length;
 
   ret = 0;
-  opt_type = 0;
-  opt_length = 0;
-  end = pnt + length;
   error = error_data;
 
-  if (BGP_DEBUG (normal, NORMAL))
-    zlog_info ("%s rcv OPEN w/ OPTION parameter len: %u",
+  if (bgp_debug_neighbor_events(peer))
+    zlog_debug ("%s rcv OPEN w/ OPTION parameter len: %u",
               peer->host, length);
   
-  while (pnt < end) 
+  while (stream_get_getp(s) < end)
     {
-      /* Check the length. */
-      if (pnt + 2 > end)
+      u_char opt_type;
+      u_char opt_length;
+      
+      /* Must have at least an OPEN option header */
+      if (STREAM_READABLE(s) < 2)
        {
          zlog_info ("%s Option length error", peer->host);
-         bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
+         bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
          return -1;
        }
 
       /* Fetch option type and length. */
-      opt_type = *pnt++;
-      opt_length = *pnt++;
+      opt_type = stream_getc (s);
+      opt_length = stream_getc (s);
       
       /* Option length check. */
-      if (pnt + opt_length > end)
+      if (STREAM_READABLE (s) < opt_length)
        {
          zlog_info ("%s Option length error", peer->host);
-         bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
+         bgp_notify_send (peer, BGP_NOTIFY_CEASE, BGP_NOTIFY_OPEN_MALFORMED_ATTR);
          return -1;
        }
 
-      if (BGP_DEBUG (normal, NORMAL))
-       zlog_info ("%s rcvd OPEN w/ optional parameter type %u (%s) len %u",
+      if (bgp_debug_neighbor_events(peer))
+       zlog_debug ("%s rcvd OPEN w/ optional parameter type %u (%s) len %u",
                   peer->host, opt_type,
                   opt_type == BGP_OPEN_OPT_AUTH ? "Authentication" :
                   opt_type == BGP_OPEN_OPT_CAP ? "Capability" : "Unknown",
@@ -510,11 +947,10 @@ bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
       switch (opt_type)
        {
        case BGP_OPEN_OPT_AUTH:
-         ret = bgp_auth_parse (peer, pnt, opt_length);
+         ret = bgp_auth_parse (peer, opt_length);
          break;
        case BGP_OPEN_OPT_CAP:
-         ret = bgp_capability_parse (peer, pnt, opt_length, &error);
-         *capability = 1;
+         ret = bgp_capability_parse (peer, opt_length, mp_capability, &error);
          break;
        default:
          bgp_notify_send (peer, 
@@ -530,9 +966,6 @@ bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
          error and erro_data pointer, like below.  */
       if (ret < 0)
        return -1;
-
-      /* Forward pointer. */
-      pnt += opt_length;
     }
 
   /* All OPEN option is parsed.  Check capability when strict compare
@@ -560,9 +993,10 @@ bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
        }
     }
 
-  /* Check there is no common capability send Unsupported Capability
+  /* Check there are no common AFI/SAFIs and send Unsupported Capability
      error. */
-  if (*capability && ! CHECK_FLAG (peer->flags, PEER_FLAG_OVERRIDE_CAPABILITY))
+  if (*mp_capability &&
+      ! CHECK_FLAG (peer->flags, PEER_FLAG_OVERRIDE_CAPABILITY))
     {
       if (! peer->afc_nego[AFI_IP][SAFI_UNICAST] 
          && ! peer->afc_nego[AFI_IP][SAFI_MULTICAST]
@@ -570,10 +1004,11 @@ bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
          && ! peer->afc_nego[AFI_IP6][SAFI_UNICAST]
          && ! peer->afc_nego[AFI_IP6][SAFI_MULTICAST])
        {
-         plog_err (peer->log, "%s [Error] No common capability", peer->host);
+         zlog_err ("%s [Error] Configured AFI/SAFIs do not "
+                   "overlap with received MP capabilities",
+                   peer->host);
 
          if (error != error_data)
-
            bgp_notify_send_with_data (peer, 
                                       BGP_NOTIFY_OPEN_ERR, 
                                       BGP_NOTIFY_OPEN_UNSUP_CAPBL, 
@@ -588,7 +1023,7 @@ bgp_open_option_parse (struct peer *peer, u_char length, int *capability)
   return 0;
 }
 
-void
+static void
 bgp_open_capability_orf (struct stream *s, struct peer *peer,
                          afi_t afi, safi_t safi, u_char code)
 {
@@ -600,18 +1035,18 @@ bgp_open_capability_orf (struct stream *s, struct peer *peer,
   int number_of_orfs = 0;
 
   if (safi == SAFI_MPLS_VPN)
-    safi = BGP_SAFI_VPNV4;
+    safi = SAFI_MPLS_LABELED_VPN;
 
   stream_putc (s, BGP_OPEN_OPT_CAP);
-  capp = stream_get_putp (s);           /* Set Capability Len Pointer */
+  capp = stream_get_endp (s);           /* Set Capability Len Pointer */
   stream_putc (s, 0);                   /* Capability Length */
   stream_putc (s, code);                /* Capability Code */
-  orfp = stream_get_putp (s);           /* Set ORF Len Pointer */
+  orfp = stream_get_endp (s);           /* Set ORF Len Pointer */
   stream_putc (s, 0);                   /* ORF Length */
   stream_putw (s, afi);
   stream_putc (s, 0);
   stream_putc (s, safi);
-  numberp = stream_get_putp (s);        /* Set Number Pointer */
+  numberp = stream_get_endp (s);        /* Set Number Pointer */
   stream_putc (s, 0);                   /* Number of ORFs */
 
   /* Address Prefix ORF */
@@ -645,11 +1080,11 @@ bgp_open_capability_orf (struct stream *s, struct peer *peer,
   stream_putc_at (s, numberp, number_of_orfs);
 
   /* Total ORF Len. */
-  orf_len = stream_get_putp (s) - orfp - 1;
+  orf_len = stream_get_endp (s) - orfp - 1;
   stream_putc_at (s, orfp, orf_len);
 
   /* Total Capability Len. */
-  cap_len = stream_get_putp (s) - capp - 1;
+  cap_len = stream_get_endp (s) - capp - 1;
   stream_putc_at (s, capp, cap_len);
 }
 
@@ -658,12 +1093,15 @@ void
 bgp_open_capability (struct stream *s, struct peer *peer)
 {
   u_char len;
-  unsigned long cp;
+  unsigned long cp, capp, rcapp;
   afi_t afi;
   safi_t safi;
+  as_t local_as;
+  u_int32_t restart_time;
+  u_char afi_safi_count = 0;
 
   /* Remember current pointer for Opt Parm Len. */
-  cp = stream_get_putp (s);
+  cp = stream_get_endp (s);
 
   /* Opt Parm Len. */
   stream_putc (s, 0);
@@ -673,23 +1111,6 @@ bgp_open_capability (struct stream *s, struct peer *peer)
       || CHECK_FLAG (peer->flags, PEER_FLAG_DONT_CAPABILITY))
     return;
 
-  /* When the peer is IPv4 unicast only, do not send capability. */
-  if (! peer->afc[AFI_IP][SAFI_MULTICAST] 
-      && ! peer->afc[AFI_IP][SAFI_MPLS_VPN]
-      && ! peer->afc[AFI_IP6][SAFI_UNICAST] 
-      && ! peer->afc[AFI_IP6][SAFI_MULTICAST]
-      && CHECK_FLAG (peer->flags, PEER_FLAG_NO_ROUTE_REFRESH_CAP)
-      && ! CHECK_FLAG (peer->af_flags[AFI_IP][SAFI_UNICAST],
-                      PEER_FLAG_ORF_PREFIX_SM)
-      && ! CHECK_FLAG (peer->af_flags[AFI_IP][SAFI_UNICAST],
-                      PEER_FLAG_ORF_PREFIX_RM)
-      && ! CHECK_FLAG (peer->af_flags[AFI_IP][SAFI_MULTICAST],
-                      PEER_FLAG_ORF_PREFIX_SM)
-      && ! CHECK_FLAG (peer->af_flags[AFI_IP][SAFI_MULTICAST],
-                      PEER_FLAG_ORF_PREFIX_RM)
-      && ! CHECK_FLAG (peer->flags, PEER_FLAG_DYNAMIC_CAPABILITY))
-    return;
-
   /* IPv4 unicast. */
   if (peer->afc[AFI_IP][SAFI_UNICAST])
     {
@@ -724,9 +1145,29 @@ bgp_open_capability (struct stream *s, struct peer *peer)
       stream_putc (s, CAPABILITY_CODE_MP_LEN);
       stream_putw (s, AFI_IP);
       stream_putc (s, 0);
-      stream_putc (s, BGP_SAFI_VPNV4);
+      stream_putc (s, SAFI_MPLS_LABELED_VPN);
     }
 #ifdef HAVE_IPV6
+  /* Currently supporting RFC-5549 for Link-Local peering only */
+  if (CHECK_FLAG (peer->flags, PEER_FLAG_CAPABILITY_ENHE) &&
+      peer->su.sa.sa_family == AF_INET6 &&
+      IN6_IS_ADDR_LINKLOCAL(&peer->su.sin6.sin6_addr))
+    {
+      /* RFC 5549 Extended Next Hop Encoding */
+      SET_FLAG (peer->cap, PEER_CAP_ENHE_ADV);
+      stream_putc (s, BGP_OPEN_OPT_CAP);
+      stream_putc (s, CAPABILITY_CODE_ENHE_LEN + 2);
+      stream_putc (s, CAPABILITY_CODE_ENHE);
+      stream_putc (s, CAPABILITY_CODE_ENHE_LEN);
+      /* Currently supporting for SAFI_UNICAST only */
+      SET_FLAG (peer->af_cap[AFI_IP][SAFI_UNICAST], PEER_CAP_ENHE_AF_ADV);
+      stream_putw (s, AFI_IP);
+      stream_putw (s, SAFI_UNICAST);
+      stream_putw (s, AFI_IP6);
+
+      if (CHECK_FLAG (peer->af_cap[AFI_IP][SAFI_UNICAST], PEER_CAP_ENHE_AF_RCV))
+        SET_FLAG (peer->af_cap[AFI_IP][SAFI_UNICAST], PEER_CAP_ENHE_AF_NEGO);
+    }
   /* IPv6 unicast. */
   if (peer->afc[AFI_IP6][SAFI_UNICAST])
     {
@@ -754,18 +1195,51 @@ bgp_open_capability (struct stream *s, struct peer *peer)
 #endif /* HAVE_IPV6 */
 
   /* Route refresh. */
-  if (! CHECK_FLAG (peer->flags, PEER_FLAG_NO_ROUTE_REFRESH_CAP))
-    {
-      SET_FLAG (peer->cap, PEER_CAP_REFRESH_ADV);
-      stream_putc (s, BGP_OPEN_OPT_CAP);
-      stream_putc (s, CAPABILITY_CODE_REFRESH_LEN + 2);
-      stream_putc (s, CAPABILITY_CODE_REFRESH_OLD);
-      stream_putc (s, CAPABILITY_CODE_REFRESH_LEN);
-      stream_putc (s, BGP_OPEN_OPT_CAP);
-      stream_putc (s, CAPABILITY_CODE_REFRESH_LEN + 2);
-      stream_putc (s, CAPABILITY_CODE_REFRESH);
-      stream_putc (s, CAPABILITY_CODE_REFRESH_LEN);
-    }
+  SET_FLAG (peer->cap, PEER_CAP_REFRESH_ADV);
+  stream_putc (s, BGP_OPEN_OPT_CAP);
+  stream_putc (s, CAPABILITY_CODE_REFRESH_LEN + 2);
+  stream_putc (s, CAPABILITY_CODE_REFRESH_OLD);
+  stream_putc (s, CAPABILITY_CODE_REFRESH_LEN);
+  stream_putc (s, BGP_OPEN_OPT_CAP);
+  stream_putc (s, CAPABILITY_CODE_REFRESH_LEN + 2);
+  stream_putc (s, CAPABILITY_CODE_REFRESH);
+  stream_putc (s, CAPABILITY_CODE_REFRESH_LEN);
+
+  /* AS4 */
+  SET_FLAG (peer->cap, PEER_CAP_AS4_ADV);
+  stream_putc (s, BGP_OPEN_OPT_CAP);
+  stream_putc (s, CAPABILITY_CODE_AS4_LEN + 2);
+  stream_putc (s, CAPABILITY_CODE_AS4);
+  stream_putc (s, CAPABILITY_CODE_AS4_LEN);
+  if ( peer->change_local_as )
+    local_as = peer->change_local_as;
+  else
+    local_as = peer->local_as;
+  stream_putl (s, local_as );
+
+  /* AddPath
+   * For now we will only advertise RX support. TX support will be added later.
+   */
+  for (afi = AFI_IP ; afi < AFI_MAX ; afi++)
+    for (safi = SAFI_UNICAST ; safi < SAFI_MAX ; safi++)
+      if (peer->afc[afi][safi])
+        afi_safi_count++;
+
+  SET_FLAG (peer->cap, PEER_CAP_ADDPATH_ADV);
+  stream_putc (s, BGP_OPEN_OPT_CAP);
+  stream_putc (s, (CAPABILITY_CODE_ADDPATH_LEN * afi_safi_count) + 2);
+  stream_putc (s, CAPABILITY_CODE_ADDPATH);
+  stream_putc (s, CAPABILITY_CODE_ADDPATH_LEN * afi_safi_count);
+
+  for (afi = AFI_IP ; afi < AFI_MAX ; afi++)
+    for (safi = SAFI_UNICAST ; safi < SAFI_MAX ; safi++)
+      if (peer->afc[afi][safi])
+        {
+          stream_putw (s, afi);
+          stream_putc (s, safi);
+          stream_putc (s, BGP_ADDPATH_RX);
+          SET_FLAG (peer->af_cap[afi][safi], PEER_CAP_ADDPATH_AF_RX_ADV);
+        }
 
   /* ORF capability. */
   for (afi = AFI_IP ; afi < AFI_MAX ; afi++)
@@ -783,11 +1257,53 @@ bgp_open_capability (struct stream *s, struct peer *peer)
       SET_FLAG (peer->cap, PEER_CAP_DYNAMIC_ADV);
       stream_putc (s, BGP_OPEN_OPT_CAP);
       stream_putc (s, CAPABILITY_CODE_DYNAMIC_LEN + 2);
+      stream_putc (s, CAPABILITY_CODE_DYNAMIC_OLD);
+      stream_putc (s, CAPABILITY_CODE_DYNAMIC_LEN);
+      stream_putc (s, BGP_OPEN_OPT_CAP);
+      stream_putc (s, CAPABILITY_CODE_DYNAMIC_LEN + 2);
       stream_putc (s, CAPABILITY_CODE_DYNAMIC);
       stream_putc (s, CAPABILITY_CODE_DYNAMIC_LEN);
     }
 
+  /* Sending base graceful-restart capability irrespective of the config */
+  SET_FLAG (peer->cap, PEER_CAP_RESTART_ADV);
+  stream_putc (s, BGP_OPEN_OPT_CAP);
+  capp = stream_get_endp (s);           /* Set Capability Len Pointer */
+  stream_putc (s, 0);                   /* Capability Length */
+  stream_putc (s, CAPABILITY_CODE_RESTART);
+  rcapp = stream_get_endp (s);          /* Set Restart Capability Len Pointer */
+  stream_putc (s, 0);
+  restart_time = peer->bgp->restart_time;
+  if (peer->bgp->t_startup)
+    {
+      SET_FLAG (restart_time, RESTART_R_BIT);
+      SET_FLAG (peer->cap, PEER_CAP_RESTART_BIT_ADV);
+    }
+  stream_putw (s, restart_time);
+
+  /* Send address-family specific graceful-restart capability only when GR config
+     is present */
+  if (bgp_flag_check (peer->bgp, BGP_FLAG_GRACEFUL_RESTART))
+    {
+      for (afi = AFI_IP ; afi < AFI_MAX ; afi++)
+        for (safi = SAFI_UNICAST ; safi < SAFI_MAX ; safi++)
+          if (peer->afc[afi][safi])
+            {
+              stream_putw (s, afi);
+              stream_putc (s, safi);
+              stream_putc (s, 0); //Forwarding is not retained as of now.
+            }
+    }
+
+  /* Total Graceful restart capability Len. */
+  len = stream_get_endp (s) - rcapp - 1;
+  stream_putc_at (s, rcapp, len);
+
+  /* Total Capability Len. */
+  len = stream_get_endp (s) - capp - 1;
+  stream_putc_at (s, capp, len);
+
   /* Total Opt Parm Len. */
-  len = stream_get_putp (s) - cp - 1;
+  len = stream_get_endp (s) - cp - 1;
   stream_putc_at (s, cp, len);
 }