/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License (the "License").
 * You may not use this file except in compliance with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
 */

/*
 * lgroup topology
 */

#include <sys/cpupart.h>
#include <sys/lgrp.h>
#include <sys/promif.h>
#include <sys/types.h>


#define LGRP_TOPO_LEVELS        4       /* default height limit */
#define LGRP_TOPO_LEVELS_MAX    4       /* max height limit */


/*
 * Only collapse lgroups which have same latency (and resources)
 */
int             lgrp_collapse_equidist = 1;

int             lgrp_collapse_off = 1;  /* disable collapsing of duplicates */

/*
 * Height to limit lgroup topology
 */
unsigned int    lgrp_topo_levels = LGRP_TOPO_LEVELS;

int             lgrp_split_off = 1;     /* disable splitting lgroups */

#ifdef  DEBUG
/*
 * Debugging output
 * - 0: off
 * - >0: on and bigger means more
 */
int     lgrp_topo_debug = 0;


void
klgrpset_print(klgrpset_t lgrpset)
{
        int     i;


        prom_printf("0x%llx(", (u_longlong_t)lgrpset);
        for (i = 0; i <= lgrp_alloc_max; i++)
                if (klgrpset_ismember(lgrpset, i))
                        prom_printf("%d ", i);
        prom_printf(")\n");
}


void
lgrp_rsets_print(char *string, klgrpset_t *rsets)
{
        int     i;

        prom_printf("%s\n", string);
        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                klgrpset_print(rsets[i]);
}
#endif  /* DEBUG */


/*
 * Add "from" lgroup resources to "to" lgroup resources
 */
void
lgrp_rsets_add(klgrpset_t *from, klgrpset_t *to)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                klgrpset_or(to[i], from[i]);
}


/*
 * Copy "from" lgroup resources to "to" lgroup resources
 */
void
lgrp_rsets_copy(klgrpset_t *from, klgrpset_t *to)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                to[i] = from[i];
}


/*
 * Delete given lgroup ID from lgroup resource set of specified lgroup
 * and its ancestors if "follow_parent" is set
 */
void
lgrp_rsets_delete(lgrp_t *lgrp, lgrp_id_t lgrpid, int follow_parent)
{
        int     i;

        while (lgrp != NULL) {
                for (i = 0; i < LGRP_RSRC_COUNT; i++)
                        klgrpset_del(lgrp->lgrp_set[i], lgrpid);
                if (!follow_parent)
                        break;
                lgrp = lgrp->lgrp_parent;
        }
}


/*
 * Return whether given lgroup resource set empty
 */
int
lgrp_rsets_empty(klgrpset_t *rset)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                if (!klgrpset_isempty(rset[i]))
                        return (0);

        return (1);
}


/*
 * Return whether given lgroup resource sets are same
 */
int
lgrp_rsets_equal(klgrpset_t *rset1, klgrpset_t *rset2)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                if (rset1[i] != rset2[i])
                        return (0);

        return (1);
}


/*
 * Return whether specified lgroup ID is in given lgroup resource set
 */
int
lgrp_rsets_member(klgrpset_t *rset, lgrp_id_t lgrpid)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                if (klgrpset_ismember(rset[i], lgrpid))
                        return (1);

        return (0);
}


/*
 * Return whether specified lgroup ID is in all lgroup resources
 */
int
lgrp_rsets_member_all(klgrpset_t *rset, lgrp_id_t lgrpid)
{
        int     i;

        for (i = 0; i < LGRP_RSRC_COUNT; i++)
                if (!klgrpset_ismember(rset[i], lgrpid))
                        return (0);

        return (1);
}


/*
 * Replace resources for given lgroup with specified resources at given
 * latency and shift its old resources to its parent and its parent's resources
 * to its parent, etc. until root lgroup reached
 */
void
lgrp_rsets_replace(klgrpset_t *rset, int latency, lgrp_t *lgrp, int shift)
{
        lgrp_t          *cur;
        int             lat_new;
        int             lat_saved;
        klgrpset_t      rset_new[LGRP_RSRC_COUNT];
        klgrpset_t      rset_saved[LGRP_RSRC_COUNT];

        cur = lgrp;
        lat_saved = latency;
        lgrp_rsets_copy(rset, rset_saved);
        while (cur && cur != lgrp_root) {
                /*
                 * Save current resources and latency to insert in parent and
                 * then replace with new resources and latency
                 */
                lgrp_rsets_copy(rset_saved, rset_new);
                lgrp_rsets_copy(cur->lgrp_set, rset_saved);
                lgrp_rsets_copy(rset_new, cur->lgrp_set);

                lat_new = lat_saved;
                lat_saved = cur->lgrp_latency;
                cur->lgrp_latency = lat_new;
                if (!shift)
                        break;
                cur = cur->lgrp_parent;
        }
}


/*
 * Set "to" lgroup resource set with given lgroup ID
 */
void
lgrp_rsets_set(klgrpset_t *to, lgrp_id_t lgrpid)
{
        klgrpset_t      from;
        int             i;

        klgrpset_clear(from);
        klgrpset_add(from, lgrpid);
        for (i = 0; i < LGRP_RSRC_COUNT; i++) {
                klgrpset_clear(to[i]);
                klgrpset_or(to[i], from);
        }
}


/*
 * Delete any ancestors of given child lgroup which don't have any other
 * children
 */
int
lgrp_ancestor_delete(lgrp_t *child, klgrpset_t *changed)
{
        int             count;
        lgrp_t          *current;
        lgrp_id_t       lgrpid;
        lgrp_t          *parent;

#ifdef  DEBUG
        if (lgrp_topo_debug > 1) {
                prom_printf("lgrp_ancestor_delete(0x%p[%d],0x%p)\n",
                    (void *)child, child->lgrp_id, (void *)changed);
        }
#endif  /* DEBUG */

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        /*
         * Visit ancestors, decrement child count for each, and remove any
         * that don't have any children left until we reach an ancestor that
         * has multiple children
         */
        current = child;
        parent = child->lgrp_parent;
        lgrpid = current->lgrp_id;
        while (parent != NULL) {
#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        prom_printf("lgrp_ancestor_delete: parent %d,"
                            " current %d\n",
                            parent->lgrp_id, lgrpid);
#endif  /* DEBUG */

                klgrpset_del(parent->lgrp_leaves, lgrpid);
                klgrpset_del(parent->lgrp_children, lgrpid);
                parent->lgrp_childcnt--;
                if (changed)
                        klgrpset_add(*changed, parent->lgrp_id);
                count++;
                if (parent->lgrp_childcnt != 0)
                        break;

                current = parent;
                parent = current->lgrp_parent;
                lgrpid = current->lgrp_id;

#ifdef  DEBUG
                if (lgrp_topo_debug > 0)
                        prom_printf("lgrp_ancestor_delete: destroy"
                            " lgrp %d at 0x%p\n",
                            current->lgrp_id, (void *)current);
#endif  /* DEBUG */
                lgrp_destroy(current);
        }

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_ancestor_delete: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (count);
}


/*
 * Consolidate lgrp1 into lgrp2
 */
int
lgrp_consolidate(lgrp_t *lgrp1, lgrp_t *lgrp2, klgrpset_t *changed)
{
        klgrpset_t      changes;
        lgrp_t          *child;
        int             count;
        int             i;
        lgrp_t          *parent;

        /*
         * Leaf lgroups should never need to be consolidated
         */
        if (lgrp1 == NULL || lgrp2 == NULL || lgrp1->lgrp_childcnt < 1 ||
            lgrp2->lgrp_childcnt < 1)
                return (0);

#ifdef  DEBUG
        if (lgrp_topo_debug > 0)
                prom_printf("lgrp_consolidate(0x%p[%d],0x%p[%d],0x%p)\n",
                    (void *)lgrp1, lgrp1->lgrp_id, (void *)lgrp2,
                    lgrp2->lgrp_id, (void *)changed);
#endif  /* DEBUG */

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        /*
         * Lgroup represents resources within certain latency, so need to keep
         * biggest latency value of lgroups being consolidated
         */
        if (lgrp1->lgrp_latency > lgrp2->lgrp_latency)
                lgrp2->lgrp_latency = lgrp1->lgrp_latency;

        /*
         * Delete ancestors of lgrp1 that don't have any other children
         */
#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_consolidate: delete ancestors\n");
#endif  /* DEBUG */
        count += lgrp_ancestor_delete(lgrp1, &changes);
        if (changed) {
                klgrpset_or(*changed, changes);
                klgrpset_or(*changed, lgrp1->lgrp_id);
                count++;
        }

        /*
         * Reparent children lgroups of lgrp1 to lgrp2
         */
        for (i = 0; i <= lgrp_alloc_max; i++) {
                if (i == lgrp2->lgrp_id ||
                    !klgrpset_ismember(lgrp1->lgrp_children, i))
                        continue;
                child = lgrp_table[i];
                if (!LGRP_EXISTS(child))
                        continue;
#ifdef  DEBUG
                if (lgrp_topo_debug > 0)
                        prom_printf("lgrp_consolidate: reparent "
                            "lgrp %d to lgrp %d\n",
                            child->lgrp_id, lgrp2->lgrp_id);
#endif  /* DEBUG */
                klgrpset_or(lgrp2->lgrp_leaves, child->lgrp_leaves);
                klgrpset_add(lgrp2->lgrp_children, child->lgrp_id);
                lgrp2->lgrp_childcnt++;
                child->lgrp_parent = lgrp2;
                if (changed) {
                        klgrpset_add(*changed, child->lgrp_id);
                        klgrpset_add(*changed, lgrp2->lgrp_id);
                }
                count += 2;
        }

        /*
         * Proprogate leaves from lgrp2 to root
         */
        child = lgrp2;
        parent = child->lgrp_parent;
        while (parent != NULL) {
                klgrpset_or(parent->lgrp_leaves, child->lgrp_leaves);
                if (changed)
                        klgrpset_add(*changed, parent->lgrp_id);
                count++;
                child = parent;
                parent = parent->lgrp_parent;
        }

#ifdef  DEBUG
        if (lgrp_topo_debug > 0)
                prom_printf("lgrp_consolidate: destroy lgrp %d at 0x%p\n",
                    lgrp1->lgrp_id, (void *)lgrp1);
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_consolidate: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        lgrp_destroy(lgrp1);

        return (count);
}

/*
 * Collapse duplicates of target lgroups given
 */
int
lgrp_collapse_dups(klgrpset_t target_set, int equidist_only,
    klgrpset_t *changed)
{
        klgrpset_t      changes;
        int             count;
        int             i;

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        if (lgrp_collapse_off)
                return (0);

#ifdef  DEBUG
        if (lgrp_topo_debug > 0)
                prom_printf("lgrp_collapse_dups(0x%llx)\n",
                    (u_longlong_t)target_set);
#endif  /* DEBUG */

        /*
         * Look for duplicates of each target lgroup
         */
        for (i = 0; i <= lgrp_alloc_max; i++) {
                int     j;
                lgrp_t  *keep;
                lgrp_t  *target;

                target = lgrp_table[i];

                /*
                 * Skip to next lgroup if there isn't one here, this is root
                 * or leaf lgroup, or this isn't a target lgroup
                 */
                if (!LGRP_EXISTS(target) ||
                    target == lgrp_root || target->lgrp_childcnt == 0 ||
                    !klgrpset_ismember(target_set, target->lgrp_id))
                        continue;

                /*
                 * Find all lgroups with same resources and latency
                 */
#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        prom_printf("lgrp_collapse_dups: find "
                            "dups of lgrp %d at 0x%p\n",
                            target->lgrp_id, (void *)target);
#endif  /* DEBUG */
                keep = NULL;
                for (j = 0; j <= lgrp_alloc_max; j++) {
                        lgrp_t  *lgrp;

                        lgrp = lgrp_table[j];

                        /*
                         * Skip lgroup if there isn't one here, this is root
                         * lgroup or leaf (which shouldn't have dups), or this
                         * lgroup doesn't have same resources
                         */
                        if (!LGRP_EXISTS(lgrp) ||
                            lgrp->lgrp_childcnt == 0 ||
                            !lgrp_rsets_equal(lgrp->lgrp_set,
                            target->lgrp_set) ||
                            (lgrp->lgrp_latency != target->lgrp_latency &&
                            equidist_only))
                                continue;

                        /*
                         * Keep first matching lgroup (but always keep root)
                         * and consolidate other duplicates into it
                         */
                        if (keep == NULL) {
                                keep = lgrp;
#ifdef  DEBUG
                                if (lgrp_topo_debug > 1)
                                        prom_printf("lgrp_collapse_dups: "
                                            "keep lgrp %d at 0x%p\n",
                                            keep->lgrp_id, (void *)keep);
#endif  /* DEBUG */
                        } else {
                                if (lgrp == lgrp_root) {
                                        lgrp = keep;
                                        keep = lgrp_root;
                                }
#ifdef  DEBUG
                                if (lgrp_topo_debug > 0)
                                        prom_printf("lgrp_collapse_dups:"
                                            " consolidate lgrp %d at 0x%p"
                                            " into lgrp %d at 0x%p\n",
                                            lgrp->lgrp_id, (void *)lgrp,
                                            keep->lgrp_id, (void *)keep);
#endif  /* DEBUG */
                                count += lgrp_consolidate(lgrp, keep,
                                    &changes);
                                if (changed)
                                        klgrpset_or(*changed, changes);
                        }
                }
        }

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_collapse_dups: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (count);
}


/*
 * Create new parent lgroup with given latency and resources for
 * specified child lgroup, and insert it into hierarchy
 */
int
lgrp_new_parent(lgrp_t *child, int latency, klgrpset_t *rset,
    klgrpset_t *changed)
{
        int     count;
        lgrp_t  *new;
        lgrp_t  *old;

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        /*
         * Create lgroup and set its latency and resources
         */
        new = lgrp_create();
        new->lgrp_latency = latency;
        lgrp_rsets_add(rset, new->lgrp_set);

        /*
         * Insert new lgroup into hierarchy
         */
        old = child->lgrp_parent;
        new->lgrp_parent = old;
        klgrpset_add(new->lgrp_children, child->lgrp_id);
        new->lgrp_childcnt++;
        klgrpset_add(new->lgrp_children, child->lgrp_id);
        klgrpset_copy(new->lgrp_leaves, child->lgrp_leaves);

        child->lgrp_parent = new;
        if (old) {
                klgrpset_del(old->lgrp_children, child->lgrp_id);
                klgrpset_add(old->lgrp_children, new->lgrp_id);
                if (changed)
                        klgrpset_add(*changed, old->lgrp_id);
                count++;
        }

        if (changed) {
                klgrpset_add(*changed, child->lgrp_id);
                klgrpset_add(*changed, new->lgrp_id);
        }
        count += 2;

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_new_parent: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (count);
}


/*
 * Proprogate resources of new leaf into parent lgroup of given child
 */
int
lgrp_proprogate(lgrp_t *newleaf, lgrp_t *child, int latency,
    klgrpset_t *changed)
{
        int     count;
        lgrp_t  *parent;

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        if (child == NULL || child->lgrp_parent == NULL)
                return (0);

        parent = child->lgrp_parent;
        klgrpset_or(parent->lgrp_leaves, child->lgrp_leaves);
        if (changed)
                klgrpset_add(*changed, parent->lgrp_id);
        count++;

        /*
         * Don't proprogate new leaf resources to parent if it already
         * contains these resources
         */
        if (lgrp_rsets_member_all(parent->lgrp_set, newleaf->lgrp_id)) {
#ifdef  DEBUG
                if (lgrp_topo_debug > 1 && changed)
                        prom_printf("lgrp_proprogate: changed %d lgrps:"
                            " 0x%llx\n",
                            count, (u_longlong_t)*changed);
#endif  /* DEBUG */
                return (count);
        }

        /*
         * Add leaf resources to parent lgroup
         */
        lgrp_rsets_add(newleaf->lgrp_set, parent->lgrp_set);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1) {
                prom_printf("lgrp_proprogate: newleaf %d(0x%p), "
                    "latency %d, child %d(0x%p), parent %d(0x%p)\n",
                    newleaf->lgrp_id, (void *)newleaf, latency, child->lgrp_id,
                    (void *)child, parent->lgrp_id, (void *)parent);
                prom_printf("lgrp_proprogate: parent's leaves becomes 0x%llx\n",
                    (u_longlong_t)parent->lgrp_leaves);
        }
        if (lgrp_topo_debug > 0) {
                prom_printf("lgrp_proprogate: adding to parent %d (0x%p)\n",
                    parent->lgrp_id, (void *)parent);
                lgrp_rsets_print("parent resources become:", parent->lgrp_set);
        }

        if (lgrp_topo_debug > 2 && changed)
                prom_printf("lgrp_proprogate: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);

#endif  /* DEBUG */

        return (count);
}


/*
 * Split parent lgroup of given child if child's leaf decendant (oldleaf) has
 * different latency to new leaf lgroup (newleaf) than leaf lgroups of given
 * child's siblings
 */
int
lgrp_split(lgrp_t *oldleaf, lgrp_t *newleaf, lgrp_t *child,
    klgrpset_t *changed)
{
        klgrpset_t      changes;
        int             count;
        int             i;
        int             latency;
        lgrp_t          *parent;

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        if (lgrp_split_off || newleaf == NULL || child == NULL)
                return (0);

        /*
         * Parent must have more than one child to have a child split from it
         * and root lgroup contains all resources and never needs to be split
         */
        parent = child->lgrp_parent;
        if (parent == NULL || parent->lgrp_childcnt < 2 || parent == lgrp_root)
                return (0);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_split(0x%p[%d],0x%p[%d],0x%p[%d],0x%p)\n",
                    (void *)oldleaf, oldleaf->lgrp_id,
                    (void *)newleaf, newleaf->lgrp_id,
                    (void *)child, child->lgrp_id, (void *)changed);
#endif  /* DEBUG */

        /*
         * Get latency between new leaf and old leaf whose lineage it is
         * being added
         */
        latency = lgrp_plat_latency(oldleaf->lgrp_plathand,
            newleaf->lgrp_plathand);

        /*
         * Check whether all sibling leaves of given child lgroup have same
         * latency to new leaf
         */
        for (i = 0; i <= lgrp_alloc_max; i++) {
                lgrp_t          *grandparent;
                lgrp_t          *lgrp;
                int             sibling_latency;

                lgrp = lgrp_table[i];

                /*
                 * Skip non-existent lgroups, old leaf, and any lgroups that
                 * don't have parent as common ancestor
                 */
                if (!LGRP_EXISTS(lgrp) || lgrp == oldleaf ||
                    !klgrpset_ismember(parent->lgrp_leaves, lgrp->lgrp_id))
                        continue;

                /*
                 * Same latency, so skip
                 */
                sibling_latency = lgrp_plat_latency(lgrp->lgrp_plathand,
                    newleaf->lgrp_plathand);
#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        prom_printf("lgrp_split: latency(%d,%d) %d,"
                            " latency(%d,%d) %d\n",
                            oldleaf->lgrp_id, newleaf->lgrp_id, latency,
                            lgrp->lgrp_id, newleaf->lgrp_id, sibling_latency);
#endif  /* DEBUG */
                if (sibling_latency == latency)
                        continue;

                /*
                 * Different latencies, so  remove child from its parent and
                 * make new parent for old leaf with same latency and same
                 * resources
                 */
                parent->lgrp_childcnt--;
                klgrpset_del(parent->lgrp_children, child->lgrp_id);
                klgrpset_del(parent->lgrp_leaves, oldleaf->lgrp_id);
                grandparent = parent->lgrp_parent;
                if (grandparent) {
                        grandparent->lgrp_childcnt++;
                        klgrpset_add(grandparent->lgrp_children,
                            child->lgrp_id);
                        count++;
                        if (changed)
                                klgrpset_add(*changed, grandparent->lgrp_id);
                }
                child->lgrp_parent = grandparent;

                count += lgrp_new_parent(child, parent->lgrp_latency,
                    parent->lgrp_set, &changes);
                if (changed) {
                        klgrpset_or(*changed, changes);

                        klgrpset_add(*changed, parent->lgrp_id);
                        klgrpset_add(*changed, child->lgrp_id);
                        count += 2;
                }

                parent = child->lgrp_parent;
#ifdef  DEBUG
                if (lgrp_topo_debug > 0) {
                        prom_printf("lgrp_split: new parent %d (0x%p) for"
                            " lgrp %d (0x%p)\n",
                            parent->lgrp_id, (void *)parent,
                            child->lgrp_id, (void *)child);
                        lgrp_rsets_print("new parent resources:",
                            parent->lgrp_set);
                }

                if (lgrp_topo_debug > 1 && changed)
                        prom_printf("lgrp_split: changed %d lgrps: 0x%llx\n",
                            count, (u_longlong_t)*changed);
#endif  /* DEBUG */

                return (count);
        }

#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_split: no changes\n");
#endif  /* DEBUG */

        return (count);
}


/*
 * Return height of lgroup topology from given lgroup to root
 */
int
lgrp_topo_height(lgrp_t *lgrp)
{
        int     nlevels;

        if (!LGRP_EXISTS(lgrp))
                return (0);

        nlevels = 0;
        while (lgrp != NULL) {
                lgrp = lgrp->lgrp_parent;
                nlevels++;
        }
        return (nlevels);
}


/*
 * Add resources of new leaf to old leaf's lineage
 *
 * Assumes the following:
 * - Lgroup hierarchy consists of at least a root lgroup and its leaves
 *   including old and new ones given below
 * - New leaf lgroup has been created and does not need to have its resources
 *   added to it
 * - Latencies have been set for root and leaf lgroups
 */
int
lgrp_lineage_add(lgrp_t *newleaf, lgrp_t *oldleaf, klgrpset_t *changed)
{
        klgrpset_t      changes;
        lgrp_t          *child;
        klgrpset_t      collapse;
        int             count;
        int             latency;
        int             nlevels;
        lgrp_t          *parent;
        int             proprogate;
        int             total;


        count = total = 0;
        if (changed)
                klgrpset_clear(*changed);

        if (newleaf == NULL || oldleaf == NULL || newleaf == oldleaf)
                return (0);

#ifdef  DEBUG
        if (lgrp_topo_debug > 0)
                prom_printf("\nlgrp_lineage_add(0x%p[%d],0x%p[%d],0x%p)\n",
                    (void *)newleaf, newleaf->lgrp_id,
                    (void *)oldleaf, oldleaf->lgrp_id,
                    (void *)changed);
#endif  /* DEBUG */

        /*
         * Get latency between old and new leaves, so we can determine
         * where the new leaf fits in the old leaf's lineage
         */
        latency = lgrp_plat_latency(oldleaf->lgrp_plathand,
            newleaf->lgrp_plathand);

        /*
         * Determine height of lgroup topology from old leaf to root lgroup,
         * so height of topology may be limited if necessary
         */
        nlevels = lgrp_topo_height(oldleaf);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_lineage_add: latency(%d,%d) 0x%x, ht %d\n",
                    oldleaf->lgrp_id, newleaf->lgrp_id, latency, nlevels);
#endif  /* DEBUG */

        /*
         * Can't add new leaf to old leaf's lineage if we haven't
         * determined latency between them yet
         */
        if (latency == 0)
                return (0);

        child = oldleaf;
        parent = child->lgrp_parent;
        proprogate = 0;
        klgrpset_clear(collapse);

        /*
         * Lineage of old leaf is basically a sorted list of the other leaves
         * from closest to farthest, so find where to add new leaf to the
         * lineage and proprogate its resources from that point up to the root
         * lgroup since parent lgroups contain all the resources of their
         * children
         */
        do {
                klgrpset_t      rset[LGRP_RSRC_COUNT];

#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        prom_printf("lgrp_lineage_add: child %d (0x%p), parent"
                            " %d (0x%p)\n",
                            child->lgrp_id, (void *)child,
                            parent->lgrp_id, (void *)parent);
#endif  /* DEBUG */

                /*
                 * See whether parent lgroup needs to be split
                 *
                 * May need to split parent lgroup when it is ancestor to more
                 * than one leaf, but all its leaves don't have latency to new
                 * leaf within the parent lgroup's latency
                 * NOTE: Don't want to collapse this lgroup since we just split
                 * it from parent
                 */
                count = lgrp_split(oldleaf, newleaf, child, &changes);
                if (count) {
#ifdef  DEBUG
                        if (lgrp_topo_debug > 0)
                                prom_printf("lgrp_lineage_add: setting parent"
                                    " for child %d from %d to %d\n",
                                    child->lgrp_id, parent->lgrp_id,
                                    child->lgrp_parent->lgrp_id);
#endif  /* DEBUG */
                        parent = child->lgrp_parent;
                        total += count;
                        if (changed)
                                klgrpset_or(*changed, changes);
                }

                /*
                 * Already found where resources of new leaf belong in old
                 * leaf's lineage, so proprogate resources of new leaf up
                 * through rest of ancestors
                 */
                if (proprogate) {
                        total += lgrp_proprogate(newleaf, child, latency,
                            &changes);
                        if (changed)
                                klgrpset_or(*changed, changes);

                        parent = child->lgrp_parent;
                        klgrpset_add(collapse, parent->lgrp_id);
                        child = parent;
                        parent = parent->lgrp_parent;
                        continue;
                }

#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        prom_printf("lgrp_lineage_add: latency 0x%x,"
                            " parent latency 0x%x\n",
                            latency, parent->lgrp_latency);
#endif  /* DEBUG */
                /*
                 * As we work our way from the old leaf to the root lgroup,
                 * new leaf resources should go in between two lgroups or into
                 * one of the parent lgroups somewhere along the line
                 */
                if (latency < parent->lgrp_latency) {
                        lgrp_t  *intermed;

                        /*
                         * New leaf resources should go in between current
                         * child and parent
                         */
#ifdef  DEBUG
                        if (lgrp_topo_debug > 0)
                                prom_printf("lgrp_lineage_add: "
                                    "latency < parent latency\n");
#endif  /* DEBUG */

                        /*
                         * Create lgroup with desired resources and insert it
                         * between child and parent
                         */
                        lgrp_rsets_copy(child->lgrp_set, rset);
                        lgrp_rsets_add(newleaf->lgrp_set, rset);
                        if (nlevels >= lgrp_topo_levels) {

#ifdef  DEBUG
                                if (lgrp_topo_debug > 0) {
                                        prom_printf("lgrp_lineage_add: nlevels "
                                            "%d > lgrp_topo_levels %d\n",
                                            nlevels, lgrp_topo_levels);
                                        lgrp_rsets_print("rset ", rset);
                                }
#endif  /* DEBUG */

                                if (parent == lgrp_root) {
                                        /*
                                         * Don't proprogate new leaf resources
                                         * to parent, if it already contains
                                         * these resources
                                         */
                                        if (lgrp_rsets_member_all(
                                            parent->lgrp_set, newleaf->lgrp_id))
                                                break;

                                        total += lgrp_proprogate(newleaf, child,
                                            latency, &changes);
                                        break;
                                }

#ifdef  DEBUG
                                if (lgrp_topo_debug > 0) {
                                        prom_printf("lgrp_lineage_add: "
                                            "replaced parent lgrp %d at 0x%p"
                                            " for lgrp %d\n",
                                            parent->lgrp_id, (void *)parent,
                                            child->lgrp_id);
                                        lgrp_rsets_print("old parent"
                                            " resources:", parent->lgrp_set);
                                        lgrp_rsets_print("new parent "
                                            "resources:", rset);
                                }
#endif  /* DEBUG */
                                /*
                                 * Replace contents of parent with new
                                 * leaf + child resources since new leaf is
                                 * closer and shift its parent's resources to
                                 * its parent, etc. until root lgroup reached
                                 */
                                lgrp_rsets_replace(rset, latency, parent, 1);
                                if (*changed)
                                        klgrpset_or(*changed, parent->lgrp_id);
                                total++;
                                proprogate++;
                        } else {

#ifdef  DEBUG
                                if (lgrp_topo_debug > 0) {
                                        prom_printf("lgrp_lineage_add: "
                                            "lgrp_new_parent(0x%p,%d)\n",
                                            (void *)child, latency);
                                        lgrp_rsets_print("rset ", rset);
                                }
#endif  /* DEBUG */

                                total += lgrp_new_parent(child, latency, rset,
                                    &changes);
                                intermed = child->lgrp_parent;
                                klgrpset_add(collapse, intermed->lgrp_id);
                                if (changed)
                                        klgrpset_or(*changed, changes);
                                child = intermed;
                                proprogate++;
#ifdef  DEBUG
                                if (lgrp_topo_debug > 0) {
                                        prom_printf("lgrp_lineage_add: new "
                                            "parent lgrp %d at 0x%p for "
                                            "lgrp %d\n", intermed->lgrp_id,
                                            (void *)intermed, child->lgrp_id);
                                        lgrp_rsets_print("new parent "
                                            "resources:", rset);
                                }
#endif  /* DEBUG */
                                continue;
                        }

                } else if (latency == parent->lgrp_latency) {
                        /*
                         * New leaf resources should go into parent
                         */
#ifdef  DEBUG
                        if (lgrp_topo_debug > 0)
                                prom_printf("lgrp_lineage_add: latency == "
                                    "parent latency\n");
#endif  /* DEBUG */

                        /*
                         * It's already there, so don't need to do anything.
                         */
                        if (lgrp_rsets_member_all(parent->lgrp_set,
                            newleaf->lgrp_id))
                                break;

                        total += lgrp_proprogate(newleaf, child, latency,
                            &changes);
                        parent = child->lgrp_parent;
                        klgrpset_add(collapse, parent->lgrp_id);
                        if (changed)
                                klgrpset_or(*changed, changes);

                        proprogate++;
                }

                child = parent;
                parent = parent->lgrp_parent;
        } while (parent != NULL);

        /*
         * Consolidate any duplicate lgroups of ones just changed
         * Assume that there were no duplicates before last round of changes
         */
#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_lineage_add: collapsing dups....\n");
#endif  /* DEBUG */

        total += lgrp_collapse_dups(collapse, lgrp_collapse_equidist,
            &changes);
        if (changed)
                klgrpset_or(*changed, changes);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_lineage_add: changed %d lgrps: 0x%llx\n",
                    total, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (total);
}


/*
 * Add leaf lgroup to lgroup topology
 */
int
lgrp_leaf_add(lgrp_t *leaf, lgrp_t **lgrps, int lgrp_count,
    klgrpset_t *changed)
{
        klgrpset_t      changes;
        int             count;
        int             i;
        int             latency;

        ASSERT(MUTEX_HELD(&cpu_lock) || curthread->t_preempt > 0 ||
            !lgrp_initialized);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("\nlgrp_leaf_add(0x%p[%d],0x%p,%d,0x%p)\n",
                    (void *)leaf, leaf->lgrp_id, (void *)lgrps, lgrp_count,
                    (void *)changed);
#endif  /* DEBUG */

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        /*
         * Initialize parent of leaf lgroup to root
         */
        if (leaf->lgrp_parent == NULL) {
                leaf->lgrp_parent = lgrp_root;
                lgrp_root->lgrp_childcnt++;
                klgrpset_add(lgrp_root->lgrp_children, leaf->lgrp_id);

                klgrpset_or(lgrp_root->lgrp_leaves, leaf->lgrp_leaves);
                lgrp_rsets_add(leaf->lgrp_set, lgrp_root->lgrp_set);

#ifdef  DEBUG
                if (lgrp_topo_debug > 1)
                        lgrp_rsets_print("lgrp_leaf_add: root lgrp resources",
                            lgrp_root->lgrp_set);
#endif  /* DEBUG */

                if (changed) {
                        klgrpset_add(*changed, lgrp_root->lgrp_id);
                        klgrpset_add(*changed, leaf->lgrp_id);
                }
                count += 2;
        }

        /*
         * Can't add leaf lgroup to rest of topology (and vice versa) unless
         * latency for it is available
         */
        latency = lgrp_plat_latency(leaf->lgrp_plathand, leaf->lgrp_plathand);
        if (latency == 0) {
#ifdef  DEBUG
                if (lgrp_topo_debug > 1 && changed)
                        prom_printf("lgrp_leaf_add: changed %d lgrps: 0x%llx\n",
                            count, (u_longlong_t)*changed);
#endif  /* DEBUG */
                return (count);
        }

        /*
         * Make sure that root and leaf lgroup latencies are set
         */
        lgrp_root->lgrp_latency = lgrp_plat_latency(lgrp_root->lgrp_plathand,
            lgrp_root->lgrp_plathand);
        leaf->lgrp_latency = latency;

        /*
         * Add leaf to lineage of other leaves and vice versa
         * since leaves come into existence at different times
         */
        for (i = 0; i < lgrp_count; i++) {
                lgrp_t          *lgrp;

                lgrp = lgrps[i];

                /*
                 * Skip non-existent lgroups, new leaf lgroup, and
                 * non-leaf lgroups
                 */
                if (!LGRP_EXISTS(lgrp) || lgrp == leaf ||
                    lgrp->lgrp_childcnt != 0) {
#ifdef  DEBUG
                        if (lgrp_topo_debug > 1)
                                prom_printf("lgrp_leaf_add: skip "
                                    "lgrp %d at 0x%p\n",
                                    lgrp->lgrp_id, (void *)lgrp);
#endif  /* DEBUG */
                        continue;
                }

#ifdef  DEBUG
                if (lgrp_topo_debug > 0)
                        prom_printf("lgrp_leaf_add: lgrp %d (0x%p) =>"
                            " lgrp %d (0x%p)\n",
                            leaf->lgrp_id, (void *)leaf, lgrp->lgrp_id,
                            (void *)lgrp);
#endif  /* DEBUG */

                count += lgrp_lineage_add(leaf, lgrp, &changes);
                if (changed)
                        klgrpset_or(*changed, changes);

                count += lgrp_lineage_add(lgrp, leaf, &changes);
                if (changed)
                        klgrpset_or(*changed, changes);
        }

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_leaf_add: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (count);
}


/*
 * Remove resources of leaf from lgroup hierarchy
 */
int
lgrp_leaf_delete(lgrp_t *leaf, lgrp_t **lgrps, int lgrp_count,
    klgrpset_t *changed)
{
        klgrpset_t      changes;
        klgrpset_t      collapse;
        int             count;
        int             i;
        lgrp_t          *lgrp;

        ASSERT(MUTEX_HELD(&cpu_lock) || curthread->t_preempt > 0 ||
            !lgrp_initialized);

        count = 0;
        klgrpset_clear(collapse);
        if (changed)
                klgrpset_clear(*changed);

        /*
         * Nothing to do if no leaf given
         */
        if (leaf == NULL)
                return (0);

#ifdef  DEBUG
        if (lgrp_topo_debug > 0)
                prom_printf("lgrp_leaf_delete(0x%p[%d],0x%p,%d,0x%p)\n",
                    (void *)leaf, leaf->lgrp_id, (void *)lgrps, lgrp_count,
                    (void *)changed);
#endif  /* DEBUG */

        /*
         * Remove leaf from any lgroups containing its resources
         */
        for (i = 0; i < lgrp_count; i++) {
                lgrp = lgrps[i];
                if (lgrp == NULL || lgrp->lgrp_id == LGRP_NONE ||
                    !lgrp_rsets_member(lgrp->lgrp_set, leaf->lgrp_id))
                        continue;

#ifdef  DEBUG
                if (lgrp_topo_debug > 0)
                        prom_printf("lgrp_leaf_delete: remove leaf from"
                            " lgrp %d at %p\n", lgrp->lgrp_id, (void *)lgrp);
#endif  /* DEBUG */

                lgrp_rsets_delete(lgrp, leaf->lgrp_id, 0);
                klgrpset_del(lgrp->lgrp_leaves, leaf->lgrp_id);

                klgrpset_add(collapse, lgrp->lgrp_id);
                count++;
        }

        /*
         * Remove leaf and its ancestors that don't have any other children
         */
#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_leaf_delete: remove leaf and ancestors\n");
#endif  /* DEBUG */

        count += lgrp_ancestor_delete(leaf, &changes);
        klgrpset_or(collapse, changes);
        klgrpset_add(collapse, leaf->lgrp_id);
        count++;
        lgrp_destroy(leaf);

        /*
         * Consolidate any duplicate lgroups of ones just changed
         * Assume that there were no duplicates before last round of changes
         */
#ifdef  DEBUG
        if (lgrp_topo_debug > 1)
                prom_printf("lgrp_leaf_delete: collapsing dups\n");
#endif  /* DEBUG */
        count += lgrp_collapse_dups(collapse, lgrp_collapse_equidist,
            &changes);
        klgrpset_or(collapse, changes);
        if (changed)
                klgrpset_copy(*changed, collapse);

#ifdef  DEBUG
        if (lgrp_topo_debug > 1 && changed)
                prom_printf("lgrp_leaf_delete: changed %d lgrps: 0x%llx\n",
                    count, (u_longlong_t)*changed);
#endif  /* DEBUG */

        return (count);
}


/*
 * Flatten lgroup topology down to height specified
 */
int
lgrp_topo_flatten(int levels, lgrp_t **lgrps, int lgrp_count,
    klgrpset_t *changed)
{
        int     count;
        int     i;
        lgrp_t  *lgrp;
        lgrp_handle_t hdl;

        /*
         * Only flatten down to 2 level for now
         */
        if (levels != 2)
                return (0);

        /*
         * Look for non-leaf lgroups to remove and leaf lgroups to reparent
         */
        count = 0;
        for (i = 0; i <= lgrp_count; i++) {
                /*
                 * Skip non-existent lgroups and root
                 */
                lgrp = lgrps[i];
                if (!LGRP_EXISTS(lgrp))
                        continue;

                hdl = lgrp->lgrp_plathand;

                if (lgrp == lgrp_root) {
                        lgrp->lgrp_latency = lgrp_plat_latency(hdl, hdl);
                        continue;
                }

                if (lgrp->lgrp_childcnt > 0) {
                        lgrp_t  *parent;

                        /*
                         * Remove non-leaf lgroup from lgroup topology
                         */
                        parent = lgrp->lgrp_parent;
                        if (changed) {
                                klgrpset_add(*changed, lgrp->lgrp_id);
                                klgrpset_add(*changed, parent->lgrp_id);
                                count += 2;
                        }
                        if (parent) {
                                klgrpset_del(parent->lgrp_children,
                                    lgrp->lgrp_id);
                                parent->lgrp_childcnt--;
                        }
                        lgrp_destroy(lgrp);
                } else if (lgrp->lgrp_parent != lgrp_root) {
                        /*
                         * Reparent leaf lgroup to root
                         */
                        if (changed) {
                                klgrpset_add(*changed, lgrp_root->lgrp_id);
                                klgrpset_add(*changed, lgrp->lgrp_id);
                                count += 2;
                        }
                        lgrp->lgrp_parent = lgrp_root;
                        klgrpset_add(lgrp_root->lgrp_children, lgrp->lgrp_id);
                        lgrp_root->lgrp_childcnt++;
                        klgrpset_add(lgrp_root->lgrp_leaves, lgrp->lgrp_id);

                        lgrp->lgrp_latency = lgrp_plat_latency(hdl, hdl);
                }
        }

        return (count);
}


/*
 * Return current height limit for lgroup topology
 */
int
lgrp_topo_ht_limit(void)
{
        return (lgrp_topo_levels);
}


/*
 * Return default height limit for lgroup topology
 */
int
lgrp_topo_ht_limit_default(void)
{
        return (LGRP_TOPO_LEVELS);
}


/*
 * Set height limit for lgroup topology
 */
int
lgrp_topo_ht_limit_set(int ht)
{
        if (ht > LGRP_TOPO_LEVELS_MAX)
                lgrp_topo_levels = LGRP_TOPO_LEVELS_MAX;
        else
                lgrp_topo_levels = ht;

        return (ht);
}


/*
 * Update lgroup topology for any leaves that don't have their latency set
 *
 * This may happen on some machines when the lgroup platform support doesn't
 * know the latencies between nodes soon enough to provide it when the
 * resources are being added.  If the lgroup platform code needs to probe
 * memory to determine the latencies between nodes, it must wait until the
 * CPUs become active so at least one CPU in each node can probe memory in
 * each node.
 */
int
lgrp_topo_update(lgrp_t **lgrps, int lgrp_count, klgrpset_t *changed)
{
        klgrpset_t      changes;
        int             count;
        int             i;
        lgrp_t          *lgrp;

        count = 0;
        if (changed)
                klgrpset_clear(*changed);

        /*
         * For UMA machines, make sure that root lgroup contains all
         * resources.  The root lgrp should also name itself as its own leaf
         */
        if (nlgrps == 1) {
                for (i = 0; i < LGRP_RSRC_COUNT; i++)
                        klgrpset_add(lgrp_root->lgrp_set[i],
                            lgrp_root->lgrp_id);
                klgrpset_add(lgrp_root->lgrp_leaves, lgrp_root->lgrp_id);
                return (0);
        }

        mutex_enter(&cpu_lock);
        pause_cpus(NULL, NULL);

        /*
         * Look for any leaf lgroup without its latency set, finish adding it
         * to the lgroup topology assuming that it exists and has the root
         * lgroup as its parent, and update the memory nodes of all lgroups
         * that have it as a memory resource.
         */
        for (i = 0; i < lgrp_count; i++) {
                lgrp = lgrps[i];

                /*
                 * Skip non-existent and non-leaf lgroups and any lgroup
                 * with its latency set already
                 */
                if (lgrp == NULL || lgrp->lgrp_id == LGRP_NONE ||
                    lgrp->lgrp_childcnt != 0 || lgrp->lgrp_latency != 0)
                        continue;

#ifdef  DEBUG
                if (lgrp_topo_debug > 1) {
                        prom_printf("\nlgrp_topo_update: updating lineage "
                            "of lgrp %d at 0x%p\n", lgrp->lgrp_id,
                            (void *)lgrp);
                }
#endif  /* DEBUG */

                count += lgrp_leaf_add(lgrp, lgrps, lgrp_count, &changes);
                if (changed)
                        klgrpset_or(*changed, changes);

                if (!klgrpset_isempty(changes))
                        (void) lgrp_mnode_update(changes, NULL);

#ifdef  DEBUG
                if (lgrp_topo_debug > 1 && changed)
                        prom_printf("lgrp_topo_update: changed %d lgrps: "
                            "0x%llx\n",
                            count, (u_longlong_t)*changed);
#endif  /* DEBUG */
        }

        if (lgrp_topo_levels < LGRP_TOPO_LEVELS && lgrp_topo_levels == 2) {
                count += lgrp_topo_flatten(2, lgrps, lgrp_count, changed);
                (void) lpl_topo_flatten(2);
        }

        start_cpus();
        mutex_exit(&cpu_lock);

        return (count);
}

#ifdef  DEBUG
void
lgrp_print(lgrp_t *lgrp)
{
        lgrp_t  *parent;

        prom_printf("LGRP %d", lgrp->lgrp_id);
        if (lgrp->lgrp_childcnt == 0)
                prom_printf(" (plathand %p)\n",
                    (void *)lgrp->lgrp_plathand);
        else
                prom_printf("\n");

        prom_printf("\tlatency %d\n", lgrp->lgrp_latency);

        lgrp_rsets_print("\tresources", lgrp->lgrp_set);

        parent = lgrp->lgrp_parent;
        prom_printf("\tparent 0x%p", (void *)parent);
        if (parent)
                prom_printf("[%d]\n", parent->lgrp_id);
        else
                prom_printf("\n");

        prom_printf("\tchild count %d, children ", lgrp->lgrp_childcnt);
        klgrpset_print(lgrp->lgrp_children);

        prom_printf("\tleaves ");
        klgrpset_print(lgrp->lgrp_leaves);
}


void
lgrp_topo_print(lgrp_t **lgrps, int lgrp_max)
{
        klgrpset_t      siblings;

        lgrp_print(lgrp_root);
        siblings = lgrp_root->lgrp_children;
        while (!klgrpset_isempty(siblings)) {
                klgrpset_t      children;
                int             i;

                klgrpset_clear(children);
                for (i = 0; i <= lgrp_max; i++) {
                        lgrp_t  *lgrp;

                        lgrp = lgrps[i];
                        if (lgrp == NULL || !klgrpset_ismember(siblings, i))
                                continue;
                        lgrp_print(lgrp);
                        klgrpset_or(children, lgrp->lgrp_children);
                }
                klgrpset_copy(siblings, children);
        }
}
#endif  /* DEBUG */