* Add llt_clear_entry() callback which is able to do all lle

   cleanup including unlinking/freeing
* Relax locking in lltable_prefix_free_af/lltable_free
* Do not pass @llt to lle free callback: it is always NULL now.
* Unify arptimer/nd6_llinfo_timer: explicitly unlock lle avoiding
   unlock/lock sequinces
* Do not pass unlocked lle to nd6_ns_output(): add nd6_llinfo_get_holdsrc()
   to retrieve preferred source address from lle hold queue and pass it
   instead of lle.
* Finally, make nd6_create() create and return unlocked lle
* Separate defrtr handling code from nd6_free():
   use nd6_check_del_defrtr() to check if we need to keep entry instead of
    performing GC,
   use nd6_check_recalc_defrtr() to perform actual recalc on lle removal.
* Move isRouter handling from nd6_cache_lladdr() to separate
   nd6_check_router()
* Add initial code to maintain lle runtime flags in sync.

10 files changed, 547 insertions, 360 deletions

diff --git a/sys/net/if_llatbl.c b/sys/net/if_llatbl.c
index 67523695d6c5..c8b33df4339a 100644
--- a/sys/net/if_llatbl.c
+++ b/sys/net/if_llatbl.c
@@ -254,17 +254,17 @@ lltable_free(struct lltable *llt)
 	for (i = 0; i < LLTBL_HASHTBL_SIZE; i++) {
 		LIST_FOREACH_SAFE(lle, &llt->lle_head[i], lle_next, next) {
 			LLE_WLOCK(lle);
-			llt->llt_stop_timers(lle);
 			LIST_INSERT_HEAD(&dchain, lle, lle_chain);
 		}
 	}
 	IF_AFDATA_RUN_WLOCK(llt->llt_ifp);
 	llentries_unlink(&dchain);
 	IF_AFDATA_RUN_WUNLOCK(llt->llt_ifp);
-	LIST_FOREACH_SAFE(lle, &dchain, lle_chain, next)
-		llentry_free(lle);
 	IF_AFDATA_CFG_WUNLOCK(llt->llt_ifp);
 
+	LIST_FOREACH_SAFE(lle, &dchain, lle_chain, next)
+		llt->llt_clear_entry(llt, lle);
+
 	free(llt, M_LLTABLE);
 }
 
@@ -282,7 +282,6 @@ lltable_prefix_free_af(struct lltable *llt, const struct sockaddr *prefix,
 		LIST_FOREACH_SAFE(lle, &llt->lle_head[i], lle_next, next) {
 			if (llt->llt_match_prefix(prefix, mask, flags, lle)) {
 				LLE_WLOCK(lle);
-				llt->llt_stop_timers(lle);
 				LIST_INSERT_HEAD(&dchain, lle, lle_chain);
 			}
 		}
@@ -290,9 +289,10 @@ lltable_prefix_free_af(struct lltable *llt, const struct sockaddr *prefix,
 	IF_AFDATA_RUN_WLOCK(llt->llt_ifp);
 	llentries_unlink(&dchain);
 	IF_AFDATA_RUN_WUNLOCK(llt->llt_ifp);
-	LIST_FOREACH_SAFE(lle, &dchain, lle_chain, next)
-		llentry_free(lle);
 	IF_AFDATA_CFG_WUNLOCK(llt->llt_ifp);
+
+	LIST_FOREACH_SAFE(lle, &dchain, lle_chain, next)
+		llt->llt_clear_entry(llt, lle);
 }
 
 #if 0
diff --git a/sys/net/if_llatbl.h b/sys/net/if_llatbl.h
index 13c20f08f969..c8feb8851895 100644
--- a/sys/net/if_llatbl.h
+++ b/sys/net/if_llatbl.h
@@ -68,7 +68,7 @@ struct llentry {
 	/* FIELDS PROTECTED BY LLE rwlock */
 	struct lltable		 *lle_tbl;
 	struct llentries	 *lle_head;
-	void			(*lle_free)(struct lltable *, struct llentry *);
+	void			(*lle_free)(struct llentry *);
 	struct mbuf		 *la_hold;
 	int			 la_numheld;  /* # of packets currently held */
 	time_t			 la_expire;
@@ -117,7 +117,7 @@ struct llentry {
 
 #define	LLE_FREE_LOCKED(lle) do {				\
 	if ((lle)->lle_refcnt == 1)				\
-		(lle)->lle_free((lle)->lle_tbl, (lle));		\
+		(lle)->lle_free((lle));		\
 	else {							\
 		LLE_REMREF(lle);				\
 		LLE_WUNLOCK(lle);				\
@@ -158,7 +158,7 @@ typedef int (llt_dump_entry_t)(struct lltable *, struct llentry *,
 typedef uint32_t (llt_hash_t)(const struct llentry *);
 typedef int (llt_match_prefix_t)(const struct sockaddr *,
     const struct sockaddr *, u_int, struct llentry *);
-typedef void (llt_stop_timers_t)(struct llentry *lle);
+typedef void (llt_clear_entry_t)(struct lltable *, struct llentry *);
 
 struct lltable {
 	SLIST_ENTRY(lltable)	llt_link;
@@ -172,7 +172,7 @@ struct lltable {
 	llt_dump_entry_t	*llt_dump_entry;
 	llt_hash_t		*llt_hash;
 	llt_match_prefix_t	*llt_match_prefix;
-	llt_stop_timers_t	*llt_stop_timers;
+	llt_clear_entry_t	*llt_clear_entry;
 };
 
 MALLOC_DECLARE(M_LLTABLE);
diff --git a/sys/netinet/if_ether.c b/sys/netinet/if_ether.c
index ef5d49ed3ddc..6b54bbc00caa 100644
--- a/sys/netinet/if_ether.c
+++ b/sys/netinet/if_ether.c
@@ -182,43 +182,33 @@ static void
 arptimer(void *arg)
 {
 	struct llentry *lle = (struct llentry *)arg;
+	struct lltable *llt;
 	struct ifnet *ifp;
-	size_t pkts_dropped;
-	uint16_t la_flags;
-	int state;
+	int evt;
 
 	if (lle->la_flags & LLE_STATIC) {
+		/* TODO: ensure we won't get here */
 		LLE_WUNLOCK(lle);
 		return;
 	}
 
-	if (lle->la_falgs & LLE_DELETED) {
-		/* XXX: Temporary */
+	if (lle->la_flags & LLE_DELETED) {
 		/* We have been deleted. Drop callref and return */
-		if ((lle->la_flags & LLE_CALLOUTREF) != 0) {
-			LLE_REMREF(lle);
-			lle->la_flags &= ~LLE_CALLOUTREF;
-		}
+		KASSERT((lle->la_flags & LLE_CALLOUTREF) != 0,
+		    ("arptimer was called without callout reference"));
 
-		pkts_dropped = llentry_free(lle);
-		ARPSTAT_ADD(dropped, pkts_dropped);
+		/* Assume the entry was already cleared */
+		lle->la_flags &= ~LLE_CALLOUTREF;
+		LLE_FREE_LOCKED(lle);
 		return;
 	}
 
-	/* Unlink entry */
-	IF_AFDATA_RUN_WLOCK(ifp);
-	llentry_unlink(lle);
-	IF_AFDATA_RUN_WUNLOCK(ifp);
-
-	pkts_dropped = llentry_free(lle);
-	ARPSTAT_ADD(dropped, pkts_dropped);
+	llt = lle->lle_tbl;
+	ifp = llt->llt_ifp;
 
-	la_flags = lle->la_flags;
-	state = (la_flags & LLE_DELETED) ? ARP_LLINFO_DELETED : lle->ln_state;
-	ifp = lle->lle_tbl->llt_ifp;
 	CURVNET_SET(ifp->if_vnet);
 
-	switch (state) {
+	switch (lle->ln_state) {
 	case ARP_LLINFO_REACHABLE:
 
 		/*
@@ -234,6 +224,7 @@ arptimer(void *arg)
 		lle->ln_state = ARP_LLINFO_VERIFY;
 		callout_schedule(&lle->la_timer, hz * V_arpt_rexmit);
 		LLE_WUNLOCK(lle);
+		CURVNET_RESTORE();
 		return;
 	case ARP_LLINFO_VERIFY:
 		if (lle->r_kick == 0 && lle->la_preempt > 0) {
@@ -245,60 +236,78 @@ arptimer(void *arg)
 			lle->r_kick = 1;
 			callout_schedule(&lle->la_timer, hz * V_arpt_rexmit);
 			LLE_WUNLOCK(lle);
+			CURVNET_RESTORE();
 			return;
 		}
 		/* Nothing happened. Reschedule if not too late */
 		if (lle->la_expire > time_uptime) {
 			callout_schedule(&lle->la_timer, hz * V_arpt_rexmit);
 			LLE_WUNLOCK(lle);
+			CURVNET_RESTORE();
 			return;
 		}
 		break;
 	case ARP_LLINFO_INCOMPLETE:
-	case ARP_LLINFO_DELETED:
 		break;
 	}
 
-	if ((lle->la_flags & LLE_DELETED) == 0) {
-		int evt;
+	/* We have to delete entr */
+	if (lle->la_flags & LLE_VALID)
+		evt = LLENTRY_EXPIRED;
+	else
+		evt = LLENTRY_TIMEDOUT;
+	EVENTHANDLER_INVOKE(lle_event, lle, evt);
 
-		if (lle->la_flags & LLE_VALID)
-			evt = LLENTRY_EXPIRED;
-		else
-			evt = LLENTRY_TIMEDOUT;
-		EVENTHANDLER_INVOKE(lle_event, lle, evt);
-	}
+	llt->llt_clear_entry(llt, lle);
 
-	callout_stop(&lle->la_timer);
+	ARPSTAT_INC(timeouts);
 
-	/* XXX: LOR avoidance. We still have ref on lle. */
-	LLE_WUNLOCK(lle);
-	IF_AFDATA_CFG_WLOCK(ifp);
-	LLE_WLOCK(lle);
+	CURVNET_RESTORE();
+}
 
-	/*
-	 * Note other thread could have removed given entry
-	 * stopping callout and removing LLE reference.
-	 */
-	//llt->llt_stop_timers(lle);
-	if ((lle->la_flags & LLE_CALLOUTREF) != 0) {
+/*
+ * Calback for lltable.
+ */
+void
+arp_lltable_clear_entry(struct lltable *llt, struct llentry *lle)
+{
+	struct ifnet *ifp;
+	size_t pkts_dropped;
+
+	LLE_WLOCK_ASSERT(lle);
+	KASSERT(llt != NULL, ("lltable is NULL"));
+
+	/* Unlink entry from table if not already */
+	if ((lle->la_flags & LLE_LINKED) != 0) {
+
+		ifp = llt->llt_ifp;
+		/*
+		 * Lock order needs to be maintained
+		 */
+		LLE_ADDREF(lle);
+		LLE_WUNLOCK(lle);
+		IF_AFDATA_CFG_WLOCK(ifp);
+		LLE_WLOCK(lle);
 		LLE_REMREF(lle);
-		lle->la_flags &= ~LLE_CALLOUTREF;
+
+		IF_AFDATA_RUN_WLOCK(ifp);
+		llentry_unlink(lle);
+		IF_AFDATA_RUN_WUNLOCK(ifp);
+		
+		IF_AFDATA_CFG_WUNLOCK(ifp);
 	}
 
-	/* Unlink entry */
-	IF_AFDATA_RUN_WLOCK(ifp);
-	llentry_unlink(lle);
-	IF_AFDATA_RUN_WUNLOCK(ifp);
+	/* cancel timer */
+	if (callout_stop(&lle->la_timer) != 0) {
+		if ((lle->la_flags & LLE_CALLOUTREF) != 0) {
+			LLE_REMREF(lle);
+			lle->la_flags &= ~LLE_CALLOUTREF;
+		}
+	}
 
+	/* Finally, free entry */
 	pkts_dropped = llentry_free(lle);
 	ARPSTAT_ADD(dropped, pkts_dropped);
-
-	IF_AFDATA_CFG_WUNLOCK(ifp);
-
-	ARPSTAT_INC(timeouts);
-
-	CURVNET_RESTORE();
 }
 
 /*
diff --git a/sys/netinet/if_ether.h b/sys/netinet/if_ether.h
index 03d1e9d15937..82aafee61aaf 100644
--- a/sys/netinet/if_ether.h
+++ b/sys/netinet/if_ether.h
@@ -112,6 +112,7 @@ struct sockaddr_inarp {
 extern u_char	ether_ipmulticast_min[ETHER_ADDR_LEN];
 extern u_char	ether_ipmulticast_max[ETHER_ADDR_LEN];
 
+struct lltable;
 struct llentry;
 struct ifaddr;
 
@@ -124,6 +125,7 @@ void	arprequest(struct ifnet *, const struct in_addr *,
 void	arp_ifinit(struct ifnet *, struct ifaddr *);
 void	arp_ifinit2(struct ifnet *, struct ifaddr *, u_char *);
 void	arp_ifscrub(struct ifnet *, uint32_t);
+void	arp_lltable_clear_entry(struct lltable *, struct llentry *);
 #endif
 
 #endif
diff --git a/sys/netinet/in.c b/sys/netinet/in.c
index b6629fc4c3fa..1efb9f4f5f12 100644
--- a/sys/netinet/in.c
+++ b/sys/netinet/in.c
@@ -994,14 +994,15 @@ struct in_llentry {
 };
 
 /*
- * Deletes an address from the address table.
+ * Frees unlinked record.
  * This function is called by the timer functions
  * such as arptimer() and nd6_llinfo_timer(), and
  * the caller does the locking.
  */
 static void
-in_lltable_free(struct lltable *llt, struct llentry *lle)
+in_lltable_free(struct llentry *lle)
 {
+
 	LLE_WUNLOCK(lle);
 	LLE_LOCK_DESTROY(lle);
 	free(lle, M_LLTABLE);
@@ -1035,17 +1036,6 @@ in_lltable_new(const struct sockaddr *l3addr, u_int flags)
 	return (&lle->base);
 }
 
-static void
-in_lltable_stop_timers(struct llentry *lle)
-{
-
-	LLE_WLOCK_ASSERT(lle);
-	if (callout_stop(&lle->la_timer)) {
-		LLE_REMREF(lle);
-		lle->la_flags &= ~LLE_CALLOUTREF;
-	}
-}
-
 #define IN_ARE_MASKED_ADDR_EQUAL(d, a, m)	(			\
 	    (((ntohl((d)->sin_addr.s_addr) ^ (a)->sin_addr.s_addr) & (m)->sin_addr.s_addr)) == 0 )
 
@@ -1314,7 +1304,7 @@ in_domifattach(struct ifnet *ifp)
 		llt->llt_delete = in_lltable_delete;
 		llt->llt_dump_entry = in_lltable_dump_entry;
 		llt->llt_hash = in_lltable_hash;
-		llt->llt_stop_timers = in_lltable_stop_timers;
+		llt->llt_clear_entry = arp_lltable_clear_entry;
 		llt->llt_match_prefix = in_lltable_match_prefix;
 	}
 	ii->ii_llt = llt;
diff --git a/sys/netinet/toecore.c b/sys/netinet/toecore.c
index 9493463a3d54..0ee1e548f2be 100644
--- a/sys/netinet/toecore.c
+++ b/sys/netinet/toecore.c
@@ -37,6 +37,8 @@ __FBSDID("$FreeBSD$");
 #include <sys/mbuf.h>
 #include <sys/module.h>
 #include <sys/types.h>
+#include <sys/lock.h>
+#include <sys/rmlock.h>
 #include <sys/sockopt.h>
 #include <sys/sysctl.h>
 #include <sys/socket.h>
@@ -453,7 +455,7 @@ toe_route_redirect_event(void *arg __unused, struct nhopu_extended *nh0,
 static int
 toe_nd6_resolve(struct ifnet *ifp, struct sockaddr *sa, uint8_t *lladdr)
 {
-	struct llentry *lle;
+	struct llentry *lle, *lle_tmp;
 	struct sockaddr_in6 *sin6 = (void *)sa;
 	int rc, flags = 0;
 
@@ -462,19 +464,39 @@ restart:
 	lle = lltable_lookup_lle(LLTABLE6(ifp), flags, sa);
 	IF_AFDATA_RUNLOCK(ifp);
 	if (lle == NULL) {
-		IF_AFDATA_CFG_WLOCK(ifp);
 		lle = nd6_create(&sin6->sin6_addr, 0, ifp);
-		IF_AFDATA_CFG_WUNLOCK(ifp);
 		if (lle == NULL)
 			return (ENOMEM); /* Couldn't create entry in cache. */
 		lle->ln_state = ND6_LLINFO_INCOMPLETE;
-		nd6_llinfo_settimer_locked(lle,
-		    (long)ND_IFINFO(ifp)->retrans * hz / 1000);
-		LLE_WUNLOCK(lle);
+		IF_AFDATA_CFG_WLOCK(ifp);
+		LLE_WLOCK(lle);
+		/* Check if the same record was addded */
+		lle_tmp = lltable_lookup_lle(LLTABLE6(ifp), LLE_EXCLUSIVE, sa);
+		if (lle_tmp == NULL) {
+			/*
+			 * No entry has been found. Link new one.
+			 */
+			IF_AFDATA_RUN_WLOCK(ifp);
+			llentry_link(LLTABLE6(ifp), lle);
+			IF_AFDATA_RUN_WUNLOCK(ifp);
+		}
+		IF_AFDATA_CFG_WUNLOCK(ifp);
+		if (lle_tmp == NULL) {
+			/* Set up timer for our new lle */
+			nd6_llinfo_settimer_locked(lle,
+			    (long)ND_IFINFO(ifp)->retrans * hz / 1000);
+			LLE_WUNLOCK(lle);
 
-		nd6_ns_output(ifp, NULL, &sin6->sin6_addr, NULL, 0);
+			nd6_ns_output(ifp, NULL, &sin6->sin6_addr, NULL, 0);
+
+			return (EWOULDBLOCK);
+		}
 
-		return (EWOULDBLOCK);
+		/* Existing lle has been found. Free new one */
+		LLE_FREE_LOCKED(lle);
+		lle = lle_tmp;
+		lle_tmp = NULL;
+		flags |= LLE_EXCLUSIVE;
 	}
 
 	if (lle->ln_state == ND6_LLINFO_STALE) {
diff --git a/sys/netinet6/in6.c b/sys/netinet6/in6.c
index 9a2998c438b3..14550d8591e3 100644
--- a/sys/netinet6/in6.c
+++ b/sys/netinet6/in6.c
@@ -2052,14 +2052,12 @@ struct in6_llentry {
 };
 
 /*
- * Deletes an address from the address table.
- * This function is called by the timer functions
- * such as arptimer() and nd6_llinfo_timer(), and
- * the caller does the locking.
+ * Frees already unlinked @lle.
  */
 static void
-in6_lltable_free(struct lltable *llt, struct llentry *lle)
+in6_lltable_free(struct llentry *lle)
 {
+
 	LLE_WUNLOCK(lle);
 	LLE_LOCK_DESTROY(lle);
 	free(lle, M_LLTABLE);
@@ -2087,17 +2085,6 @@ in6_lltable_new(const struct sockaddr *l3addr, u_int flags)
 	return (&lle->base);
 }
 
-static void
-in6_lltable_stop_timers(struct llentry *lle)
-{
-
-	LLE_WLOCK_ASSERT(lle);
-	if (callout_stop(&lle->la_timer)) {
-		LLE_REMREF(lle);
-		lle->la_flags &= ~LLE_CALLOUTREF;
-	}
-}
-
 static int
 in6_lltable_match_prefix(const struct sockaddr *prefix,
     const struct sockaddr *mask, u_int flags, struct llentry *lle)
@@ -2219,19 +2206,13 @@ static struct llentry *
 in6_lltable_create(struct lltable *llt, u_int flags,
 	const struct sockaddr *l3addr)
 {
-	const struct sockaddr_in6 *sin6 = (const struct sockaddr_in6 *)l3addr;
 	struct ifnet *ifp = llt->llt_ifp;
 	struct llentry *lle;
 
 	KASSERT(l3addr->sa_family == AF_INET6,
 	    ("sin_family %d", l3addr->sa_family));
 
-	lle = in6_lltable_find_dst(llt, &sin6->sin6_addr);
-
-	if (lle != NULL) {
-		LLE_WLOCK(lle);
-		return (lle);
-	}
+	IF_AFDATA_CFG_UNLOCK_ASSERT(ifp);
 
 	/*
 	 * A route that covers the given address must have
@@ -2248,7 +2229,6 @@ in6_lltable_create(struct lltable *llt, u_int flags,
 		return NULL;
 	}
 	lle->la_flags = flags;
-	LLE_WLOCK(lle);
 
 	return (lle);
 }
@@ -2381,7 +2361,7 @@ in6_domifattach(struct ifnet *ifp)
 		ext->lltable->llt_delete = in6_lltable_delete;
 		ext->lltable->llt_dump_entry = in6_lltable_dump_entry;
 		ext->lltable->llt_hash = in6_lltable_hash;
-		ext->lltable->llt_stop_timers = in6_lltable_stop_timers;
+		ext->lltable->llt_clear_entry = nd6_lltable_clear_entry;
 		ext->lltable->llt_match_prefix = in6_lltable_match_prefix;
 	}
 
diff --git a/sys/netinet6/nd6.c b/sys/netinet6/nd6.c
index 907ca54a42fb..0ab6f306f4f4 100644
--- a/sys/netinet6/nd6.c
+++ b/sys/netinet6/nd6.c
@@ -132,10 +132,14 @@ static int nd6_is_new_addr_neighbor(struct sockaddr_in6 *,
 static void nd6_setmtu0(struct ifnet *, struct nd_ifinfo *);
 static void nd6_slowtimo(void *);
 static int regen_tmpaddr(struct in6_ifaddr *);
-static struct llentry *nd6_free(struct llentry *, int);
+static void nd6_free(struct llentry *, int);
 static void nd6_llinfo_timer(void *);
 static void clear_llinfo_pqueue(struct llentry *);
 static void nd6_rtrequest(int, struct rtentry *, struct rt_addrinfo *);
+static struct in6_addr *nd6_llinfo_get_holdsrc(struct llentry *,
+    struct in6_addr *);
+static int nd6_check_del_defrtr(struct lltable *, struct llentry *);
+static void nd6_check_recalc_defrtr(struct lltable *, struct llentry *);
 
 static VNET_DEFINE(struct callout, nd6_slowtimo_ch);
 #define	V_nd6_slowtimo_ch		VNET(nd6_slowtimo_ch)
@@ -433,6 +437,10 @@ nd6_llinfo_settimer_locked(struct llentry *ln, long tick)
 		ln->la_expire = 0;
 		ln->ln_ntick = 0;
 		canceled = callout_stop(&ln->ln_timer_ch);
+		if (canceled != 0) {
+			ln->la_flags &= ~LLE_CALLOUTREF;
+			LLE_REMREF(ln);
+		}
 	} else {
 		ln->la_expire = time_uptime + tick / hz;
 		LLE_ADDREF(ln);
@@ -445,11 +453,11 @@ nd6_llinfo_settimer_locked(struct llentry *ln, long tick)
 			canceled = callout_reset(&ln->ln_timer_ch, tick,
 			    nd6_llinfo_timer, ln);
 		}
+		if (canceled != 0)
+			LLE_REMREF(ln);
+		else
+			ln->la_flags |= LLE_CALLOUTREF;
 	}
-	if (canceled)
-		LLE_REMREF(ln);
-	else
-		ln->la_flags |= LLE_CALLOUTREF;
 }
 
 void
@@ -461,6 +469,34 @@ nd6_llinfo_settimer(struct llentry *ln, long tick)
 	LLE_WUNLOCK(ln);
 }
 
+/*
+ * Gets source address of the first packet in hold queue
+ * and stores it in @src.
+ * Returns pointer to @src (if hold queue is not empty) or NULL.
+ *
+ */
+static struct in6_addr *
+nd6_llinfo_get_holdsrc(struct llentry *ln, struct in6_addr *src)
+{
+	struct ip6_hdr *phdr;
+
+	if (ln->la_hold == NULL)
+		return (NULL);
+
+	/*
+	 * assuming every packet in la_hold has the same IP
+	 * header
+	 */
+	phdr = mtod(ln->la_hold, struct ip6_hdr *);
+	/* XXX pullup? */
+	if (sizeof(*phdr) < ln->la_hold->m_len) {
+		*src = phdr->ip6_src;
+		return (src);
+	}
+
+	return (NULL);
+}
+
 static void
 nd6_llinfo_timer(void *arg)
 {
@@ -468,13 +504,28 @@ nd6_llinfo_timer(void *arg)
 	struct in6_addr *dst;
 	struct ifnet *ifp;
 	struct nd_ifinfo *ndi = NULL;
+	struct in6_addr src, *psrc;
 
 	KASSERT(arg != NULL, ("%s: arg NULL", __func__));
 	ln = (struct llentry *)arg;
 	LLE_WLOCK_ASSERT(ln);
-	ifp = ln->lle_tbl->llt_ifp;
 
-	CURVNET_SET(ifp->if_vnet);
+	if (ln->la_flags & LLE_STATIC) {
+		/* TODO: ensure we won't get here */
+		LLE_WUNLOCK(ln);
+		return;
+	}
+
+	if (ln->la_flags & LLE_DELETED) {
+		/* We have been deleted. Drop callref and return */
+		KASSERT((ln->la_flags & LLE_CALLOUTREF) != 0,
+		    ("nd6_llinfo_timer was called without callout reference"));
+
+		/* Assume the entry was already cleared */
+		ln->la_flags &= ~LLE_CALLOUTREF;
+		LLE_FREE_LOCKED(ln);
+		return;
+	}
 
 	if (ln->ln_ntick > 0) {
 		if (ln->ln_ntick > INT_MAX) {
@@ -484,29 +535,27 @@ nd6_llinfo_timer(void *arg)
 			ln->ln_ntick = 0;
 			nd6_llinfo_settimer_locked(ln, ln->ln_ntick);
 		}
-		goto done;
+		LLE_WUNLOCK(ln);
+		return;
 	}
 
+	ifp = ln->lle_tbl->llt_ifp;
+	CURVNET_SET(ifp->if_vnet);
+
 	ndi = ND_IFINFO(ifp);
 	dst = &L3_ADDR_SIN6(ln)->sin6_addr;
-	if (ln->la_flags & LLE_STATIC) {
-		goto done;
-	}
-
-	if (ln->la_flags & LLE_DELETED) {
-		(void)nd6_free(ln, 0);
-		ln = NULL;
-		goto done;
-	}
 
+	/*
+	 * Each case statement needs to unlock @ln before break/return.
+	 */
 	switch (ln->ln_state) {
 	case ND6_LLINFO_INCOMPLETE:
 		if (ln->la_asked < V_nd6_mmaxtries) {
 			ln->la_asked++;
 			nd6_llinfo_settimer_locked(ln, (long)ndi->retrans * hz / 1000);
+			psrc = nd6_llinfo_get_holdsrc(ln, &src);
 			LLE_WUNLOCK(ln);
-			nd6_ns_output(ifp, NULL, dst, ln, 0);
-			LLE_WLOCK(ln);
+			nd6_ns_output(ifp, NULL, dst, psrc, 0);
 		} else {
 			struct mbuf *m = ln->la_hold;
 			if (m) {
@@ -522,7 +571,7 @@ nd6_llinfo_timer(void *arg)
 				clear_llinfo_pqueue(ln);
 			}
 			EVENTHANDLER_INVOKE(lle_event, ln, LLENTRY_TIMEDOUT);
-			(void)nd6_free(ln, 0);
+			nd6_free(ln, 0);
 			ln = NULL;
 			if (m != NULL)
 				icmp6_error2(m, ICMP6_DST_UNREACH,
@@ -534,15 +583,17 @@ nd6_llinfo_timer(void *arg)
 			ln->ln_state = ND6_LLINFO_STALE;
 			nd6_llinfo_settimer_locked(ln, (long)V_nd6_gctimer * hz);
 		}
+		LLE_WUNLOCK(ln);
 		break;
 
 	case ND6_LLINFO_STALE:
 		/* Garbage Collection(RFC 2461 5.3) */
 		if (!ND6_LLINFO_PERMANENT(ln)) {
 			EVENTHANDLER_INVOKE(lle_event, ln, LLENTRY_EXPIRED);
-			(void)nd6_free(ln, 1);
+			nd6_free(ln, 1);
 			ln = NULL;
 		}
+		LLE_WUNLOCK(ln);
 		break;
 
 	case ND6_LLINFO_DELAY:
@@ -551,24 +602,25 @@ nd6_llinfo_timer(void *arg)
 			ln->la_asked = 1;
 			ln->ln_state = ND6_LLINFO_PROBE;
 			nd6_llinfo_settimer_locked(ln, (long)ndi->retrans * hz / 1000);
+			psrc = nd6_llinfo_get_holdsrc(ln, &src);
 			LLE_WUNLOCK(ln);
-			nd6_ns_output(ifp, dst, dst, ln, 0);
-			LLE_WLOCK(ln);
+			nd6_ns_output(ifp, dst, dst, psrc, 0);
 		} else {
 			ln->ln_state = ND6_LLINFO_STALE; /* XXX */
 			nd6_llinfo_settimer_locked(ln, (long)V_nd6_gctimer * hz);
+			LLE_WUNLOCK(ln);
 		}
 		break;
 	case ND6_LLINFO_PROBE:
 		if (ln->la_asked < V_nd6_umaxtries) {
 			ln->la_asked++;
 			nd6_llinfo_settimer_locked(ln, (long)ndi->retrans * hz / 1000);
+			psrc = nd6_llinfo_get_holdsrc(ln, &src);
 			LLE_WUNLOCK(ln);
-			nd6_ns_output(ifp, dst, dst, ln, 0);
-			LLE_WLOCK(ln);
+			nd6_ns_output(ifp, dst, dst, psrc, 0);
 		} else {
 			EVENTHANDLER_INVOKE(lle_event, ln, LLENTRY_EXPIRED);
-			(void)nd6_free(ln, 0);
+			nd6_free(ln, 0);
 			ln = NULL;
 		}
 		break;
@@ -576,9 +628,7 @@ nd6_llinfo_timer(void *arg)
 		panic("%s: paths in a dark night can be confusing: %d",
 		    __func__, ln->ln_state);
 	}
-done:
-	if (ln != NULL)
-		LLE_FREE_LOCKED(ln);
+
 	CURVNET_RESTORE();
 }
 
@@ -866,9 +916,8 @@ nd6_lookup(struct in6_addr *addr6, int flags, struct ifnet *ifp)
 	return (ln);
 }
 
-/* 
- * the caller acquires and releases the lock on the lltbls
- * Returns the llentry wlocked
+/*
+ * Creates and returns new, unlinked and unlocked lle.
  */
 struct llentry *
 nd6_create(struct in6_addr *addr6, int flags, struct ifnet *ifp)
@@ -881,15 +930,10 @@ nd6_create(struct in6_addr *addr6, int flags, struct ifnet *ifp)
 	sin6.sin6_family = AF_INET6;
 	sin6.sin6_addr = *addr6;
 
-	IF_AFDATA_CFG_WLOCK_ASSERT(ifp);
+	IF_AFDATA_CFG_UNLOCK_ASSERT(ifp);
 
 	ln = lltable_create_lle(LLTABLE6(ifp), 0, (struct sockaddr *)&sin6);
-	if (ln != NULL) {
-		IF_AFDATA_RUN_WLOCK(ifp);
-		ln->ln_state = ND6_LLINFO_NOSTATE;
-		llentry_link(LLTABLE6(ifp), ln);
-		IF_AFDATA_RUN_WUNLOCK(ifp);
-	}
+	ln->ln_state = ND6_LLINFO_NOSTATE;
 	
 	return (ln);
 }
@@ -1025,34 +1069,96 @@ nd6_is_addr_neighbor(struct sockaddr_in6 *addr, struct ifnet *ifp)
 
 /*
  * Free an nd6 llinfo entry.
- * Since the function would cause significant changes in the kernel, DO NOT
- * make it global, unless you have a strong reason for the change, and are sure
- * that the change is safe.
+ * Internal function used by timer code.
  */
-static struct llentry *
+static void
 nd6_free(struct llentry *ln, int gc)
 {
-        struct llentry *next;
-	struct nd_defrouter *dr;
+	struct lltable *llt;
+
+	LLE_WLOCK_ASSERT(ln);
+
+	if ((ln->la_flags & LLE_DELETED) != 0) {
+		/* Unlinked entry. Stop timer/callout. */
+		nd6_llinfo_settimer_locked(ln, -1);
+		llentry_free(ln);
+		return;
+	}
+
+	llt = ln->lle_tbl;
+	/* Check if we can delete/unlink given entry */
+	if (gc != 0 && nd6_check_del_defrtr(llt, ln) == 0) {
+		LLE_WUNLOCK(ln);
+		return;
+	}
+
+	llt->llt_clear_entry(ln->lle_tbl, ln);
+}
+
+/*
+ * Calback for lltable.
+ */
+void
+nd6_lltable_clear_entry(struct lltable *llt, struct llentry *ln)
+{
 	struct ifnet *ifp;
 
 	LLE_WLOCK_ASSERT(ln);
+	KASSERT(llt != NULL, ("lltable is NULL"));
 
-	/*
-	 * we used to have pfctlinput(PRC_HOSTDEAD) here.
-	 * even though it is not harmful, it was not really necessary.
-	 */
+	/* Unlink entry from table */
+	if ((ln->la_flags & LLE_LINKED) != 0) {
+
+		ifp = llt->llt_ifp;
+		/*
+		 * Lock order needs to be maintained
+		 */
+		LLE_ADDREF(ln);
+		LLE_WUNLOCK(ln);
+		IF_AFDATA_CFG_WLOCK(ifp);
+		LLE_WLOCK(ln);
+		LLE_REMREF(ln);
+
+		IF_AFDATA_RUN_WLOCK(ifp);
+		llentry_unlink(ln);
+		IF_AFDATA_RUN_WUNLOCK(ifp);
+		
+		IF_AFDATA_CFG_WUNLOCK(ifp);
+	}
 
 	/* cancel timer */
 	nd6_llinfo_settimer_locked(ln, -1);
 
-	ifp = ln->lle_tbl->llt_ifp;
+	/* Check if default router needs to be recalculated */
+	nd6_check_recalc_defrtr(llt, ln);
+
+	/* Finally, free entry */
+	llentry_free(ln);
+}
+
+/*
+ * Checks if we can delete given entry.
+ * Perfoms defrtr selection if needed.
+ *
+ * return non-zero value if lle can be deleted.
+ */
+static int
+nd6_check_del_defrtr(struct lltable *llt, struct llentry *ln)
+{
+	struct ifnet *ifp;
+	struct nd_defrouter *dr;
+	struct in6_addr dst;
+
+	ifp = llt->llt_ifp;
+	dst = L3_ADDR_SIN6(ln)->sin6_addr;
+
+	LLE_WLOCK_ASSERT(ln);
 
 	if (ND_IFINFO(ifp)->flags & ND6_IFF_ACCEPT_RTADV) {
-		dr = defrouter_lookup(&L3_ADDR_SIN6(ln)->sin6_addr, ifp);
+		dr = defrouter_lookup(&dst, ifp);
 
 		if (dr != NULL && dr->expire &&
-		    ln->ln_state == ND6_LLINFO_STALE && gc) {
+		    ln->ln_state == ND6_LLINFO_STALE) {
 			/*
 			 * If the reason for the deletion is just garbage
 			 * collection, and the neighbor is an active default
@@ -1072,11 +1178,27 @@ nd6_free(struct llentry *ln, int gc)
 				nd6_llinfo_settimer_locked(ln,
 				    (long)V_nd6_gctimer * hz);
 
-			next = LIST_NEXT(ln, lle_next);
-			LLE_REMREF(ln);
-			LLE_WUNLOCK(ln);
-			return (next);
+			return (0);
 		}
+	}
+
+	return (1);
+}
+
+static void
+nd6_check_recalc_defrtr(struct lltable *llt, struct llentry *ln)
+{
+	struct ifnet *ifp;
+	struct nd_defrouter *dr;
+	struct in6_addr dst;
+
+	ifp = llt->llt_ifp;
+	dst = L3_ADDR_SIN6(ln)->sin6_addr;
+
+	LLE_WLOCK_ASSERT(ln);
+
+	if (ND_IFINFO(ifp)->flags & ND6_IFF_ACCEPT_RTADV) {
+		dr = defrouter_lookup(&dst, ifp);
 
 		if (dr) {
 			/*
@@ -1103,6 +1225,7 @@ nd6_free(struct llentry *ln, int gc)
 			 * defrouter_select() in the block further down for calls
 			 * into nd6_lookup().  We still hold a ref.
 			 */
+			LLE_ADDREF(ln);
 			LLE_WUNLOCK(ln);
 
 			/*
@@ -1110,7 +1233,7 @@ nd6_free(struct llentry *ln, int gc)
 			 * is in the Default Router List.
 			 * See a corresponding comment in nd6_na_input().
 			 */
-			rt6_flush(&L3_ADDR_SIN6(ln)->sin6_addr, ifp);
+			rt6_flush(&dst, ifp);
 		}
 
 		if (dr) {
@@ -1128,44 +1251,11 @@ nd6_free(struct llentry *ln, int gc)
 			defrouter_select();
 		}
 
-		if (ln->ln_router || dr)
+		if (ln->ln_router || dr) {
 			LLE_WLOCK(ln);
+			LLE_REMREF(ln);
+		}
 	}
-
-	/*
-	 * Before deleting the entry, remember the next entry as the
-	 * return value.  We need this because pfxlist_onlink_check() above
-	 * might have freed other entries (particularly the old next entry) as
-	 * a side effect (XXX).
-	 */
-	next = LIST_NEXT(ln, lle_next);
-
-	/*
-	 * Save to unlock. We still hold an extra reference and will not
-	 * free(9) in llentry_free() if someone else holds one as well.
-	 */
-	LLE_WUNLOCK(ln);
-	IF_AFDATA_CFG_WLOCK(ifp);
-	LLE_WLOCK(ln);
-
-	/*
-	 * Note other thread could have removed given entry
-	 * stopping callout and removing LLE reference.
-	 */
-	if ((ln->la_flags & LLE_CALLOUTREF) != 0) {
-		LLE_REMREF(ln);
-		ln->la_flags &= ~LLE_CALLOUTREF;
-	}
-
-	IF_AFDATA_RUN_WLOCK(ifp);
-	llentry_unlink(ln);
-	IF_AFDATA_RUN_WUNLOCK(ifp);
-
-	llentry_free(ln);
-
-	IF_AFDATA_CFG_WUNLOCK(ifp);
-
-	return (next);
 }
 
 /*
@@ -1564,6 +1654,79 @@ nd6_ioctl(u_long cmd, caddr_t data, struct ifnet *ifp)
 	return (error);
 }
 
+static int
+nd6_check_router(int type, int code, int is_new, int old_addr, int new_addr,
+    int ln_router)
+{
+
+	/*
+	 * ICMP6 type dependent behavior.
+	 *
+	 * NS: clear IsRouter if new entry
+	 * RS: clear IsRouter
+	 * RA: set IsRouter if there's lladdr
+	 * redir: clear IsRouter if new entry
+	 *
+	 * RA case, (1):
+	 * The spec says that we must set IsRouter in the following cases:
+	 * - If lladdr exist, set IsRouter.  This means (1-5).
+	 * - If it is old entry (!newentry), set IsRouter.  This means (7).
+	 * So, based on the spec, in (1-5) and (7) cases we must set IsRouter.
+	 * A quetion arises for (1) case.  (1) case has no lladdr in the
+	 * neighbor cache, this is similar to (6).
+	 * This case is rare but we figured that we MUST NOT set IsRouter.
+	 *
+	 * newentry olladdr  lladdr  llchange	    NS  RS  RA	redir
+	 *							D R
+	 *	0	n	n	--	(1)	c   ?     s
+	 *	0	y	n	--	(2)	c   s     s
+	 *	0	n	y	--	(3)	c   s     s
+	 *	0	y	y	n	(4)	c   s     s
+	 *	0	y	y	y	(5)	c   s     s
+	 *	1	--	n	--	(6) c	c	c s
+	 *	1	--	y	--	(7) c	c   s	c s
+	 *
+	 *					(c=clear s=set)
+	 */
+	switch (type & 0xff) {
+	case ND_NEIGHBOR_SOLICIT:
+		/*
+		 * New entry must have is_router flag cleared.
+		 */
+		if (is_new)	/* (6-7) */
+			ln_router = 0;
+		break;
+	case ND_REDIRECT:
+		/*
+		 * If the icmp is a redirect to a better router, always set the
+		 * is_router flag.  Otherwise, if the entry is newly created,
+		 * clear the flag.  [RFC 2461, sec 8.3]
+		 */
+		if (code == ND_REDIRECT_ROUTER)
+			ln_router = 1;
+		else if (is_new) /* (6-7) */
+			ln_router = 0;
+		break;
+	case ND_ROUTER_SOLICIT:
+		/*
+		 * is_router flag must always be cleared.
+		 */
+		ln_router = 0;
+		break;
+	case ND_ROUTER_ADVERT:
+		/*
+		 * Mark an entry with lladdr as a router.
+		 */
+		if ((!is_new && (old_addr || new_addr)) ||	/* (2-5) */
+		    (is_new && new_addr)) {			/* (7) */
+			ln_router = 1;
+		}
+		break;
+	}
+
+	return (ln_router);
+}
+
 /*
  * Create neighbor cache entry and cache link-layer address,
  * on reception of inbound ND6 packets.  (RS/RA/NS/redirect)
@@ -1584,12 +1747,13 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 	int do_update;
 	int olladdr;
 	int llchange;
-	int flags;
 	int newstate = 0;
 	uint16_t router = 0;
 	struct sockaddr_in6 sin6;
 	struct mbuf *chain = NULL;
-	int static_route = 0;
+	int r_update;
+	int new_rvalid, old_rvalid;
+	struct llentry *ln_tmp;
 
 	IF_AFDATA_CFG_UNLOCK_ASSERT(ifp);
 
@@ -1609,21 +1773,19 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 	 * Spec says nothing in sections for RA, RS and NA.  There's small
 	 * description on it in NS section (RFC 2461 7.2.3).
 	 */
-	flags = lladdr ? ND6_EXCLUSIVE : 0;
 	IF_AFDATA_CFG_RLOCK(ifp);
-	ln = nd6_lookup(from, flags, ifp);
+	ln = nd6_lookup(from, ND6_EXCLUSIVE, ifp);
 	IF_AFDATA_CFG_RUNLOCK(ifp);
 	if (ln == NULL) {
-		flags |= ND6_EXCLUSIVE;
-		IF_AFDATA_CFG_WLOCK(ifp);
 		ln = nd6_create(from, 0, ifp);
-		IF_AFDATA_CFG_WUNLOCK(ifp);
+		if (ln != NULL)
+			LLE_WLOCK(ln);
 		is_newentry = 1;
 	} else {
-		/* do nothing if static ndp is set */
+		/* do nothing if record is static  */
 		if (ln->la_flags & LLE_STATIC) {
-			static_route = 1;
-			goto done;
+			LLE_WUNLOCK(ln);
+			return (NULL);
 		}
 		is_newentry = 0;
 	}
@@ -1637,6 +1799,22 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 	} else
 		llchange = 0;
 
+
+	if (!is_newentry) {
+		if ((!olladdr && lladdr != NULL) ||	/* (3) */
+		    (olladdr && lladdr != NULL && llchange)) {	/* (5) */
+			do_update = 1;
+			newstate = ND6_LLINFO_STALE;
+		} else					/* (1-2,4) */
+			do_update = 0;
+	} else {
+		do_update = 1;
+		if (lladdr == NULL)			/* (6) */
+			newstate = ND6_LLINFO_NOSTATE;
+		else					/* (7) */
+			newstate = ND6_LLINFO_STALE;
+	}
+
 	/*
 	 * newentry olladdr  lladdr  llchange	(*=record)
 	 *	0	n	n	--	(1)
@@ -1648,29 +1826,95 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 	 *	1	--	y	--	(7) * STALE
 	 */
 
-	if (lladdr) {		/* (3-5) and (7) */
+	if (lladdr != NULL || do_update != 0) {	/* (3-5) and (7) */
 		/*
 		 * Record source link-layer address
 		 * XXX is it dependent to ifp->if_type?
+		 *
+		 * We have to update either link-layer address
+		 * or state. In most cases this require cfg/runtime locks
+		 * to be held, so we have to do unlock/lock procedure to
+		 * maintain proper lock order.
 		 */
-		bcopy(lladdr, &ln->ll_addr, ifp->if_addrlen);
-		ln->la_flags |= LLE_VALID;
-		EVENTHANDLER_INVOKE(lle_event, ln, LLENTRY_RESOLVED);
-	}
 
-	if (!is_newentry) {
-		if ((!olladdr && lladdr != NULL) ||	/* (3) */
-		    (olladdr && lladdr != NULL && llchange)) {	/* (5) */
-			do_update = 1;
-			newstate = ND6_LLINFO_STALE;
-		} else					/* (1-2,4) */
-			do_update = 0;
-	} else {
-		do_update = 1;
-		if (lladdr == NULL)			/* (6) */
-			newstate = ND6_LLINFO_NOSTATE;
-		else					/* (7) */
-			newstate = ND6_LLINFO_STALE;
+		LLE_ADDREF(ln);
+		LLE_WUNLOCK(ln);
+		IF_AFDATA_CFG_WLOCK(ifp);
+		LLE_WLOCK(ln);
+		/* Check if entry got deleted */
+		if (ln->la_flags & LLE_DELETED) {
+			IF_AFDATA_CFG_WUNLOCK(ifp);
+			LLE_FREE_LOCKED(ln);
+			return (NULL);
+		}
+
+		/* Check if similar entry has been added */
+		if (is_newentry != 0 &&
+		    (ln_tmp = nd6_lookup(from, ND6_EXCLUSIVE, ifp)) != NULL) {
+			LLE_FREE_LOCKED(ln);
+			ln = ln_tmp;
+			ln_tmp = NULL;
+			is_newentry = 0;
+		}
+
+		/*
+		 * Check if we really need to update runtime data:
+		 * e.g. lladdr & r_flags
+		 */
+		r_update = 0;
+
+		/*
+		 * no old lladdr, but new one exists. (3-5),(7)
+		 */
+		if (((ln->la_flags & LLE_VALID) == 0 && lladdr != NULL) ||
+		    ((ln->la_flags & LLE_VALID) != 0 &&
+		    memcmp(lladdr, &ln->ll_addr, ifp->if_addrlen))) {
+			r_update = 1;
+		}
+
+		/*
+		 * state switch changing rlle_valid flag
+		 */
+		new_rvalid = 0;
+		if (newstate == ND6_LLINFO_REACHABLE ||
+		    newstate == ND6_LLINFO_DELAY)
+			new_rvalid = 1;
+
+		old_rvalid = (ln->r_flags & RLLE_VALID) ? 1 : 0;
+
+		if (old_rvalid != new_rvalid)
+			r_update = 1;
+
+		/* linking new entry requires runtime lock */
+		if (is_newentry != 0)
+			r_update = 1;
+
+		if (r_update != 0) {
+			IF_AFDATA_RUN_WLOCK(ifp);
+			if (is_newentry != 0)
+				llentry_link(LLTABLE6(ifp), ln);
+			if (lladdr != NULL) {
+				bcopy(lladdr, &ln->ll_addr, ifp->if_addrlen);
+				ln->la_flags |= LLE_VALID;
+			}
+			if (new_rvalid != 0)
+				ln->r_flags |= RLLE_VALID;
+			else
+				ln->r_flags &= ~RLLE_VALID;
+			IF_AFDATA_RUN_WUNLOCK(ifp);
+		}
+		IF_AFDATA_CFG_WUNLOCK(ifp);
+		LLE_REMREF(ln);
+
+		if (lladdr != NULL) {
+			/* We might still have to copy lladdr */
+			if (r_update == 0) {
+				bcopy(lladdr, &ln->ll_addr, ifp->if_addrlen);
+				ln->la_flags |= LLE_VALID;
+			}
+
+			EVENTHANDLER_INVOKE(lle_event, ln, LLENTRY_RESOLVED);
+		}
 	}
 
 	if (do_update) {
@@ -1722,82 +1966,13 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 		}
 	}
 
-	/*
-	 * ICMP6 type dependent behavior.
-	 *
-	 * NS: clear IsRouter if new entry
-	 * RS: clear IsRouter
-	 * RA: set IsRouter if there's lladdr
-	 * redir: clear IsRouter if new entry
-	 *
-	 * RA case, (1):
-	 * The spec says that we must set IsRouter in the following cases:
-	 * - If lladdr exist, set IsRouter.  This means (1-5).
-	 * - If it is old entry (!newentry), set IsRouter.  This means (7).
-	 * So, based on the spec, in (1-5) and (7) cases we must set IsRouter.
-	 * A quetion arises for (1) case.  (1) case has no lladdr in the
-	 * neighbor cache, this is similar to (6).
-	 * This case is rare but we figured that we MUST NOT set IsRouter.
-	 *
-	 * newentry olladdr  lladdr  llchange	    NS  RS  RA	redir
-	 *							D R
-	 *	0	n	n	--	(1)	c   ?     s
-	 *	0	y	n	--	(2)	c   s     s
-	 *	0	n	y	--	(3)	c   s     s
-	 *	0	y	y	n	(4)	c   s     s
-	 *	0	y	y	y	(5)	c   s     s
-	 *	1	--	n	--	(6) c	c	c s
-	 *	1	--	y	--	(7) c	c   s	c s
-	 *
-	 *					(c=clear s=set)
-	 */
-	switch (type & 0xff) {
-	case ND_NEIGHBOR_SOLICIT:
-		/*
-		 * New entry must have is_router flag cleared.
-		 */
-		if (is_newentry)	/* (6-7) */
-			ln->ln_router = 0;
-		break;
-	case ND_REDIRECT:
-		/*
-		 * If the icmp is a redirect to a better router, always set the
-		 * is_router flag.  Otherwise, if the entry is newly created,
-		 * clear the flag.  [RFC 2461, sec 8.3]
-		 */
-		if (code == ND_REDIRECT_ROUTER)
-			ln->ln_router = 1;
-		else if (is_newentry) /* (6-7) */
-			ln->ln_router = 0;
-		break;
-	case ND_ROUTER_SOLICIT:
-		/*
-		 * is_router flag must always be cleared.
-		 */
-		ln->ln_router = 0;
-		break;
-	case ND_ROUTER_ADVERT:
-		/*
-		 * Mark an entry with lladdr as a router.
-		 */
-		if ((!is_newentry && (olladdr || lladdr)) ||	/* (2-5) */
-		    (is_newentry && lladdr)) {			/* (7) */
-			ln->ln_router = 1;
-		}
-		break;
-	}
+	/* Check if we need to update router status */
+	router = nd6_check_router(type, code, is_newentry, olladdr,
+	    lladdr != NULL ? 1 : 0, ln->ln_router);
 
-	if (ln != NULL) {
-		static_route = (ln->la_flags & LLE_STATIC);
-		router = ln->ln_router;
+	ln->ln_router = router;
+	LLE_WUNLOCK(ln);
 
-		if (flags & ND6_EXCLUSIVE)
-			LLE_WUNLOCK(ln);
-		else
-			LLE_RUNLOCK(ln);
-		if (static_route)
-			ln = NULL;
-	}
 	if (chain)
 		nd6_output_flush(ifp, ifp, chain, &sin6);
 	
@@ -1825,16 +2000,6 @@ nd6_cache_lladdr(struct ifnet *ifp, struct in6_addr *from, char *lladdr,
 	}
 	
 	return (ln);
-done:	
-	if (ln != NULL) {
-		if (flags & ND6_EXCLUSIVE)
-			LLE_WUNLOCK(ln);
-		else
-			LLE_RUNLOCK(ln);
-		if (static_route)
-			ln = NULL;
-	}
-	return (ln);
 }
 
 static void
@@ -1975,6 +2140,8 @@ nd6_output_lle(struct ifnet *ifp, struct ifnet *origifp, struct mbuf *m,
 	int error = 0;
 	int has_lle = 0;
 	int ip6len;
+	struct llentry *lle_tmp;
+	struct in6_addr src, *psrc;
 
 #ifdef INVARIANTS
 	if (lle != NULL) {
@@ -2016,9 +2183,35 @@ nd6_output_lle(struct ifnet *ifp, struct ifnet *origifp, struct mbuf *m,
 			 * the condition below is not very efficient.  But we believe
 			 * it is tolerable, because this should be a rare case.
 			 */
-			IF_AFDATA_CFG_WLOCK(ifp);
 			lle = nd6_create(&dst->sin6_addr, 0, ifp);
-			IF_AFDATA_CFG_WUNLOCK(ifp);
+			if (lle != NULL) {
+				IF_AFDATA_CFG_WLOCK(ifp);
+				LLE_WLOCK(lle);
+				/* Check if the same record was addded */
+				lle_tmp = nd6_lookup(&dst->sin6_addr,
+				    ND6_EXCLUSIVE, ifp);
+				if (lle_tmp == NULL) {
+
+					/*
+					 * No entry has been found.
+					 * Link new one.
+					 */
+					IF_AFDATA_RUN_WLOCK(ifp);
+					llentry_link(LLTABLE6(ifp), lle);
+					IF_AFDATA_RUN_WUNLOCK(ifp);
+				}
+				IF_AFDATA_CFG_WUNLOCK(ifp);
+				if (lle_tmp != NULL) {
+
+					/*
+					 * Existing lle has been found.
+					 * Free new one.
+					 */
+					LLE_FREE_LOCKED(lle);
+					lle = lle_tmp;
+					lle_tmp = NULL;
+				}
+			}
 		}
 	} 
 	if (lle == NULL) {
@@ -2106,8 +2299,9 @@ nd6_output_lle(struct ifnet *ifp, struct ifnet *origifp, struct mbuf *m,
 		
 		nd6_llinfo_settimer_locked(lle,
 		    (long)ND_IFINFO(ifp)->retrans * hz / 1000);
+		psrc = nd6_llinfo_get_holdsrc(lle, &src);
 		LLE_WUNLOCK(lle);
-		nd6_ns_output(ifp, NULL, &dst->sin6_addr, lle, 0);
+		nd6_ns_output(ifp, NULL, &dst->sin6_addr, psrc, 0);
 		if (has_lle != 0)
 			LLE_WLOCK(lle);
 	} else if (has_lle == 0) {
@@ -2269,29 +2463,35 @@ nd6_add_ifa_lle(struct in6_ifaddr *ia)
 	struct llentry *ln;
 
 	ifp = ia->ia_ifa.ifa_ifp;
-	IF_AFDATA_CFG_WLOCK(ifp);
+	ia->ia_ifa.ifa_rtrequest = nd6_rtrequest;
+
 	ln = lltable_create_lle(LLTABLE6(ifp), LLE_IFADDR,
 	    (struct sockaddr *)&ia->ia_addr);
-	if (ln != NULL) {
-		IF_AFDATA_RUN_WLOCK(ifp);
-		bcopy(IF_LLADDR(ifp), &ln->ll_addr, ifp->if_addrlen);
-		ln->la_flags |= (LLE_VALID | LLE_STATIC);
-		ln->r_flags |= RLLE_VALID;
-		ln->la_expire = 0;  /* for IPv6 this means permanent */
-		ln->ln_state = ND6_LLINFO_REACHABLE;
-		llentry_link(LLTABLE6(ifp), ln);
-		IF_AFDATA_RUN_WUNLOCK(ifp);
-	}
+	if (ln == NULL)
+		return (ENOBUFS);
 
-	ia->ia_ifa.ifa_rtrequest = nd6_rtrequest;
+	ln->la_flags |= (LLE_VALID | LLE_STATIC);
+	ln->r_flags |= RLLE_VALID;
+	ln->la_expire = 0;  /* for IPv6 this means permanent */
+	ln->ln_state = ND6_LLINFO_REACHABLE;
+
+	IF_AFDATA_CFG_WLOCK(ifp);
+	/* Lock or new shiny lle */
+	LLE_WLOCK(ln);
+
+	lltable_delete_lle(LLTABLE6(ifp), LLE_IFADDR,
+	    (struct sockaddr *)&ia->ia_addr);
+
+	bcopy(IF_LLADDR(ifp), &ln->ll_addr, ifp->if_addrlen);
+	/* Finally, link our lle to the list */
+	IF_AFDATA_RUN_WLOCK(ifp);
+	llentry_link(LLTABLE6(ifp), ln);
+	IF_AFDATA_RUN_WUNLOCK(ifp);
 	IF_AFDATA_CFG_WUNLOCK(ifp);
-	if (ln != NULL) {
-		LLE_WUNLOCK(ln);
-		in6_newaddrmsg(ia, RTM_ADD);
-		return (0);
-	}
 
-	return (ENOBUFS);
+	LLE_WUNLOCK(ln);
+	in6_newaddrmsg(ia, RTM_ADD);
+	return (0);
 }
 
 /*
diff --git a/sys/netinet6/nd6.h b/sys/netinet6/nd6.h
index 1ca7278980a9..1a1ebf042761 100644
--- a/sys/netinet6/nd6.h
+++ b/sys/netinet6/nd6.h
@@ -41,6 +41,7 @@
 #include <sys/queue.h>
 #include <sys/callout.h>
 
+struct lltable;
 struct llentry;
 
 #define ND6_LLINFO_NOSTATE	-2
@@ -419,6 +420,7 @@ int nd6_add_ifa_lle(struct in6_ifaddr *);
 void nd6_rem_ifa_lle(struct in6_ifaddr *);
 int nd6_storelladdr(struct ifnet *, struct mbuf *,
 	const struct sockaddr *, u_char *, struct llentry **);
+void nd6_lltable_clear_entry(struct lltable *, struct llentry *);
 
 /* nd6_nbr.c */
 void nd6_na_input(struct mbuf *, int, int);
@@ -426,7 +428,7 @@ void nd6_na_output(struct ifnet *, const struct in6_addr *,
 	const struct in6_addr *, u_long, int, struct sockaddr *);
 void nd6_ns_input(struct mbuf *, int, int);
 void nd6_ns_output(struct ifnet *, const struct in6_addr *,
-	const struct in6_addr *, struct llentry *, int);
+	const struct in6_addr *, const struct in6_addr *, int);
 caddr_t nd6_ifptomac(struct ifnet *);
 void nd6_dad_start(struct ifaddr *, int);
 void nd6_dad_stop(struct ifaddr *);
diff --git a/sys/netinet6/nd6_nbr.c b/sys/netinet6/nd6_nbr.c
index 60080f3c986b..39d1f4023ad2 100644
--- a/sys/netinet6/nd6_nbr.c
+++ b/sys/netinet6/nd6_nbr.c
@@ -381,7 +381,7 @@ nd6_ns_input(struct mbuf *m, int off, int icmp6len)
  */
 void
 nd6_ns_output(struct ifnet *ifp, const struct in6_addr *daddr6, 
-    const struct in6_addr *taddr6, struct llentry *ln, int dad)
+    const struct in6_addr *taddr6, const struct in6_addr *csrc, int dad)
 {
 	struct mbuf *m;
 	struct m_tag *mtag;
@@ -462,29 +462,11 @@ nd6_ns_output(struct ifnet *ifp, const struct in6_addr *daddr6,
 		 * - saddr6 belongs to the outgoing interface.
 		 * Otherwise, we perform the source address selection as usual.
 		 */
-		struct in6_addr *hsrc;
 
-		hsrc = NULL;
-		if (ln != NULL) {
-			LLE_RLOCK(ln);
-			if (ln->la_hold != NULL) {
-				struct ip6_hdr *hip6;		/* hold ip6 */
-
-				/*
-				 * assuming every packet in la_hold has the same IP
-				 * header
-				 */
-				hip6 = mtod(ln->la_hold, struct ip6_hdr *);
-				/* XXX pullup? */
-				if (sizeof(*hip6) < ln->la_hold->m_len) {
-					ip6->ip6_src = hip6->ip6_src;
-					hsrc = &hip6->ip6_src;
-				}
-			}
-			LLE_RUNLOCK(ln);
-		}
-		if (hsrc && (ifa = (struct ifaddr *)in6ifa_ifpwithaddr(ifp,
-		    hsrc)) != NULL) {
+		if (csrc != NULL)
+			ip6->ip6_src = *csrc;
+		if (csrc && (ifa = (struct ifaddr *)in6ifa_ifpwithaddr(ifp,
+		    (struct in6_addr *)csrc)) != NULL) {
 			/* ip6_src set already. */
 			ifa_free(ifa);
 		} else {