#include "include.h"

#ifdef IPV6_MULTICAST

#ifdef PROTO_INET6
#include "inet6.h"
#include "inet6_multi.h"
#endif /* PROTO_INET6 */

PROTOTYPE(mfc_delete_group_v6, static void, (group_node *));
PROTOTYPE(mfc_insert_group_v6, static group_node *, (group_node *, sockaddr_un *)); 
PROTOTYPE(mfc_insert_node_v6, static void, (group_node *, mfc *, mfc *));
PROTOTYPE(mfc_print_node_v6, static void, (mfc *, caddr_t));

static block_t mfc_node_block_index_v6;
static block_t mfc_downstream_block_index_v6;
static block_t group_node_block_index_v6;
block_t mfc_unicast_block_index_v6;  /* not static, will be used by pimv6.c */

#define MFC_NODE_FREE(dp) \
                task_block_free(mfc_node_block_index_v6, (void_t)(dp))
#define GROUP_NODE_ALLOC() \
                (task_block_alloc(group_node_block_index_v6))
#define GROUP_NODE_FREE(dp) \
                task_block_free(group_node_block_index_v6, (void_t)(dp))

/* 
 * mfc_init_v6()
 */
void
mfc_init_v6 __PF0(void)
{
    mfc_node_block_index_v6 = task_block_init(sizeof (mfc),
                                               "mfc_node_v6");
    mfc_downstream_block_index_v6 = task_block_init(sizeof (downstream),
                                               "mfc_downstream_v6");
    group_node_block_index_v6 = task_block_init(sizeof (group_node),
                                               "group_node_v6");
    mfc_unicast_block_index_v6 = task_block_init(sizeof (mfc),
                                               "mfc_unicast_v6"); 
}      

mfc *
mfc_alloc_node_v6 __PF0(void)
{
    return((mfc *) task_block_alloc(mfc_node_block_index_v6));
}

downstream *
mfc_alloc_downstream_v6  __PF0(void)
{   
    return((downstream *) task_block_alloc(mfc_downstream_block_index_v6));
}

void                                
mfc_free_downstream_v6  __PF1(ds, downstream *)
{                                     
    task_block_free(mfc_downstream_block_index_v6, ds);
}

/*                                             
 * mfc_source_link_unicast_v6()
 *
 * This routine links the mfc to the unicast route it depends on.
 * If the route changes later, then it will be easy to determine
 * if the cache entry needs to be updated or moved to a different
 * unicast route.
 */
  
void
mfc_source_link_unicast_v6 __PF2(tp, task *, mfcp, mfc *)
{
    mfc_src_list *head = 0, *msl;
    register rt_head *rth;
    register rt_list *rtl = rthlist_match(&mfcp->mfc_src);

    rt_open(tp);

    RT_LIST(rth, rtl, rt_head) {

            /*
             * Check the one thats installed in the kernel
             */

        if (krt_kernel_rt(rth)) {
            if (rtbit_isset(rth->rth_active, tp->task_rtbit)) {
                rttsi_get(rth, tp->task_rtbit, (byte *) &head);
                if (head && mfcp->msl) {
                        /*
                         * Already in table.
                         */
                    goto reset;
                }
            }
            if (!head) {
                msl = task_block_alloc(mfc_unicast_block_index_v6);
                msl->forw = msl;
                msl->back = msl;
                head = msl;

                rttsi_set(rth, tp->task_rtbit, (byte *) &head);
                rtbit_set(rth->rth_active, tp->task_rtbit);
            }
                /*
                 * add to head of list
                 */
            msl = task_block_alloc(mfc_unicast_block_index_v6);
            msl->mfcp = mfcp;
            mfcp->msl = msl;
            INSQUE(msl, head->forw);

            break;
        }
    } RT_LIST_END(rth, rtl, rt_head) ;

reset:
    RTLIST_RESET(rtl);

    /* Close the table */
    rt_close(tp, (gw_entry *) 0, 0, NULL);
}

void 
mfc_source_unlink_unicast_v6 __PF2(tp, task *, mfcp, mfc *)
{
    mfc_src_list *head = 0;
    register rt_head *rth;
    register rt_list *rtl = rthlist_match(&mfcp->mfc_src);

    rt_open(tp);

    RT_LIST(rth, rtl, rt_head) {

            /*
             * Check the one thats installed in the kernel
             */
        if (krt_kernel_rt(rth) &&
            rtbit_isset(rth->rth_active, tp->task_rtbit)) {
                
            rttsi_get(rth, tp->task_rtbit, (byte *) &head);
            assert(head);  
            assert(mfcp->msl);
            REMQUE(mfcp->msl); 
            task_block_free(mfc_unicast_block_index_v6, (void_t) mfcp->msl);
            mfcp->msl = (mfc_src_list *) 0;
            if (head->forw == head) {
                    /* 
                     * delete head of list
                     */
                task_block_free(mfc_unicast_block_index_v6, head);
                rtbit_reset(rth->rth_active, tp->task_rtbit);
                rttsi_reset(rth, tp->task_rtbit);
            }
            goto reset;
        }
    } RT_LIST_END(rth, rtl, rt_head) ;

reset:
    RTLIST_RESET(rtl);

    /* Close the table */
    rt_close(tp, (gw_entry *) 0, 0, NULL);
}

static int group_node_count_v6 = 0;

static group_node *group_node_head_v6 = NULL;

static void 
mfc_terminate_source_v6 __PF1(head, group_node *)
{
    int i, n, doing_right;
    mfc *stack[sizeof(struct in6_addr) * NBBY];
    register mfc *dp, *dprev = head->src_head;

    if (head->src_node_count == 0) {
		return;
    }

    /* Catch the case where only one node in the tree */
    if (dprev->mfc_pfx == INET6_NODE_NOMASK)
    {
		krt_delete_cache(&head->group_key, &dprev->mfc_src);
		return;
    }

    doing_right = 0;
    i = n = 0;
    for (;;) {
		if (doing_right) {
			dp = dprev->right;
		} else {
			dp = dprev->left;
		}

		if (dp->mfc_pfx <= dprev->mfc_pfx) {

			krt_delete_cache(&head->group_key, &dp->mfc_src);

			if (++n >= head->src_node_count) {
				/* All found */
				break;
			}

			if (doing_right) {
				/*
				 * Finished right head, back up stack for another
				 * node to be checked on the right.
				 */
				assert(i != 0);
				dprev = stack[-i];
			} else {
				/*
				 * Finished left, try right on next pass
				 */
				doing_right = 1;
			}
		} else {
			if (doing_right) {
				/*
				 * Node on right has children. Step down the tree,
				 * starting on the left. No need to stack the previous,
				 * we've already checked both children
				 */
				doing_right = 0;
			} else {
				/* Node on left has children, stack the previous node
				 * so we check its right child later.
				 */
				stack[i++] = dprev;
			}
			dprev = dp;
		}
    }
}

void 
mfc_terminate_v6 __PF0(void)
{
    int i, n, doing_right;
    group_node *stack[sizeof(struct in6_addr) * NBBY];
    register group_node *dp, *dprev = group_node_head_v6;

    if (group_node_count_v6 == 0) {
		return;
    }

  	/* Catch the case where only one node in the tree */
    if (dprev->group_pfx == INET6_NODE_NOMASK)
    {
		mfc_terminate_source_v6(dprev);
		return;
    }

    doing_right = 0;
    i = n = 0;
    for (;;) {
        if (doing_right) {
            dp = dprev->right;
        } else {
            dp = dprev->left;
        }

		if (dp->group_pfx <= dprev->group_pfx) {

            mfc_terminate_source_v6(dp);

            if (++n >= group_node_count_v6) {
                /* All found */
                break;
            }

            if (doing_right) {
                /*
                 * Finished right hand, back up stack for another
                 * node to be checked on the right.
                 */
                assert(i != 0);
                dprev = stack[--i];
            } else {
                /*
                 * Finished left, try right on next pass
                 */
                doing_right = 1;
            }
        } else {
            if (doing_right) {
                /*
                 * Node on right has children.  Step down the tree,
                 * starting on the left.  No need to stack the previous,
                 * we've already checked both children
                 */
                doing_right = 0;
            } else {
                /*
                 * Node on left has children, stack the previous node
                 * so we check its right child later.
                 */
                stack[i++] = dprev;
            }
            dprev = dp;
        }
    }
}   

void
mfc_visit_v6(callback, arg1)
_PROTOTYPE(callback, 
	   void,
	   (mfc *,
	    caddr_t));
caddr_t arg1;
{
    int i, n, doing_right;
    group_node *stack[sizeof(struct in6_addr) * NBBY];
    register group_node *dp, *dprev = group_node_head_v6;

    if (group_node_count_v6 == 0) {
		return;
    }

	/* Catch the case where only one node in the tree */
    if (dprev->group_pfx == INET6_NODE_NOMASK) {
		mfc_source_visit_v6(dprev, callback, arg1);
		return;
    }

    doing_right = 0;
    i = n = 0;
    for (;;) {
        if (doing_right) {
            dp = dprev->right;
        } else {
            dp = dprev->left;
        }

		if (dp->group_pfx <= dprev->group_pfx) {

            mfc_source_visit_v6(dp, callback, arg1);

            if (++n >= group_node_count_v6) {
                /* All found */
                break;
            }

            if (doing_right) {
                /*
                 * Finished right hand, back up stack for another
                 * node to be checked on the right.
                 */
                assert(i != 0);
                dprev = stack[--i];
            } else {
                /*
                 * Finished left, try right on next pass
                 */
                doing_right = 1;
            }
        } else {
            if (doing_right) {
                /*
                 * Node on right has children.  Step down the tree,
                 * starting on the left.  No need to stack the previous,
                 * we've already checked both children
                 */
                doing_right = 0;
            } else {
                /*
                 * Node on left has children, stack the previous node
                 * so we check its right child later.
                 */
                stack[i++] = dprev;
            }
            dprev = dp;
        }
    }
}

void mfc_dump_v6 __PF2(tp, task *,
		       fd, FILE *)
{
    int i, n, doing_right;
    group_node *stack[sizeof(struct in6_addr) * NBBY];
    register group_node *dp, *dprev = group_node_head_v6;

    fprintf(fd, "\tIPv6 Multicast Forwarding Cache:\n");
    if (group_node_count_v6 == 0) {
        return;
    }
    
        /* catch the case where only one node in the tree. */
    if (dprev->group_pfx == INET6_NODE_NOMASK) 
    {
        fprintf(fd, "\tGroup %A:\n", &dprev->group_key);
        mfc_source_visit_v6(dprev, mfc_print_node_v6, (caddr_t) fd);
        return;
    }
    
    doing_right = 0;
    i = n = 0;
    for (;;) {
        if (doing_right) {
            dp = dprev->right;
        } else {
            dp = dprev->left;
        }

		if (dp->group_pfx <= dprev->group_pfx) {

            fprintf(fd, "\tGroup %A: GroupPfx %d\n",  &dp->group_key, dp->group_pfx);

            mfc_source_visit_v6(dp, mfc_print_node_v6, (caddr_t) fd);

            if (++n >= group_node_count_v6) {
                /* All found */
                break;
            }

            if (doing_right) {
                /*
                 * Finished right hand, back up stack for another
                 * node to be checked on the right.
                 */
                assert(i != 0);
                dprev = stack[--i];
            } else {
                /*
                 * Finished left, try right on next pass
                 */
                doing_right = 1;
            }
        } else {
            if (doing_right) {
                /*
                 * Node on right has children.  Step down the tree,
                 * starting on the left.  No need to stack the previous,
                 * we've already checked both children
                 */
                doing_right = 0;
            } else {
                /*
                 * Node on left has children, stack the previous node
                 * so we check its right child later.
                 */
                stack[i++] = dprev;
            }
            dprev = dp;
        }
    }
}   

static void
mfc_print_node_v6 __PF2(mfcp, mfc *,
			fd, caddr_t)
{
    int lifetime;
    downstream *dsp;

    fprintf((FILE *)fd, "\t  Source %A Mask %A", &mfcp->mfc_src, inet6_masks[mfcp->mfc_pfx == 0 ? 128 : mfcp->mfc_pfx]);
    if (mfcp->upstream_ifap) {
	fprintf((FILE *)fd, " UpIF %A(%s)\n",
		    mfcp->upstream_ifap->ifa_addr,
		    mfcp->upstream_ifap->ifa_link->ifl_name);
    }
    DOWNSTREAM_LIST(dsp, mfcp->ds) {
	fprintf((FILE *)fd, "\t  DownIF %A  Min Hoplimit %d\n",
		dsp->ds_addr, dsp->ds_hoplimit);
    } DOWNSTREAM_LIST_END(dsp, mfcp->ds);
    lifetime = time_sec - mfcp->mfc_ctime;
    fprintf((FILE *)fd,
	    "\t  LifeTime: %02u:%02u:%02u Use: %d LastUse: %d\n\n",
	    lifetime/3600,
	    (lifetime%3600)/60,
	    lifetime%60,
	    mfcp->mfc_use,
	    mfcp->mfc_lastuse);
}

void
mfc_source_visit_v6(head, callback, arg1)
group_node *head;
_PROTOTYPE(callback,
	   void,
	   (mfc *,
	    caddr_t));
caddr_t arg1;
{
    int i, n, doing_right;
    mfc *stack[sizeof(struct in6_addr) * NBBY];
    register mfc *dp, *dprev = head->src_head;

    if (head->src_node_count == 0) {
        return;
    }
    
        /* catch the case where only one node in the tree. */
    if (dprev->mfc_pfx == INET6_NODE_NOMASK)
    {
        (*callback)(dprev, arg1);
        return;
    }
    
    doing_right = 0;
    i = n = 0;
    for (;;) {
        if (doing_right) {
            dp = dprev->right;
        } else {
            dp = dprev->left;
        }

		if (dp->mfc_pfx <= dprev->mfc_pfx) {

            (*callback)(dp, arg1);

            if (++n >= head->src_node_count) {
                /* All found */
                break;
            }

            if (doing_right) {
                /*
                 * Finished right hand, back up stack for another
                 * node to be checked on the right.
                 */
                assert(i != 0);
                dprev = stack[--i];
            } else {
                /*
                 * Finished left, try right on next pass
                 */
                doing_right = 1;
            }
	} else {
            if (doing_right) {
                /*
                 * Node on right has children.  Step down the tree,
                 * starting on the left.  No need to stack the previous,
                 * we've already checked both children
                 */
                doing_right = 0;
            } else {
                /*
                 * Node on left has children, stack the previous node
                 * so we check its right child later.
                 */
                stack[i++] = dprev;
            }
            dprev = dp;
        }
    }
}   

group_node *
mfc_locate_group_v6 __PF1(group, sockaddr_un *)
{
    register group_node *dp = group_node_head_v6;

    /* 
     * Try to find the group in the tree
     */
    if (dp) {
		u_short 	prefixlen;
		do {
			prefixlen = dp->group_pfx;
			if (IN6_BIT_CHECK(group->g6_addr, prefixlen)) {
				dp = dp->right;
			} else {
				dp = dp->left;
			}
		} while (dp->group_pfx > prefixlen);
    }

    /*
     * If the group exists, return it
     */
    if (dp && SAME_ADDR6(dp->group_key.g6_addr, group->g6_addr)) {
		return (dp);
    }
    return((group_node*) 0);
}

mfc *
mfc_locate_mfc_v6 __PF2(dst, sockaddr_un *,
		     src, sockaddr_un *)
{
    register group_node *dp = mfc_locate_group_v6(dst);

    if (dp) {
		register mfc *mp = dp->src_head;

		/*
		 * Try to find the mfc in the tree
		 */
		if (mp) {
			u_short prefixlen;
			do {
				prefixlen = mp->mfc_pfx;
				if (IN6_BIT_CHECK(src->g6_addr, prefixlen)) {
					mp = mp->right;
				} else {
					mp = mp->left;
				}
			} while (mp->mfc_pfx > prefixlen);
		}
		if (mp && SAME_ADDR6(mp->mfc_src.g6_addr, src->g6_addr)) {
			return (mp);
		}
    }
    return ((mfc *) 0);
}

int
mfc_add_node_v6 __PF2(dst, sockaddr_un *,
		   mfcp, mfc *)
{
    register group_node *dp = group_node_head_v6;
    sockaddr_un tmpSock;

    /*
     * Try to find the group in the tree
     */
    if (dp) {
		u_short	prefixlen;
		do {
			prefixlen = dp->group_pfx;
			if(IN6_BIT_CHECK(dst->g6_addr, prefixlen)) {
				dp = dp->right;
			} else {
				dp = dp->left;
			}
		} while (dp->group_pfx > prefixlen );
    }

    /*
     * If the group exists, we can look up source
     * If not, insert new group in group tree
     */
    if (dp && SAME_ADDR6(dp->group_key.g6_addr, dst->g6_addr)) {
		register mfc *mp = dp->src_head;

		/*
		 * Try to find the mfc in the tree
		 */
		if (mp) {
			u_short	prefixlen;
			do {
				prefixlen = mp->mfc_pfx;
				if (IN6_BIT_CHECK(mfcp->mfc_src.g6_addr, prefixlen)) {
					mp = mp->right;
				} else {
					mp = mp->left;
				}
			} while (mp->mfc_pfx > prefixlen);
		}
		if (mp && SAME_ADDR6(mp->mfc_src.g6_addr, mfcp->mfc_src.g6_addr)) {
                /*
                 * already there, log error
                 */
            trace_only_tp(inet6_task,
                          0,
                          ("mfc_add_node_v6: Duplicate MFC request for group %A source %A",
                          dst,
                          &mfcp->mfc_src));
            return(1);
        } else {
            mfc_insert_node_v6(dp, mp, mfcp);
        }
    } else {
		register mfc *mp;
       
        dp = mfc_insert_group_v6(dp, dst);
       
        mp = dp->src_head;
        mfc_insert_node_v6(dp, mp, mfcp);
    }  
    return(0);   
}      
            
void   
mfc_delete_node_v6  __PF1(dp, mfc *)
{
    register mfc *tmp;    
    register mfc *dprev, *dcurr;
    sockaddr_un key, tmpKey;
    mfc *intprev;
            
    /*   
     * Free up the downstream list head
     */ 
       
    mfc_free_downstream_v6(dp->ds);
       
    /*  
     * Detect the trivial case, that there is only one guy in the tree,
     * and fix this up.
     */
    dcurr = dp->mfc_group->src_head;
    if (dcurr->mfc_pfx == INET6_NODE_NOMASK) {
        assert(dcurr == dp);
        dp->mfc_group->src_head = (mfc *) 0;
        dp->mfc_group->src_node_count = 0;
 
        mfc_delete_group_v6(dp->mfc_group);
    
        MFC_NODE_FREE(dp);
        return;  
    }       
    
    /* 
     * We will be in the tree twice, once as an internal node and once
     * as an external (or up-)node.  Find the guy who points to our man
     * in the tree in both cases, and the guy who points to him in the
     * latter.
     */ 
    dprev = (mfc *) 0;
    intprev = (mfc *) 0;
    key = dp->mfc_src;
    for (;;) {
		if (IN6_BIT_CHECK(key.g6_addr, dcurr->mfc_pfx)) {
			tmp = dcurr->right;
		} else {
			tmp = dcurr->left;
		}
		if (tmp == dp) {
			if (dp->mfc_pfx <= dcurr->mfc_pfx) {
				break;          /* got it */
			}    
			intprev = dcurr;    /* current is in front of internal */
		} else {
			assert(tmp->mfc_pfx > dcurr->mfc_pfx);
		} 
		dprev = dcurr;
		dcurr = tmp;
	}
     
	/*
	 * Now we have dcurr pointing at the node which contains the up-link
	 * to our deleted node, nprev pointing at the node which points to
	 * ncurr and intprev pointing at the guy who precedes our node in its
	 * internal incarnation.
	 *      
	 * There are several cases here.  We know the information in dcurr
	 * is no longer needed, and that the sibling pointer in dcurr will
	 * be moved to dprev (replacing dcurr).  If our node is the special
	 * one with no internal information, we make dcurr the special.
	 * If dcurr is our node (i.e. intprev == dprev) then we simply
	 * point nprev at our sibling and we're done.  Otherwise we
	 * additionally copy our information to dcurr, and point intprev
	 * at dcurr.
	 *  
	 * There is probably a better way to do this, but I'm an engineer,
	 * not a (mere) programmer.
	 */ 
	if (dcurr->right == dp) {
		tmp = dcurr->left;
	} else {
		tmp = dcurr->right;
	}
	if (!dprev) {
		dp->mfc_group->src_head = tmp;
	} else if (dprev->right == dcurr) {
		dprev->right = tmp;
	} else { 
		dprev->left = tmp;
	}
	if (dcurr != dp) {
		if (dp->mfc_pfx == INET6_NODE_NOMASK) {
			dcurr->mfc_pfx = INET6_NODE_NOMASK;
			dcurr->left = dcurr->right = dcurr;
		} else {
			dcurr->mfc_pfx = dp->mfc_pfx;
			dcurr->left = dp->left;
			dcurr->right = dp->right;
			if (!intprev) {
				dp->mfc_group->src_head = dcurr;
			} else if (intprev->right == dp) {
				intprev->right = dcurr;
			} else {
				intprev->left = dcurr;
			}
		}
	} 
	
	dp->mfc_group->src_node_count--;
	MFC_NODE_FREE(dp);
}   
    
static void
mfc_delete_group_v6  __PF1(dp, group_node *) 
{           
    register group_node *tmp;
    register group_node *dprev, *dcurr;
    sockaddr_un key; 
    group_node *intprev; 
            
    /*          
     * Detect the trivial case, that there is only one guy in the tree,
     * and fix this up.
     */     
    dcurr = group_node_head_v6;
    if (dcurr->group_pfx == INET6_NODE_NOMASK) {
        assert(dcurr == dp);
        group_node_count_v6--;
        GROUP_NODE_FREE(dp);
        group_node_head_v6 = (group_node *) 0;
        return;
    }
    
    /* 
     * We will be in the tree twice, once as an internal node and once
     * as an external (or up-)node.  Find the guy who points to our man
     * in the tree in both cases, and the guy who points to him in the
     * latter.
     */
    dprev = (group_node *) 0;
    intprev = (group_node *) 0;
    key = dp->group_key;
    for (;;) {
		if (IN6_BIT_CHECK(key.g6_addr, dcurr->group_pfx)) {
            tmp = dcurr->right;
        } else {
            tmp = dcurr->left;
        }
        if (tmp == dp) {   
			if (dp->group_pfx <= dcurr->group_pfx) {
                break;          /* got it */
            }  
            intprev = dcurr;    /* current is in front of internal */
        } else {
			assert(tmp->group_pfx > dcurr->group_pfx);
        } 
        dprev = dcurr;
        dcurr = tmp;
    }
     
    /*
     * Now we have dcurr pointing at the node which contains the up-link
     * to our deleted node, nprev pointing at the node which points to
     * ncurr and intprev pointing at the guy who precedes our node in its
     * internal incarnation.
     *      
     * There are several cases here.  We know the information in dcurr
     * is no longer needed, and that the sibling pointer in dcurr will
     * be moved to dprev (replacing dcurr).  If our node is the special
     * one with no internal information, we make dcurr the special.
     * If dcurr is our node (i.e. intprev == dprev) then we simply
     * point nprev at our sibling and we're done.  Otherwise we
     * additionally copy our information to dcurr, and point intprev
     * at dcurr.
     *  
     * There is probably a better way to do this, but I'm an engineer,
     * not a (mere) programmer.
     */ 
    if (dcurr->right == dp) {
        tmp = dcurr->left;
    } else {
        tmp = dcurr->right;
    }
    if (!dprev) {
        group_node_head_v6 = tmp;
    } else if (dprev->right == dcurr) {
        dprev->right = tmp;
    } else { 
        dprev->left = tmp;
    }
    if (dcurr != dp) {
	if (dp->group_pfx == INET6_NODE_NOMASK) {
			dcurr->group_pfx = INET6_NODE_NOMASK;
            dcurr->left = dcurr->right = dcurr;
        } else {
            dcurr->group_pfx = dp->group_pfx;
            dcurr->left = dp->left;
            dcurr->right = dp->right;
            if (!intprev) {
                group_node_head_v6 = dcurr;
            } else if (intprev->right == dp) {
                intprev->right = dcurr;
            } else {
                intprev->left = dcurr;
            }
        }
    } 
        
    group_node_count_v6--;
    GROUP_NODE_FREE(dp);  
}   
    
/*      
 * mfc_insert_node_v6 - add a mfc node to the demux tree.  Note that
 *                 found_dp points to the mfc node that was "found" when the
 *                 tree was searched for the address we are adding.
 *                 If it is NULL we do the search on our own.
 */         
            
static void 
mfc_insert_node_v6  __PF3(found_dp, group_node *,
                       found_mp, mfc *,
                       mp, mfc *)
{           
    int i;            
    register mfc *mprev, *mcurr;
    u_short 	prefixlen, oprefixlen;
    struct in6_addr tmpAddr;
    sockaddr_un tmpSock;
    
    /*  
     * Increment count in group head
     */
    found_dp->src_node_count++;
    
    /*  
     * If he didn't include a "found" demux node, but there are nodes
     * to "find", "find" it.  If there are no nodes to find then
     * this is the first node in the tree and just needs to get
     * initialized appropriately.
     */     
    if (!found_mp) {
        if (!(mcurr = found_dp->src_head)) {
            found_dp->src_head = mp->right = mp->left = mp;
			mp->mfc_pfx = INET6_NODE_NOMASK;
            mp->mfc_group = found_dp;
            return;
        }       
		prefixlen = INET6_NODE_NOMASK;
		while (mcurr->mfc_pfx > prefixlen) {
            prefixlen = mcurr->mfc_pfx;
			if (IN6_BIT_CHECK(mp->mfc_src.g6_addr, prefixlen)) {
                mcurr = mcurr->right;
            } else {
                mcurr = mcurr->left;
            }
        }
        found_mp = mcurr;
    }
     
    /* 
     * Here we have an insertion to do.  Figure out the first
     * (most significant) bit which differs from the "found" node.  This
     * tells us the bit we need to test in the node we insert.
     *      
     * Do a binary search to find the byte the first bit is in, then use
     * the table to form the mask.   
     */     
    IN6_ADDR_OP(tmpAddr, mp->mfc_src.g6_addr, found_mp->mfc_src.g6_addr, ^);
    prefixlen = 0;
    for(i = 0; i < sizeof(struct in6_addr); i++) {
		if(tmpAddr.s6_addr[i]) {
			register byte tmpMask;
			register u_int pos;
			pos = 0;
			tmpMask = tmpAddr.s6_addr[i];
			while (!(tmpMask & 0x80)) {
				pos++;
				tmpMask <<= 1;
			}
			prefixlen += pos + 1;
			break;
		} else {
			prefixlen += NBBY;
			continue;
		}
    }
     
    /* 
     * XXX Sanity.  This could only happen if a node with the same key
     * is in the tree already, which should never happen.
     */
    assert(prefixlen != 0);
    mp->mfc_pfx = prefixlen;
    
    /* 
     * Now we locate where this guy needs to be inserted.  Search down
     * the tree until we find either an end node (i.e. an upward link)
     * or a node with a mask smaller than ours.  We insert our node
     * above this (watch for the case where the node we find is the
     * current root node), making one of the pointers an uplink pointing
     * at the node itself.
     */ 
    mprev = (mfc *) 0;
    mcurr = found_dp->src_head;
    oprefixlen = INET6_NODE_NOMASK;
    while (mcurr->mfc_pfx > oprefixlen ) {
        oprefixlen = mcurr->mfc_pfx;
		if (prefixlen >= oprefixlen) {
            break;
        }
        mprev = mcurr;
		if (IN6_BIT_CHECK(mp->mfc_src.g6_addr, oprefixlen)) {
            mcurr = mcurr->right;
        } else { 
            mcurr = mcurr->left;
        }
    }
    assert(prefixlen != mcurr->mfc_pfx);
     
    /* 
     * Point the new node at the current node, and at itself.  The
     * pointer to the current node in the previous node is changed to
     * point at the new node.  Simple(?).
     */
    if (IN6_BIT_CHECK(mp->mfc_src.g6_addr, prefixlen)) {
        mp->right = mp; 
        mp->left = mcurr;
    } else {
        mp->right = mcurr;  
        mp->left = mp;
    }   
        
    if (!mprev) {
        /* New root node */
        found_dp->src_head = mp;
    } else if (mprev->right == mcurr) {
        mprev->right = mp;
    } else {
        mprev->left = mp;
    }
    mp->mfc_group = found_dp;
}    
     
static group_node * 
mfc_insert_group_v6  __PF2(found_dp, group_node *,
                         key, sockaddr_un *)
{       
    register int i;
    register group_node *dp;
    register group_node *dprev, *dcurr;
    struct in6_addr tmpAddr;
    u_short	prefixlen, oprefixlen;
    sockaddr_un tmpSock;
        
    /*  
     * Fetch a new demux structure and initialize it
     */
    dp = GROUP_NODE_ALLOC();
    bcopy(key, &dp->group_key, sizeof(sockaddr_un));
    dp->src_node_count = 0; 
    dp->graft_pending = 0;
    dp->src_head = (mfc *) 0;
        
    group_node_count_v6++;
    /*
     * If he didn't include a "found" demux node, but there are nodes
     * to "find", "find" it.  If there are no nodes to find then
     * this is the first node in the tree and just needs to get
     * initialized appropriately.
     */ 
    if (!found_dp) {
        if (!(dcurr = group_node_head_v6)) {
            group_node_head_v6 = dp->right = dp->left = dp;
			dp->group_pfx = INET6_NODE_NOMASK;
            return(dp);
        }
		prefixlen = INET6_NODE_NOMASK;
		while (dcurr->group_pfx > prefixlen) {
            prefixlen = dcurr->group_pfx;
			if (IN6_BIT_CHECK(key->g6_addr, prefixlen)) {
                dcurr = dcurr->right;
            } else {
                dcurr = dcurr->left;
            }
        }
        found_dp = dcurr;
    }
     
    /* 
     * Here we have an insertion to do.  Figure out the first
     * (most significant) bit which differs from the "found" node.  This
     * tells us the bit we need to test in the node we insert.
     *  
     * Do a binary search to find the byte the first bit is in, then use
     * the table to form the mask.
     */     
    IN6_ADDR_OP(tmpAddr, key->g6_addr, found_dp->group_key.g6_addr, ^);    
	prefixlen = 0;
    for(i = 0; i < sizeof(struct in6_addr); i++) {
        if(tmpAddr.s6_addr[i]) {  
            register byte tmpMask; 
            register u_int pos;
            pos = 0;
            tmpMask = tmpAddr.s6_addr[i];
            while (!(tmpMask & 0x80)) { 
                pos++;
                tmpMask <<= 1;
            }
			prefixlen += pos + 1;
            break;
        } else {
			prefixlen += NBBY;
            continue;
        }
    }
     
    /* 
     * XXX Sanity.  This could only happen if a node with the same key
     * is in the tree already, which should never happen.
     */
    assert(prefixlen != 0);
    dp->group_pfx = prefixlen;
    
    /* 
     * Now we locate where this guy needs to be inserted.  Search down
     * the tree until we find either an end node (i.e. an upward link)
     * or a node with a mask smaller than ours.  We insert our node
     * above this (watch for the case where the node we find is the
     * current root node), making one of the pointers an uplink pointing
     * at the node itself.
     */ 
    dprev = (group_node *) 0;
    dcurr = group_node_head_v6;
    oprefixlen = INET6_NODE_NOMASK;
    while (dcurr->group_pfx > oprefixlen) {
        oprefixlen = dcurr->group_pfx;
		if (prefixlen <= oprefixlen) {
            break;
        }
        dprev = dcurr;
		if (IN6_BIT_CHECK(key->g6_addr, oprefixlen)) {
            dcurr = dcurr->right;
        } else {
            dcurr = dcurr->left;
        }
    }
    assert(prefixlen != dcurr->group_pfx);
     
    /* 
     * Point the new node at the current node, and at itself.  The
     * pointer to the current node in the previous node is changed to
     * point at the new node.  Simple(?).
     */
    if (IN6_BIT_CHECK(key->g6_addr, prefixlen)) {
        dp->right = dp; 
        dp->left = dcurr;
    } else {
        dp->right = dcurr;  
        dp->left = dp;
    }   
        
    if (!dprev) { 
        /* New root node */
        group_node_head_v6 = dp;
    } else if (dprev->right == dcurr) {
        dprev->right = dp;
    } else {
        dprev->left = dp;
    }
    
    return(dp);
}    
#endif  /* IPV6_MULTICAST */
