1 files changed, 52 insertions, 56 deletions

diff --git a/bgpd/bgp_packet.c b/bgpd/bgp_packet.c
index 26532011..17ac1f73 100644
--- a/bgpd/bgp_packet.c
+++ b/bgpd/bgp_packet.c
@@ -1371,8 +1371,6 @@ bgp_open_receive (struct peer *peer, bgp_size_t size)
       ret = bgp_open_option_parse (peer, optlen, &capability);
       if (ret < 0)
 	return ret;
-
-      stream_forward_getp (peer->ibuf, optlen);
     }
   else
     {
@@ -1991,7 +1989,8 @@ static int
 bgp_capability_msg_parse (struct peer *peer, u_char *pnt, bgp_size_t length)
 {
   u_char *end;
-  struct capability cap;
+  struct capability_mp_data mpc;
+  struct capability_header *hdr;
   u_char action;
   struct bgp *bgp;
   afi_t afi;
@@ -2001,7 +2000,7 @@ bgp_capability_msg_parse (struct peer *peer, u_char *pnt, bgp_size_t length)
   end = pnt + length;
 
   while (pnt < end)
-    {
+    {      
       /* We need at least action, capability code and capability length. */
       if (pnt + 3 > end)
         {
@@ -2009,12 +2008,9 @@ bgp_capability_msg_parse (struct peer *peer, u_char *pnt, bgp_size_t length)
           bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
           return -1;
         }
-
       action = *pnt;
-
-      /* Fetch structure to the byte stream. */
-      memcpy (&cap, pnt + 1, sizeof (struct capability));
-
+      hdr = (struct capability_header *)(pnt + 1);
+      
       /* Action value check.  */
       if (action != CAPABILITY_ACTION_SET
 	  && action != CAPABILITY_ACTION_UNSET)
@@ -2027,77 +2023,77 @@ bgp_capability_msg_parse (struct peer *peer, u_char *pnt, bgp_size_t length)
 
       if (BGP_DEBUG (normal, NORMAL))
 	zlog_debug ("%s CAPABILITY has action: %d, code: %u, length %u",
-		   peer->host, action, cap.code, cap.length);
+		   peer->host, action, hdr->code, hdr->length);
 
       /* Capability length check. */
-      if (pnt + (cap.length + 3) > end)
+      if ((pnt + hdr->length + 3) > end)
         {
           zlog_info ("%s Capability length error", peer->host);
           bgp_notify_send (peer, BGP_NOTIFY_CEASE, 0);
           return -1;
         }
 
+      /* Fetch structure to the byte stream. */
+      memcpy (&mpc, pnt + 3, sizeof (struct capability_mp_data));
+
       /* We know MP Capability Code. */
-      if (cap.code == CAPABILITY_CODE_MP)
+      if (hdr->code == CAPABILITY_CODE_MP)
         {
-	  afi = ntohs (cap.mpc.afi);
-	  safi = cap.mpc.safi;
+	  afi = ntohs (mpc.afi);
+	  safi = mpc.safi;
 
           /* Ignore capability when override-capability is set. */
           if (CHECK_FLAG (peer->flags, PEER_FLAG_OVERRIDE_CAPABILITY))
 	    continue;
-
+          
+          if (!bgp_afi_safi_valid_indices (afi, &safi))
+            {
+              if (BGP_DEBUG (normal, NORMAL))
+                zlog_debug ("%s Dynamic Capability MP_EXT afi/safi invalid",
+                            peer->host, afi, safi);
+              continue;
+            }
+          
 	  /* Address family check.  */
-	  if ((afi == AFI_IP 
-	       || afi == AFI_IP6)
-	      && (safi == SAFI_UNICAST 
-		  || safi == SAFI_MULTICAST 
-		  || safi == BGP_SAFI_VPNV4))
-	    {
-	      if (BGP_DEBUG (normal, NORMAL))
-		zlog_debug ("%s CAPABILITY has %s MP_EXT CAP for afi/safi: %u/%u",
-			   peer->host,
-			   action == CAPABILITY_ACTION_SET 
-			   ? "Advertising" : "Removing",
-			   ntohs(cap.mpc.afi) , cap.mpc.safi);
-		  
-	      /* Adjust safi code. */
-	      if (safi == BGP_SAFI_VPNV4)
-		safi = SAFI_MPLS_VPN;
-	      
-	      if (action == CAPABILITY_ACTION_SET)
-		{
-		  peer->afc_recv[afi][safi] = 1;
-		  if (peer->afc[afi][safi])
-		    {
-		      peer->afc_nego[afi][safi] = 1;
-		      bgp_announce_route (peer, afi, safi);
-		    }
-		}
-	      else
-		{
-		  peer->afc_recv[afi][safi] = 0;
-		  peer->afc_nego[afi][safi] = 0;
-
-		  if (peer_active_nego (peer))
-		    bgp_clear_route (peer, afi, safi);
-		  else
-		    BGP_EVENT_ADD (peer, BGP_Stop);
-		} 
-	    }
+          if (BGP_DEBUG (normal, NORMAL))
+            zlog_debug ("%s CAPABILITY has %s MP_EXT CAP for afi/safi: %u/%u",
+                       peer->host,
+                       action == CAPABILITY_ACTION_SET 
+                       ? "Advertising" : "Removing",
+                       ntohs(mpc.afi) , mpc.safi);
+              
+          if (action == CAPABILITY_ACTION_SET)
+            {
+              peer->afc_recv[afi][safi] = 1;
+              if (peer->afc[afi][safi])
+                {
+                  peer->afc_nego[afi][safi] = 1;
+                  bgp_announce_route (peer, afi, safi);
+                }
+            }
+          else
+            {
+              peer->afc_recv[afi][safi] = 0;
+              peer->afc_nego[afi][safi] = 0;
+
+              if (peer_active_nego (peer))
+                bgp_clear_route (peer, afi, safi);
+              else
+                BGP_EVENT_ADD (peer, BGP_Stop);
+            }
         }
       else
         {
           zlog_warn ("%s unrecognized capability code: %d - ignored",
-                     peer->host, cap.code);
+                     peer->host, hdr->code);
         }
-      pnt += cap.length + 3;
+      pnt += hdr->length + 3;
     }
   return 0;
 }
 
 /* Dynamic Capability is received. */
-static void
+int
 bgp_capability_receive (struct peer *peer, bgp_size_t size)
 {
   u_char *pnt;
@@ -2130,7 +2126,7 @@ bgp_capability_receive (struct peer *peer, bgp_size_t size)
     }
 
   /* Parse packet. */
-  ret = bgp_capability_msg_parse (peer, pnt, size);
+  return bgp_capability_msg_parse (peer, pnt, size);
 }
 
 /* BGP read utility function. */