Merge pull request #8240 from reubendowle/fixes/nhrp-nat

nhrp: NAT fixes

10 files changed, 468 insertions, 95 deletions

diff --git a/nhrpd/nhrp_cache.c b/nhrpd/nhrp_cache.c
index 4de8e220c..bcf0e2168 100644
--- a/nhrpd/nhrp_cache.c
+++ b/nhrpd/nhrp_cache.c
@@ -212,7 +212,8 @@ static int nhrp_cache_do_timeout(struct thread *t)
 
 	c->t_timeout = NULL;
 	if (c->cur.type != NHRP_CACHE_INVALID)
-		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL,
+					  NULL);
 	return 0;
 }
 
@@ -301,7 +302,8 @@ static void nhrp_cache_peer_notifier(struct notifier_block *n,
 	case NOTIFY_PEER_DOWN:
 	case NOTIFY_PEER_IFCONFIG_CHANGED:
 		notifier_call(&c->notifier_list, NOTIFY_CACHE_DOWN);
-		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL,
+					  NULL);
 		break;
 	case NOTIFY_PEER_NBMA_CHANGING:
 		if (c->cur.type == NHRP_CACHE_DYNAMIC)
@@ -422,7 +424,8 @@ static void nhrp_cache_newpeer_notifier(struct notifier_block *n,
 
 int nhrp_cache_update_binding(struct nhrp_cache *c, enum nhrp_cache_type type,
 			      int holding_time, struct nhrp_peer *p,
-			      uint32_t mtu, union sockunion *nbma_oa)
+			      uint32_t mtu, union sockunion *nbma_oa,
+			      union sockunion *nbma_claimed)
 {
 	char buf[2][SU_ADDRSTRLEN];
 
@@ -464,6 +467,12 @@ int nhrp_cache_update_binding(struct nhrp_cache *c, enum nhrp_cache_type type,
 			memset(&c->cur.remote_nbma_natoa, 0,
 			       sizeof(c->cur.remote_nbma_natoa));
 
+		if (nbma_claimed)
+			c->cur.remote_nbma_claimed = *nbma_claimed;
+		else
+			memset(&c->cur.remote_nbma_claimed, 0,
+			       sizeof(c->cur.remote_nbma_claimed));
+
 		nhrp_peer_unref(p);
 	} else {
 		debugf(NHRP_DEBUG_COMMON,
@@ -474,9 +483,13 @@ int nhrp_cache_update_binding(struct nhrp_cache *c, enum nhrp_cache_type type,
 		c->new.type = type;
 		c->new.peer = p;
 		c->new.mtu = mtu;
+		c->new.holding_time = holding_time;
 		if (nbma_oa)
 			c->new.remote_nbma_natoa = *nbma_oa;
 
+		if (nbma_claimed)
+			c->new.remote_nbma_claimed = *nbma_claimed;
+
 		if (holding_time > 0)
 			c->new.expires = monotime(NULL) + holding_time;
 		else if (holding_time < 0)
diff --git a/nhrpd/nhrp_interface.c b/nhrpd/nhrp_interface.c
index a64708d88..a6880054f 100644
--- a/nhrpd/nhrp_interface.c
+++ b/nhrpd/nhrp_interface.c
@@ -255,7 +255,7 @@ static void nhrp_interface_update_address(struct interface *ifp, afi_t afi,
 		nc = nhrp_cache_get(ifp, &if_ad->addr, 0);
 		if (nc)
 			nhrp_cache_update_binding(nc, NHRP_CACHE_LOCAL, -1,
-						  NULL, 0, NULL);
+						  NULL, 0, NULL, NULL);
 	}
 
 	debugf(NHRP_DEBUG_KERNEL, "%s: IPv%d address changed to %s", ifp->name,
@@ -267,7 +267,7 @@ static void nhrp_interface_update_address(struct interface *ifp, afi_t afi,
 		nc = nhrp_cache_get(ifp, &addr, 1);
 		if (nc)
 			nhrp_cache_update_binding(nc, NHRP_CACHE_LOCAL, 0, NULL,
-						  0, NULL);
+						  0, NULL, NULL);
 	}
 
 	notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_ADDRESS_CHANGED);
@@ -364,7 +364,7 @@ static void interface_config_update_nhrp_map(struct nhrp_cache_config *cc,
 		if (c && c->map) {
 			nhrp_cache_update_binding(
 				c, c->cur.type, -1,
-				nhrp_peer_get(ifp, &nbma_addr), 0, NULL);
+				nhrp_peer_get(ifp, &nbma_addr), 0, NULL, NULL);
 		}
 		return;
 	}
@@ -375,11 +375,11 @@ static void interface_config_update_nhrp_map(struct nhrp_cache_config *cc,
 	c->map = 1;
 	if (cc->type == NHRP_CACHE_LOCAL)
 		nhrp_cache_update_binding(c, NHRP_CACHE_LOCAL, 0, NULL, 0,
-					  NULL);
+					  NULL, NULL);
 	else {
 		nhrp_cache_update_binding(c, NHRP_CACHE_STATIC, 0,
 					  nhrp_peer_get(ifp, &cc->nbma), 0,
-					  NULL);
+					  NULL, NULL);
 	}
 }
 
diff --git a/nhrpd/nhrp_nhs.c b/nhrpd/nhrp_nhs.c
index 1689facbc..b78bf9278 100644
--- a/nhrpd/nhrp_nhs.c
+++ b/nhrpd/nhrp_nhs.c
@@ -32,7 +32,9 @@ static void nhrp_reg_reply(struct nhrp_reqid *reqid, void *arg)
 	struct nhrp_cie_header *cie;
 	struct nhrp_cache *c;
 	struct zbuf extpl;
-	union sockunion cie_nbma, cie_proto, *proto;
+	union sockunion cie_nbma, cie_nbma_nhs, cie_proto, cie_proto_nhs,
+		*proto;
+	char buf[64];
 	int ok = 0, holdtime;
 	unsigned short mtu = 0;
 
@@ -66,6 +68,7 @@ static void nhrp_reg_reply(struct nhrp_reqid *reqid, void *arg)
 
 	/* Parse extensions */
 	sockunion_family(&nifp->nat_nbma) = AF_UNSPEC;
+	sockunion_family(&cie_nbma_nhs) = AF_UNSPEC;
 	while ((ext = nhrp_ext_pull(&p->extensions, &extpl)) != NULL) {
 		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 		case NHRP_EXTENSION_NAT_ADDRESS:
@@ -75,10 +78,17 @@ static void nhrp_reg_reply(struct nhrp_reqid *reqid, void *arg)
 					     &cie_proto)) {
 				nifp->nat_nbma = cie_nbma;
 				debugf(NHRP_DEBUG_IF,
-				       "%s: NAT detected, real NBMA address: %pSU",
-				       ifp->name, &nifp->nbma);
+				       "%s: NAT detected, real NBMA address: %s",
+				       ifp->name,
+				       sockunion2str(&nifp->nbma, buf,
+						     sizeof(buf)));
 			}
 			break;
+		case NHRP_EXTENSION_RESPONDER_ADDRESS:
+			/* NHS adds its own record as responder address */
+			nhrp_cie_pull(&extpl, p->hdr, &cie_nbma_nhs,
+				      &cie_proto_nhs);
+			break;
 		}
 	}
 
@@ -96,7 +106,8 @@ static void nhrp_reg_reply(struct nhrp_reqid *reqid, void *arg)
 	c = nhrp_cache_get(ifp, &p->dst_proto, 1);
 	if (c)
 		nhrp_cache_update_binding(c, NHRP_CACHE_NHS, holdtime,
-					  nhrp_peer_ref(r->peer), mtu, NULL);
+					  nhrp_peer_ref(r->peer), mtu, NULL,
+					  &cie_nbma_nhs);
 }
 
 static int nhrp_reg_timeout(struct thread *t)
@@ -111,7 +122,7 @@ static int nhrp_reg_timeout(struct thread *t)
 		c = nhrp_cache_get(r->nhs->ifp, &r->proto_addr, 0);
 		if (c)
 			nhrp_cache_update_binding(c, NHRP_CACHE_NHS, -1, NULL,
-						  0, NULL);
+						  0, NULL, NULL);
 		sockunion_family(&r->proto_addr) = AF_UNSPEC;
 	}
 
@@ -162,7 +173,7 @@ static int nhrp_reg_send_req(struct thread *t)
 	struct interface *ifp = nhs->ifp;
 	struct nhrp_interface *nifp = ifp->info;
 	struct nhrp_afi_data *if_ad = &nifp->afi[nhs->afi];
-	union sockunion *dst_proto;
+	union sockunion *dst_proto, nhs_proto;
 	struct zbuf *zb;
 	struct nhrp_packet_header *hdr;
 	struct nhrp_extension_header *ext;
@@ -207,17 +218,34 @@ static int nhrp_reg_send_req(struct thread *t)
 	/* FIXME: push CIE for each local protocol address */
 	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, NULL, NULL);
 	/* RFC2332 5.2.1 if unique is set then prefix length must be 0xff */
-	cie->prefix_length = (if_ad->flags & NHRP_IFF_REG_NO_UNIQUE) ? 8 * sockunion_get_addrlen(dst_proto) : 0xff;
+	cie->prefix_length = (if_ad->flags & NHRP_IFF_REG_NO_UNIQUE)
+				     ? 8 * sockunion_get_addrlen(dst_proto)
+				     : 0xff;
 	cie->holding_time = htons(if_ad->holdtime);
 	cie->mtu = htons(if_ad->mtu);
 
 	nhrp_ext_request(zb, hdr, ifp);
 
 	/* Cisco NAT detection extension */
+	if (sockunion_family(&r->proto_addr) != AF_UNSPEC) {
+		nhs_proto = r->proto_addr;
+	} else if (sockunion_family(&nhs->proto_addr) != AF_UNSPEC) {
+		nhs_proto = nhs->proto_addr;
+	} else {
+		/* cisco magic: If NHS is not known then use all 0s as
+		 * client protocol address in NAT Extension header
+		 */
+		memset(&nhs_proto, 0, sizeof(nhs_proto));
+		sockunion_family(&nhs_proto) = afi2family(nhs->afi);
+	}
+
 	hdr->flags |= htons(NHRP_FLAG_REGISTRATION_NAT);
 	ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
-	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &if_ad->addr);
+	/* push NHS details */
+	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &r->peer->vc->remote.nbma,
+			    &nhs_proto);
 	cie->prefix_length = 8 * sockunion_get_addrlen(&if_ad->addr);
+	cie->mtu = htons(if_ad->mtu);
 	nhrp_ext_complete(zb, ext);
 
 	nhrp_packet_complete(zb, hdr);
@@ -442,3 +470,29 @@ void nhrp_nhs_foreach(struct interface *ifp, afi_t afi,
 			cb(nhs, 0, ctx);
 	}
 }
+
+int nhrp_nhs_match_ip(union sockunion *in_ip, struct nhrp_interface *nifp)
+{
+	int i;
+	struct nhrp_nhs *nhs;
+	struct nhrp_registration *reg;
+
+	for (i = 0; i < AFI_MAX; i++) {
+		list_for_each_entry(nhs, &nifp->afi[i].nhslist_head,
+				    nhslist_entry)
+		{
+			if (!list_empty(&nhs->reglist_head)) {
+				list_for_each_entry(reg, &nhs->reglist_head,
+						    reglist_entry)
+				{
+					if (!sockunion_cmp(
+						    in_ip,
+						    &reg->peer->vc->remote
+							     .nbma))
+						return 1;
+				}
+			}
+		}
+	}
+	return 0;
+}
diff --git a/nhrpd/nhrp_packet.c b/nhrpd/nhrp_packet.c
index a983aa71b..fd77533c8 100644
--- a/nhrpd/nhrp_packet.c
+++ b/nhrpd/nhrp_packet.c
@@ -268,6 +268,7 @@ int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr,
 				    &ad->addr);
 		if (!cie)
 			goto err;
+		cie->mtu = htons(ad->mtu);
 		cie->holding_time = htons(ad->holdtime);
 		break;
 	default:
diff --git a/nhrpd/nhrp_peer.c b/nhrpd/nhrp_peer.c
index af352c68e..199f3332d 100644
--- a/nhrpd/nhrp_peer.c
+++ b/nhrpd/nhrp_peer.c
@@ -47,6 +47,7 @@ static void nhrp_peer_check_delete(struct nhrp_peer *p)
 	       p->ref, &p->vc->remote.nbma, &p->vc->local.nbma);
 
 	THREAD_OFF(p->t_fallback);
+	THREAD_OFF(p->t_timer);
 	hash_release(nifp->peer_hash, p);
 	nhrp_interface_notify_del(p->ifp, &p->ifp_notifier);
 	nhrp_vc_notify_del(p->vc, &p->vc_notifier);
@@ -182,8 +183,7 @@ static void *nhrp_peer_create(void *data)
 		.ref = 0,
 		.ifp = key->ifp,
 		.vc = key->vc,
-		.notifier_list =
-		NOTIFIER_LIST_INITIALIZER(&p->notifier_list),
+		.notifier_list = NOTIFIER_LIST_INITIALIZER(&p->notifier_list),
 	};
 	nhrp_vc_notify_add(p->vc, &p->vc_notifier, nhrp_peer_vc_notify);
 	nhrp_interface_notify_add(p->ifp, &p->ifp_notifier,
@@ -283,6 +283,30 @@ static int nhrp_peer_request_timeout(struct thread *t)
 	return 0;
 }
 
+static int nhrp_peer_defer_vici_request(struct thread *t)
+{
+	struct nhrp_peer *p = THREAD_ARG(t);
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+
+	THREAD_OFF(p->t_timer);
+
+	if (p->online) {
+		debugf(NHRP_DEBUG_COMMON,
+		       "IPsec connection to %pSU already established",
+		       &vc->remote.nbma);
+	} else {
+		vici_request_vc(nifp->ipsec_profile, &vc->local.nbma,
+				&vc->remote.nbma, p->prio);
+		thread_add_timer(
+			master, nhrp_peer_request_timeout, p,
+			(nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30,
+			&p->t_fallback);
+	}
+	return 0;
+}
+
 int nhrp_peer_check(struct nhrp_peer *p, int establish)
 {
 	struct nhrp_vc *vc = p->vc;
@@ -304,11 +328,25 @@ int nhrp_peer_check(struct nhrp_peer *p, int establish)
 
 	p->prio = establish > 1;
 	p->requested = 1;
-	vici_request_vc(nifp->ipsec_profile, &vc->local.nbma, &vc->remote.nbma,
-			p->prio);
-	thread_add_timer(master, nhrp_peer_request_timeout, p,
-			 (nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30,
-			 &p->t_fallback);
+
+	/* All NHRP registration requests are prioritized */
+	if (p->prio) {
+		vici_request_vc(nifp->ipsec_profile, &vc->local.nbma,
+				&vc->remote.nbma, p->prio);
+		thread_add_timer(
+			master, nhrp_peer_request_timeout, p,
+			(nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30,
+			&p->t_fallback);
+	} else {
+		/* Maximum timeout is 1 second */
+		int r_time_ms = rand() % 1000;
+
+		debugf(NHRP_DEBUG_COMMON,
+		       "Initiating IPsec connection request to %pSU after %d ms:",
+		       &vc->remote.nbma, r_time_ms);
+		thread_add_timer_msec(master, nhrp_peer_defer_vici_request,
+				      p, r_time_ms, &p->t_timer);
+	}
 
 	return 0;
 }
@@ -341,6 +379,60 @@ void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb)
 	zbuf_reset(zb);
 }
 
+static void nhrp_process_nat_extension(struct nhrp_packet_parser *pp,
+				       union sockunion *proto,
+				       union sockunion *cie_nbma)
+{
+	union sockunion cie_proto;
+	struct zbuf payload;
+	struct nhrp_extension_header *ext;
+	struct zbuf *extensions;
+
+	if (!cie_nbma)
+		return;
+
+	sockunion_family(cie_nbma) = AF_UNSPEC;
+
+	if (!proto || sockunion_family(proto) == AF_UNSPEC)
+		return;
+
+	/* Handle extensions */
+	extensions = zbuf_alloc(zbuf_used(&pp->extensions));
+	if (extensions) {
+		zbuf_copy_peek(extensions, &pp->extensions,
+				 zbuf_used(&pp->extensions));
+		while ((ext = nhrp_ext_pull(extensions, &payload)) != NULL) {
+			switch (htons(ext->type)
+				& ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+			case NHRP_EXTENSION_NAT_ADDRESS:
+				/* Process the NBMA and proto address in NAT
+				 * extension and update the cache without which
+				 * the neighbor table in the kernel contains the
+				 * source NBMA address which is not reachable
+				 * since it is behind a NAT device
+				 */
+				debugf(NHRP_DEBUG_COMMON,
+				       "shortcut res_resp: Processing NAT Extension for %pSU",
+				       proto);
+				while (nhrp_cie_pull(&payload, pp->hdr,
+						     cie_nbma, &cie_proto)) {
+					if (sockunion_family(&cie_proto)
+					    == AF_UNSPEC)
+						continue;
+
+					if (!sockunion_cmp(proto, &cie_proto)) {
+						debugf(NHRP_DEBUG_COMMON,
+						       "\tcie_nbma for proto %pSU is %pSU",
+						       proto, cie_nbma);
+						break;
+					}
+				}
+			}
+		}
+		zbuf_free(extensions);
+	}
+}
+
 static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 {
 	struct interface *ifp = pp->ifp;
@@ -349,12 +441,12 @@ static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 	struct nhrp_cie_header *cie;
 	struct nhrp_extension_header *ext;
 	struct nhrp_cache *c;
-	union sockunion cie_nbma, cie_proto, *proto_addr, *nbma_addr;
+	union sockunion cie_nbma, cie_nbma_nat, cie_proto, *proto_addr,
+		*nbma_addr, *claimed_nbma_addr;
 	int holdtime, prefix_len, hostprefix_len;
 	struct nhrp_interface *nifp = ifp->info;
 	struct nhrp_peer *peer;
 	size_t paylen;
-	char buf[SU_ADDRSTRLEN];
 
 	if (!(pp->if_ad->flags & NHRP_IFF_SHORTCUT)) {
 		debugf(NHRP_DEBUG_COMMON, "Shortcuts disabled");
@@ -406,9 +498,41 @@ static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 		proto_addr = (sockunion_family(&cie_proto) == AF_UNSPEC)
 				     ? &pp->src_proto
 				     : &cie_proto;
-		nbma_addr = (sockunion_family(&cie_nbma) == AF_UNSPEC)
-				    ? &pp->src_nbma
-				    : &cie_nbma;
+
+		/* Check for this proto_addr in NHRP_NAT_EXTENSION */
+		nhrp_process_nat_extension(pp, proto_addr, &cie_nbma_nat);
+
+		if (sockunion_family(&cie_nbma_nat) == AF_UNSPEC) {
+			/* It may be possible that this resolution reply is
+			 * coming directly from NATTED Spoke and there is not
+			 * NAT Extension present
+			 */
+			debugf(NHRP_DEBUG_COMMON,
+			       "shortcut res_rep: No NAT Extension for %pSU",
+			       proto_addr);
+
+			if (!sockunion_same(&pp->src_nbma,
+					    &pp->peer->vc->remote.nbma)
+			    && !nhrp_nhs_match_ip(&pp->peer->vc->remote.nbma,
+						  nifp)) {
+				cie_nbma_nat = pp->peer->vc->remote.nbma;
+				debugf(NHRP_DEBUG_COMMON,
+				       "shortcut res_rep: NAT detected using %pSU as cie_nbma",
+				       &cie_nbma_nat);
+			}
+		}
+
+		if (sockunion_family(&cie_nbma_nat) != AF_UNSPEC)
+			nbma_addr = &cie_nbma_nat;
+		else if (sockunion_family(&cie_nbma) != AF_UNSPEC)
+			nbma_addr = &cie_nbma;
+		else
+			nbma_addr = &pp->src_nbma;
+
+		if (sockunion_family(&cie_nbma) != AF_UNSPEC)
+			claimed_nbma_addr = &cie_nbma;
+		else
+			claimed_nbma_addr = &pp->src_nbma;
 
 		holdtime = htons(cie->holding_time);
 		debugf(NHRP_DEBUG_COMMON,
@@ -424,15 +548,14 @@ static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 			cie->code = NHRP_CODE_INSUFFICIENT_RESOURCES;
 			continue;
 		}
-		if (nbma_addr)
-			sockunion2str(nbma_addr, buf, sizeof(buf));
 
 		debugf(NHRP_DEBUG_COMMON,
-		       "shortcut res_rep: updating binding for nmba addr %s",
-		       nbma_addr ? buf : "(NULL)");
-		if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime,
-					       nhrp_peer_ref(pp->peer),
-					       htons(cie->mtu), nbma_addr)) {
+		       "shortcut res_rep: updating binding for nmba addr %pSU",
+		       nbma_addr);
+		if (!nhrp_cache_update_binding(
+			    c, NHRP_CACHE_DYNAMIC, holdtime,
+			    nhrp_peer_get(pp->ifp, nbma_addr), htons(cie->mtu),
+			    nbma_addr, claimed_nbma_addr)) {
 			cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 			continue;
 		}
@@ -468,17 +591,23 @@ static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 	while ((ext = nhrp_ext_pull(&pp->extensions, &payload)) != NULL) {
 		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 		case NHRP_EXTENSION_NAT_ADDRESS:
-			if (sockunion_family(&nifp->nat_nbma) == AF_UNSPEC)
-				break;
 			ext = nhrp_ext_push(zb, hdr,
 					    NHRP_EXTENSION_NAT_ADDRESS);
 			if (!ext)
 				goto err;
-			cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
-					    &nifp->nat_nbma, &pp->if_ad->addr);
-			if (!cie)
-				goto err;
-			nhrp_ext_complete(zb, ext);
+			if (sockunion_family(&nifp->nat_nbma) != AF_UNSPEC) {
+				cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
+						    &nifp->nat_nbma,
+						    &pp->if_ad->addr);
+				if (!cie)
+					goto err;
+				cie->prefix_length =
+					8 * sockunion_get_addrlen(
+							&pp->if_ad->addr);
+
+				cie->mtu = htons(pp->if_ad->mtu);
+				nhrp_ext_complete(zb, ext);
+			}
 			break;
 		default:
 			if (nhrp_ext_reply(zb, hdr, ifp, ext, &payload) < 0)
@@ -486,7 +615,6 @@ static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 			break;
 		}
 	}
-
 	nhrp_packet_complete(zb, hdr);
 	nhrp_peer_send(peer, zb);
 err:
@@ -559,7 +687,11 @@ static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
 				    : &cie_nbma;
 		nbma_natoa = NULL;
 		if (natted) {
-			nbma_natoa = nbma_addr;
+			nbma_natoa =
+				(sockunion_family(&p->peer->vc->remote.nbma)
+				 == AF_UNSPEC)
+					? nbma_addr
+					: &p->peer->vc->remote.nbma;
 		}
 
 		holdtime = htons(cie->holding_time);
@@ -574,7 +706,8 @@ static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
 
 		if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime,
 					       nhrp_peer_ref(p->peer),
-					       htons(cie->mtu), nbma_natoa)) {
+					       htons(cie->mtu), nbma_natoa,
+					       nbma_addr)) {
 			cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 			continue;
 		}
@@ -592,9 +725,13 @@ static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
 				goto err;
 			zbuf_copy(zb, &payload, zbuf_used(&payload));
 			if (natted) {
-				nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
-					      &p->peer->vc->remote.nbma,
-					      &p->src_proto);
+				cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
+						    &p->peer->vc->remote.nbma,
+						    &p->src_proto);
+				cie->prefix_length =
+					8 * sockunion_get_addrlen(
+						    &p->if_ad->addr);
+				cie->mtu = htons(p->if_ad->mtu);
 			}
 			nhrp_ext_complete(zb, ext);
 			break;
@@ -794,20 +931,23 @@ static struct {
 static void nhrp_peer_forward(struct nhrp_peer *p,
 			      struct nhrp_packet_parser *pp)
 {
-	struct zbuf *zb, extpl;
+	struct zbuf *zb, *zb_copy, extpl;
 	struct nhrp_packet_header *hdr;
 	struct nhrp_extension_header *ext, *dst;
 	struct nhrp_cie_header *cie;
 	struct nhrp_interface *nifp = pp->ifp->info;
 	struct nhrp_afi_data *if_ad = pp->if_ad;
-	union sockunion cie_nbma, cie_protocol;
+	union sockunion cie_nbma, cie_protocol, cie_protocol_mandatory, *proto;
 	uint16_t type, len;
+	struct nhrp_cache *c;
 
 	if (pp->hdr->hop_count == 0)
 		return;
 
 	/* Create forward packet - copy header */
 	zb = zbuf_alloc(1500);
+	zb_copy = zbuf_alloc(1500);
+
 	hdr = nhrp_packet_push(zb, pp->hdr->type, &pp->src_nbma, &pp->src_proto,
 			       &pp->dst_proto);
 	hdr->flags = pp->hdr->flags;
@@ -815,8 +955,13 @@ static void nhrp_peer_forward(struct nhrp_peer *p,
 	hdr->u.request_id = pp->hdr->u.request_id;
 
 	/* Copy payload */
+	zbuf_copy_peek(zb_copy, &pp->payload, zbuf_used(&pp->payload));
 	zbuf_copy(zb, &pp->payload, zbuf_used(&pp->payload));
 
+	/* Get CIE Extension from Mandatory part */
+	sockunion_family(&cie_protocol_mandatory) = AF_UNSPEC;
+	nhrp_cie_pull(zb_copy, pp->hdr, &cie_nbma, &cie_protocol_mandatory);
+
 	/* Copy extensions */
 	while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
 		type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
@@ -848,19 +993,82 @@ static void nhrp_peer_forward(struct nhrp_peer *p,
 						    &nifp->nbma, &if_ad->addr);
 				if (!cie)
 					goto err;
+				cie->mtu = htons(if_ad->mtu);
 				cie->holding_time = htons(if_ad->holdtime);
 			}
 			break;
+		case NHRP_EXTENSION_NAT_ADDRESS:
+			c = NULL;
+			proto = NULL;
+
+			/* If NAT extension is empty then attempt to populate
+			 * it with cached NBMA information
+			 */
+			if (len == 0) {
+				if (packet_types[hdr->type].type
+				    == PACKET_REQUEST) {
+					debugf(NHRP_DEBUG_COMMON,
+					       "Processing NHRP_EXTENSION_NAT_ADDRESS while forwarding the request packet");
+					proto = &pp->src_proto;
+				} else if (packet_types[hdr->type].type
+					   == PACKET_REPLY) {
+					debugf(NHRP_DEBUG_COMMON,
+					       "Processing NHRP_EXTENSION_NAT_ADDRESS while forwarding the reply packet");
+					/* For reply packet use protocol
+					 * specified in CIE of mandatory part
+					 * for cache lookup
+					 */
+					if (sockunion_family(
+						    &cie_protocol_mandatory)
+					    != AF_UNSPEC)
+						proto = &cie_protocol_mandatory;
+				}
+			}
+
+			if (proto) {
+				debugf(NHRP_DEBUG_COMMON, "Proto is %pSU",
+				       proto);
+				c = nhrp_cache_get(nifp->ifp, proto, 0);
+			}
+
+			if (c) {
+				debugf(NHRP_DEBUG_COMMON,
+				       "c->cur.remote_nbma_natoa is %pSU",
+				       &c->cur.remote_nbma_natoa);
+				if (sockunion_family(&c->cur.remote_nbma_natoa)
+				    != AF_UNSPEC) {
+					cie = nhrp_cie_push(
+						zb,
+						NHRP_CODE_SUCCESS,
+						&c->cur.remote_nbma_natoa,
+						proto);
+					if (!cie)
+						goto err;
+				}
+			} else {
+				if (proto)
+					debugf(NHRP_DEBUG_COMMON,
+					       "No cache entry for proto %pSU",
+					       proto);
+				/* Copy existing NAT extension to new packet if
+				 * either it was already not-empty, or we do not
+				 * have valid cache information
+				 */
+				zbuf_put(zb, extpl.head, len);
+			}
+			break;
 		default:
 			if (htons(ext->type) & NHRP_EXTENSION_FLAG_COMPULSORY)
 				/* FIXME: RFC says to just copy, but not
-				 * append our selves to the transit NHS list */
+				 * append our selves to the transit NHS list
+				 */
 				goto err;
 		/* fallthru */
 		case NHRP_EXTENSION_RESPONDER_ADDRESS:
 			/* Supported compulsory extensions, and any
 			 * non-compulsory that is not explicitly handled,
-			 * should be just copied. */
+			 * should be just copied.
+			 */
 			zbuf_copy(zb, &extpl, len);
 			break;
 		}
@@ -870,10 +1078,12 @@ static void nhrp_peer_forward(struct nhrp_peer *p,
 	nhrp_packet_complete(zb, hdr);
 	nhrp_peer_send(p, zb);
 	zbuf_free(zb);
+	zbuf_free(zb_copy);
 	return;
 err:
 	nhrp_packet_debug(pp->pkt, "FWD-FAIL");
 	zbuf_free(zb);
+	zbuf_free(zb_copy);
 }
 
 static void nhrp_packet_debug(struct zbuf *zb, const char *dir)
diff --git a/nhrpd/nhrp_shortcut.c b/nhrpd/nhrp_shortcut.c
index 8ce19cd4a..56861551e 100644
--- a/nhrpd/nhrp_shortcut.c
+++ b/nhrpd/nhrp_shortcut.c
@@ -203,17 +203,18 @@ static void nhrp_shortcut_recv_resolution_rep(struct nhrp_reqid *reqid,
 					      void *arg)
 {
 	struct nhrp_packet_parser *pp = arg;
+	struct interface *ifp = pp->ifp;
+	struct nhrp_interface *nifp = ifp->info;
 	struct nhrp_shortcut *s =
 		container_of(reqid, struct nhrp_shortcut, reqid);
 	struct nhrp_shortcut *ps;
 	struct nhrp_extension_header *ext;
 	struct nhrp_cie_header *cie;
 	struct nhrp_cache *c = NULL;
-	struct nhrp_cache *c_dst_proto = NULL;
+	struct nhrp_cache *c_dst = NULL;
 	union sockunion *proto, cie_proto, *nbma, cie_nbma, nat_nbma;
 	struct prefix prefix, route_prefix;
 	struct zbuf extpl;
-	char buf[4][SU_ADDRSTRLEN];
 	int holding_time = pp->if_ad->holdtime;
 
 	nhrp_reqid_free(&nhrp_packet_reqid, &s->reqid);
@@ -235,16 +236,6 @@ static void nhrp_shortcut_recv_resolution_rep(struct nhrp_reqid *reqid,
 		return;
 	}
 
-	/* Parse extensions */
-	memset(&nat_nbma, 0, sizeof(nat_nbma));
-	while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
-		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
-		case NHRP_EXTENSION_NAT_ADDRESS:
-			nhrp_cie_pull(&extpl, pp->hdr, &nat_nbma, &cie_proto);
-			break;
-		}
-	}
-
 	/* Minor sanity check */
 	prefix2sockunion(s->p, &cie_proto);
 	if (!sockunion_same(&cie_proto, &pp->dst_proto)) {
@@ -280,16 +271,58 @@ static void nhrp_shortcut_recv_resolution_rep(struct nhrp_reqid *reqid,
 			prefix.prefixlen = route_prefix.prefixlen;
 	}
 
-	debugf(NHRP_DEBUG_COMMON,
-	       "Shortcut: %pFX is at proto %pSU dst_proto %pSU cie-nbma %pSU nat-nbma %pSU cie-holdtime %d",
-	       &prefix, proto, &pp->dst_proto, &cie_nbma, &nat_nbma,
-	       htons(cie->holding_time));
+	/* Parse extensions */
+	memset(&nat_nbma, 0, sizeof(nat_nbma));
+	while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
+		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+		case NHRP_EXTENSION_NAT_ADDRESS: {
+			struct nhrp_cie_header *cie_nat;
+
+			do {
+				union sockunion cie_nat_proto, cie_nat_nbma;
+
+				sockunion_family(&cie_nat_proto) = AF_UNSPEC;
+				sockunion_family(&cie_nat_nbma) = AF_UNSPEC;
+				cie_nat = nhrp_cie_pull(&extpl, pp->hdr,
+							&cie_nat_nbma,
+							&cie_nat_proto);
+				/* We are interested only in peer CIE */
+				if (cie_nat
+				    && sockunion_same(&cie_nat_proto, proto)) {
+					nat_nbma = cie_nat_nbma;
+				}
+			} while (cie_nat);
+		} break;
+		default:
+			break;
+		}
+	}
 
 	/* Update cache entry for the protocol to nbma binding */
-	if (sockunion_family(&nat_nbma) != AF_UNSPEC)
+	if (sockunion_family(&nat_nbma) != AF_UNSPEC) {
+		debugf(NHRP_DEBUG_COMMON,
+		       "Shortcut: NAT detected (NAT extension) proto %pSU NBMA %pSU claimed-NBMA %pSU",
+		       proto, &nat_nbma, &cie_nbma);
 		nbma = &nat_nbma;
-	else
+	}
+	/* For NHRP resolution reply the cie_nbma in mandatory part is the
+	 * address of the actual address of the sender
+	 */
+	else if (!sockunion_same(&cie_nbma, &pp->peer->vc->remote.nbma)
+		 && !nhrp_nhs_match_ip(&pp->peer->vc->remote.nbma, nifp)) {
+		debugf(NHRP_DEBUG_COMMON,
+		       "Shortcut: NAT detected (no NAT Extension) proto %pSU NBMA %pSU claimed-NBMA %pSU",
+		       proto, &pp->peer->vc->remote.nbma, &cie_nbma);
+		nbma = &pp->peer->vc->remote.nbma;
+		nat_nbma = *nbma;
+	} else {
 		nbma = &cie_nbma;
+	}
+
+	debugf(NHRP_DEBUG_COMMON,
+	       "Shortcut: %pFX is at proto %pSU dst_proto %pSU NBMA %pSU cie-holdtime %d",
+	       &prefix, proto, &pp->dst_proto, nbma,
+	       htons(cie->holding_time));
 
 	if (sockunion_family(nbma)) {
 		c = nhrp_cache_get(pp->ifp, proto, 1);
@@ -299,25 +332,31 @@ static void nhrp_shortcut_recv_resolution_rep(struct nhrp_reqid *reqid,
 			nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC,
 						  holding_time,
 						  nhrp_peer_get(pp->ifp, nbma),
-						  htons(cie->mtu), nbma);
+						  htons(cie->mtu),
+						  nbma,
+						  &cie_nbma);
 		} else {
 			debugf(NHRP_DEBUG_COMMON,
-			       "Shortcut: no cache for nbma %s", buf[2]);
+			       "Shortcut: no cache for proto %pSU", proto);
 		}
 
 		/* Update cache binding for dst_proto as well */
-		if (proto != &pp->dst_proto) {
-			c_dst_proto = nhrp_cache_get(pp->ifp, &pp->dst_proto, 1);
-			if (c_dst_proto) {
+		if (sockunion_cmp(proto, &pp->dst_proto)) {
+			c_dst = nhrp_cache_get(pp->ifp, &pp->dst_proto, 1);
+			if (c_dst) {
 				debugf(NHRP_DEBUG_COMMON,
-			       "Shortcut: cache found, update binding");
-				nhrp_cache_update_binding(c_dst_proto, NHRP_CACHE_DYNAMIC,
+				       "Shortcut: cache found, update binding");
+				nhrp_cache_update_binding(c_dst,
+						  NHRP_CACHE_DYNAMIC,
 						  holding_time,
 						  nhrp_peer_get(pp->ifp, nbma),
-						  htons(cie->mtu), nbma);
+						  htons(cie->mtu),
+						  nbma,
+						  &cie_nbma);
 			} else {
 				debugf(NHRP_DEBUG_COMMON,
-			       "Shortcut: no cache for nbma %s", buf[2]);
+				       "Shortcut: no cache for proto %pSU",
+				       &pp->dst_proto);
 			}
 		}
 	}
@@ -356,6 +395,7 @@ static void nhrp_shortcut_send_resolution_req(struct nhrp_shortcut *s)
 	struct nhrp_afi_data *if_ad;
 	struct nhrp_peer *peer;
 	struct nhrp_cie_header *cie;
+	struct nhrp_extension_header *ext;
 
 	if (nhrp_route_address(NULL, &s->addr, NULL, &peer)
 	    != NHRP_ROUTE_NBMA_NEXTHOP)
@@ -399,7 +439,14 @@ static void nhrp_shortcut_send_resolution_req(struct nhrp_shortcut *s)
 
 	/* Cisco NAT detection extension */
 	hdr->flags |= htons(NHRP_FLAG_RESOLUTION_NAT);
-	nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+	ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+	if (sockunion_family(&nifp->nat_nbma) != AF_UNSPEC) {
+		cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nat_nbma,
+				    &if_ad->addr);
+		cie->prefix_length = 8 * sockunion_get_addrlen(&if_ad->addr);
+		cie->mtu = htons(if_ad->mtu);
+		nhrp_ext_complete(zb, ext);
+	}
 
 	nhrp_packet_complete(zb, hdr);
 
diff --git a/nhrpd/nhrp_vty.c b/nhrpd/nhrp_vty.c
index 1ea4c5e64..4358605e2 100644
--- a/nhrpd/nhrp_vty.c
+++ b/nhrpd/nhrp_vty.c
@@ -525,11 +525,11 @@ DEFUN(if_nhrp_map, if_nhrp_map_cmd,
 	c->map = 1;
 	if (type == NHRP_CACHE_LOCAL)
 		nhrp_cache_update_binding(c, NHRP_CACHE_LOCAL, 0, NULL, 0,
-					  NULL);
+					  NULL, NULL);
 	else
 		nhrp_cache_update_binding(c, NHRP_CACHE_STATIC, 0,
 					  nhrp_peer_get(ifp, &nbma_addr), 0,
-					  NULL);
+					  NULL, NULL);
 	return CMD_SUCCESS;
 }
 
@@ -565,7 +565,8 @@ DEFUN(if_no_nhrp_map, if_no_nhrp_map_cmd,
 		return CMD_SUCCESS;
 
 	nhrp_cache_update_binding(c, c->cur.type, -1,
-				  nhrp_peer_get(ifp, &nbma_addr), 0, NULL);
+				  nhrp_peer_get(ifp, &nbma_addr), 0, NULL,
+				  NULL);
 	return CMD_SUCCESS;
 }
 
@@ -629,7 +630,7 @@ static void show_ip_nhrp_cache(struct nhrp_cache *c, void *pctx)
 {
 	struct info_ctx *ctx = pctx;
 	struct vty *vty = ctx->vty;
-	char buf[2][SU_ADDRSTRLEN];
+	char buf[3][SU_ADDRSTRLEN];
 	struct json_object *json = NULL;
 
 	if (ctx->afi != family2afi(sockunion_family(&c->remote_addr)))
@@ -637,17 +638,42 @@ static void show_ip_nhrp_cache(struct nhrp_cache *c, void *pctx)
 
 
 	if (!ctx->count && !ctx->json) {
-		vty_out(vty, "%-8s %-8s %-24s %-24s %-6s %s\n", "Iface", "Type",
-			"Protocol", "NBMA", "Flags", "Identity");
+		vty_out(vty, "%-8s %-8s %-24s %-24s %-24s %-6s %s\n", "Iface",
+			"Type", "Protocol", "NBMA", "Claimed NBMA", "Flags",
+			"Identity");
 	}
 	ctx->count++;
 
 	sockunion2str(&c->remote_addr, buf[0], sizeof(buf[0]));
-	if (c->cur.peer)
-		sockunion2str(&c->cur.peer->vc->remote.nbma,
-			      buf[1], sizeof(buf[1]));
-	else
-		snprintf(buf[1], sizeof(buf[1]), "-");
+	if (c->cur.type == NHRP_CACHE_LOCAL) {
+		struct nhrp_interface *nifp = c->ifp->info;
+
+		if (sockunion_family(&nifp->nbma) != AF_UNSPEC) {
+			sockunion2str(&nifp->nbma, buf[1], sizeof(buf[1]));
+			sockunion2str(&nifp->nbma, buf[2], sizeof(buf[2]));
+		} else {
+			snprintf(buf[1], sizeof(buf[1]), "-");
+			snprintf(buf[2], sizeof(buf[2]), "-");
+		}
+
+		/* if we are behind NAT then update NBMA field */
+		if (sockunion_family(&nifp->nat_nbma) != AF_UNSPEC)
+			sockunion2str(&nifp->nat_nbma, buf[1], sizeof(buf[1]));
+	} else {
+		if (c->cur.peer)
+			sockunion2str(&c->cur.peer->vc->remote.nbma,
+				      buf[1], sizeof(buf[1]));
+		else
+			snprintf(buf[1], sizeof(buf[1]), "-");
+
+		if (c->cur.peer
+		    && sockunion_family(&c->cur.remote_nbma_claimed)
+		    != AF_UNSPEC)
+			sockunion2str(&c->cur.remote_nbma_claimed,
+				      buf[2], sizeof(buf[2]));
+		else
+			snprintf(buf[2], sizeof(buf[2]), "-");
+	}
 
 	if (ctx->json) {
 		json = json_object_new_object();
@@ -656,6 +682,7 @@ static void show_ip_nhrp_cache(struct nhrp_cache *c, void *pctx)
 				       nhrp_cache_type_str[c->cur.type]);
 		json_object_string_add(json, "protocol", buf[0]);
 		json_object_string_add(json, "nbma", buf[1]);
+		json_object_string_add(json, "claimed_nbma", buf[2]);
 
 		if (c->used)
 			json_object_boolean_true_add(json, "used");
@@ -681,9 +708,10 @@ static void show_ip_nhrp_cache(struct nhrp_cache *c, void *pctx)
 		json_object_array_add(ctx->json, json);
 		return;
 	}
-	vty_out(ctx->vty, "%-8s %-8s %-24s %-24s %c%c%c    %s\n", c->ifp->name,
+	vty_out(ctx->vty, "%-8s %-8s %-24s %-24s %-24s %c%c%c    %s\n",
+		c->ifp->name,
 		nhrp_cache_type_str[c->cur.type],
-		buf[0], buf[1],
+		buf[0], buf[1], buf[2],
 		c->used ? 'U' : ' ', c->t_timeout ? 'T' : ' ',
 		c->t_auth ? 'A' : ' ',
 		c->cur.peer ? c->cur.peer->vc->remote.id : "-");
@@ -970,7 +998,8 @@ static void clear_nhrp_cache(struct nhrp_cache *c, void *data)
 {
 	struct info_ctx *ctx = data;
 	if (c->cur.type <= NHRP_CACHE_DYNAMIC) {
-		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL,
+					  NULL);
 		ctx->count++;
 	}
 }
diff --git a/nhrpd/nhrpd.h b/nhrpd/nhrpd.h
index 9c8c293b9..e4afb22f8 100644
--- a/nhrpd/nhrpd.h
+++ b/nhrpd/nhrpd.h
@@ -156,6 +156,7 @@ struct nhrp_peer {
 	struct nhrp_vc *vc;
 	struct thread *t_fallback;
 	struct notifier_block vc_notifier, ifp_notifier;
+	struct thread *t_timer;
 };
 
 struct nhrp_packet_parser {
@@ -225,9 +226,11 @@ struct nhrp_cache {
 	struct {
 		enum nhrp_cache_type type;
 		union sockunion remote_nbma_natoa;
+		union sockunion remote_nbma_claimed;
 		struct nhrp_peer *peer;
 		time_t expires;
 		uint32_t mtu;
+		int holding_time;
 	} cur, new;
 };
 
@@ -383,7 +386,8 @@ void nhrp_cache_config_foreach(struct interface *ifp,
 void nhrp_cache_set_used(struct nhrp_cache *, int);
 int nhrp_cache_update_binding(struct nhrp_cache *, enum nhrp_cache_type type,
 			      int holding_time, struct nhrp_peer *p,
-			      uint32_t mtu, union sockunion *nbma_natoa);
+			      uint32_t mtu, union sockunion *nbma_natoa,
+			      union sockunion *claimed_nbma);
 void nhrp_cache_notify_add(struct nhrp_cache *c, struct notifier_block *,
 			   notifier_fn_t);
 void nhrp_cache_notify_del(struct nhrp_cache *c, struct notifier_block *);
@@ -465,4 +469,6 @@ void nhrp_peer_recv(struct nhrp_peer *p, struct zbuf *zb);
 void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb);
 void nhrp_peer_send_indication(struct interface *ifp, uint16_t, struct zbuf *);
 
+int nhrp_nhs_match_ip(union sockunion *in_ip, struct nhrp_interface *nifp);
+
 #endif
diff --git a/nhrpd/zbuf.c b/nhrpd/zbuf.c
index a78d827ea..a23526af4 100644
--- a/nhrpd/zbuf.c
+++ b/nhrpd/zbuf.c
@@ -230,3 +230,15 @@ void zbuf_copy(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
 		return;
 	memcpy(dst, src, len);
 }
+
+void zbuf_copy_peek(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
+{
+	const void *src;
+	void *dst;
+
+	dst = zbuf_pushn(zdst, len);
+	src = zbuf_pulln(zsrc, 0);
+	if (!dst || !src)
+		return;
+	memcpy(dst, src, len);
+}
diff --git a/nhrpd/zbuf.h b/nhrpd/zbuf.h
index e6f7101d6..d4a7c15a9 100644
--- a/nhrpd/zbuf.h
+++ b/nhrpd/zbuf.h
@@ -190,6 +190,7 @@ static inline void zbuf_put_be32(struct zbuf *zb, uint32_t val)
 }
 
 void zbuf_copy(struct zbuf *zb, struct zbuf *src, size_t len);
+void zbuf_copy_peek(struct zbuf *zdst, struct zbuf *zsrc, size_t len);
 
 void zbufq_init(struct zbuf_queue *);
 void zbufq_reset(struct zbuf_queue *);