1 files changed, 108 insertions, 2 deletions

diff --git a/bgpd/bgp_packet.c b/bgpd/bgp_packet.c
index c5e390b04..ca2e8de04 100644
--- a/bgpd/bgp_packet.c
+++ b/bgpd/bgp_packet.c
@@ -1620,9 +1620,38 @@ void bgp_capability_send(struct peer *peer, afi_t afi, safi_t safi,
 	case CAPABILITY_CODE_AS4:
 	case CAPABILITY_CODE_DYNAMIC:
 	case CAPABILITY_CODE_ENHANCED_RR:
-	case CAPABILITY_CODE_ENHE:
 	case CAPABILITY_CODE_EXT_MESSAGE:
 		break;
+	case CAPABILITY_CODE_ENHE:
+		FOREACH_AFI_SAFI (afi, safi) {
+			if (!peer->afc[afi][safi])
+				continue;
+
+			bgp_map_afi_safi_int2iana(afi, safi, &pkt_afi, &pkt_safi);
+
+			if (CHECK_FLAG(peer->flags, PEER_FLAG_CAPABILITY_ENHE) &&
+			    peer->connection->su.sa.sa_family == AF_INET6 && afi == AFI_IP &&
+			    (safi == SAFI_UNICAST || safi == SAFI_MPLS_VPN ||
+			     safi == SAFI_LABELED_UNICAST)) {
+				stream_putc(s, action);
+				stream_putc(s, CAPABILITY_CODE_ENHE);
+				stream_putc(s, CAPABILITY_CODE_ENHE_LEN);
+				stream_putw(s, pkt_afi);
+				stream_putw(s, pkt_safi);
+				stream_putw(s, afi_int2iana(AFI_IP6));
+
+				COND_FLAG(peer->af_cap[AFI_IP][safi], PEER_CAP_ENHE_AF_ADV,
+					  action == CAPABILITY_ACTION_SET);
+
+				if (CHECK_FLAG(peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_RCV))
+					COND_FLAG(peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_NEGO,
+						  action == CAPABILITY_ACTION_SET);
+			}
+		}
+		COND_FLAG(peer->cap, PEER_CAP_ENHE_ADV, action == CAPABILITY_ACTION_SET);
+		update_group_adjust_peer_afs(peer);
+		bgp_announce_route_all(peer);
+		break;
 	case CAPABILITY_CODE_ROLE:
 		stream_putc(s, action);
 		stream_putc(s, CAPABILITY_CODE_ROLE);
@@ -3321,6 +3350,81 @@ ignore:
 	}
 }
 
+static void bgp_dynamic_capability_enhe(uint8_t *pnt, int action, struct capability_header *hdr,
+					struct peer *peer)
+{
+	uint8_t *data = pnt + 3;
+	uint8_t *end = data + hdr->length;
+	size_t len = end - data;
+
+	if (data + CAPABILITY_CODE_ENHE_LEN > end) {
+		flog_warn(EC_BGP_CAPABILITY_INVALID_LENGTH,
+			  "Extended NH: Received invalid length %zu, less than %d", len,
+			  CAPABILITY_CODE_ENHE_LEN);
+		return;
+	}
+
+	if (action == CAPABILITY_ACTION_SET) {
+		if (hdr->length % CAPABILITY_CODE_ENHE_LEN) {
+			flog_warn(EC_BGP_CAPABILITY_INVALID_LENGTH,
+				  "Extended NH: Received invalid length %d, non-multiple of %d",
+				  hdr->length, CAPABILITY_CODE_ENHE_LEN);
+			return;
+		}
+
+		while (data + CAPABILITY_CODE_ENHE_LEN <= end) {
+			afi_t afi;
+			safi_t safi;
+			afi_t nh_afi;
+			struct bgp_enhe_capability bec = {};
+
+			memcpy(&bec, data, sizeof(bec));
+			afi = ntohs(bec.afi);
+			safi = ntohs(bec.safi);
+			nh_afi = afi_iana2int(ntohs(bec.nh_afi));
+
+			/* RFC 5549 specifies use of this capability only for IPv4 AFI,
+			 * with the Nexthop AFI being IPv6. A future spec may introduce
+			 * other possibilities, so we ignore other values with a log.
+			 * Also, only SAFI_UNICAST and SAFI_LABELED_UNICAST are currently
+			 * supported (and expected).
+			 */
+			if (afi != AFI_IP || nh_afi != AFI_IP6 ||
+			    !(safi == SAFI_UNICAST || safi == SAFI_MPLS_VPN ||
+			      safi == SAFI_LABELED_UNICAST)) {
+				flog_warn(EC_BGP_CAPABILITY_INVALID_DATA,
+					  "%s Unexpected afi/safi/next-hop afi: %s/%s/%u in Extended Next-hop capability, ignoring",
+					  peer->host, afi2str(afi), safi2str(safi), nh_afi);
+				goto ignore;
+			}
+
+			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);
+
+ignore:
+			data += CAPABILITY_CODE_ENHE_LEN;
+		}
+
+		SET_FLAG(peer->cap, PEER_CAP_ENHE_RCV);
+		update_group_adjust_peer_afs(peer);
+		bgp_announce_route_all(peer);
+	} else {
+		afi_t afi;
+		safi_t safi;
+
+		UNSET_FLAG(peer->cap, PEER_CAP_ENHE_RCV);
+
+		FOREACH_AFI_SAFI (afi, safi) {
+			UNSET_FLAG(peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_RCV);
+
+			if (CHECK_FLAG(peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_ADV))
+				UNSET_FLAG(peer->af_cap[afi][safi], PEER_CAP_ENHE_AF_NEGO);
+		}
+	}
+}
+
 static void bgp_dynamic_capability_orf(uint8_t *pnt, int action,
 				       struct capability_header *hdr,
 				       struct peer *peer)
@@ -3943,9 +4047,11 @@ static int bgp_capability_msg_parse(struct peer *peer, uint8_t *pnt,
 		case CAPABILITY_CODE_AS4:
 		case CAPABILITY_CODE_DYNAMIC:
 		case CAPABILITY_CODE_ENHANCED_RR:
-		case CAPABILITY_CODE_ENHE:
 		case CAPABILITY_CODE_EXT_MESSAGE:
 			break;
+		case CAPABILITY_CODE_ENHE:
+			bgp_dynamic_capability_enhe(pnt, action, hdr, peer);
+			break;
 		case CAPABILITY_CODE_ROLE:
 			bgp_dynamic_capability_role(pnt, action, peer);
 			break;