/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License, Version 1.0 only
 * (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) 1999-2000 by Sun Microsystems, Inc.
 * All rights reserved.
 */

/*
 * s1394_bus_reset.c
 *    1394 Services Layer Bus Reset Routines
 *    These routines handle all of the tasks relating to 1394 bus resets
 */

#include <sys/conf.h>
#include <sys/ddi.h>
#include <sys/sunddi.h>
#include <sys/types.h>
#include <sys/kmem.h>
#include <sys/1394/t1394.h>
#include <sys/1394/s1394.h>
#include <sys/1394/h1394.h>
#include <sys/1394/ieee1394.h>
#include <sys/1394/ieee1212.h>

static uint8_t selfid_speed(s1394_selfid_pkt_t *s);

static int selfid_num_ports(s1394_selfid_pkt_t *s);

static int selfid_port_type(s1394_selfid_pkt_t *s, int port);

static void s1394_hal_stack_push(s1394_hal_t *hal, void *o);

static void *s1394_hal_stack_pop(s1394_hal_t *hal);

static void s1394_hal_queue_insert(s1394_hal_t *hal, void *o);

static void *s1394_hal_queue_remove(s1394_hal_t *hal);

static void s1394_node_number_list_add(s1394_hal_t *hal, int node_num);

static void s1394_speed_map_fill_speed_N(s1394_hal_t *hal, int min_spd);

static void s1394_speed_map_initialize(s1394_hal_t *hal);

int s1394_ignore_invalid_gap_cnt = 0; /* patch for invalid gap_cnts */

/*
 * Gap_count look-up table (See IEEE P1394a Table C-2) - Draft 3.0
 * (modified from original table IEEE 1394-1995 8.4.6.2)
 */
static int gap_count[MAX_HOPS + 1] = {
        0, 5, 7, 8, 10, 13, 16, 18, 21,
        24, 26, 29, 32, 35, 37, 40, 43,
        46, 48, 51, 54, 57, 59, 62
};

/*
 * s1394_parse_selfid_buffer()
 *    takes the SelfID data buffer and parses it, testing whether each packet
 *    is valid (has a correct inverse packet) and setting the pointers in
 *    selfid_ptrs[] to the appropriate offsets within the buffer.
 */
int
s1394_parse_selfid_buffer(s1394_hal_t *hal, void *selfid_buf_addr,
    uint32_t selfid_size)
{
        s1394_selfid_pkt_t *s;
        uint32_t           *data;
        uint_t             i = 0;
        uint_t             j = 0;
        boolean_t          error = B_FALSE;
        int                valid_pkt_id;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        data = (uint32_t *)selfid_buf_addr;

        if (selfid_size == 0) {
                /* Initiate a bus reset */
                s1394_initiate_hal_reset(hal, CRITICAL);

                /* Set error status */
                error = B_TRUE;

                /* Release HAL lock and return */
                goto parse_buffer_done;
        }

        /* Convert bytes to quadlets */
        selfid_size = selfid_size >> 2;

        while (j < selfid_size) {
                valid_pkt_id = ((data[j] & IEEE1394_SELFID_PCKT_ID_MASK) >>
                    IEEE1394_SELFID_PCKT_ID_SHIFT);

                s = (s1394_selfid_pkt_t *)(&data[j]);

                /* Test if packet has valid inverse quadlet */
                if (IEEE1394_SELFID_ISVALID(s) &&
                    (valid_pkt_id == IEEE1394_SELFID_PCKT_ID_VALID)) {

                        hal->selfid_ptrs[i] = s;

                        /* While this packet contains multiple quadlets */
                        j += 2;

                        while (IEEE1394_SELFID_ISMORE(s)) {
                                valid_pkt_id =
                                    ((data[j] & IEEE1394_SELFID_PCKT_ID_MASK) >>
                                    IEEE1394_SELFID_PCKT_ID_SHIFT);

                                s = (s1394_selfid_pkt_t *)(&data[j]);

                                /* Test if packet has valid inverse quadlet */
                                if (IEEE1394_SELFID_ISVALID(s) &&
                                    (valid_pkt_id ==
                                        IEEE1394_SELFID_PCKT_ID_VALID)) {
                                        j += 2;
                                } else {
                                        /* Initiate a bus reset */
                                        s1394_initiate_hal_reset(hal, CRITICAL);

                                        /* Set error status */
                                        error = B_TRUE;

                                        /* Release HAL lock and return */
                                        goto parse_buffer_done;
                                }
                        }
                        i++;
                } else {
                        /* Initiate a bus reset */
                        s1394_initiate_hal_reset(hal, CRITICAL);

                        /* Set error status */
                        error = B_TRUE;

                        /* Release HAL lock and return */
                        goto parse_buffer_done;
                }
        }

        hal->number_of_nodes = i;

parse_buffer_done:
        if (error == B_TRUE)
                return (DDI_FAILURE);
        else
                return (DDI_SUCCESS);
}

/*
 * s1394_sort_selfids()
 *    takes the selfid_ptrs[] in the HAL struct and sorts them by node number,
 *    using a heapsort.
 */
void
s1394_sort_selfids(s1394_hal_t *hal)
{
        s1394_selfid_pkt_t *current;
        uint_t             number_of_nodes;
        int                i;
        int                j;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;

        /* We start at one because the root has no parent to check */
        for (i = 1; i < number_of_nodes; i++) {
                current = hal->selfid_ptrs[i];
                j = i;
                while ((j > 0) && (IEEE1394_SELFID_PHYID(current) >
                    IEEE1394_SELFID_PHYID(hal->selfid_ptrs[j / 2]))) {
                        hal->selfid_ptrs[j] = hal->selfid_ptrs[j / 2];
                        hal->selfid_ptrs[j / 2] = current;
                        j = j / 2;
                }
        }

        for (i = number_of_nodes - 1; i > 0; i--) {
                current = hal->selfid_ptrs[i];
                hal->selfid_ptrs[i] = hal->selfid_ptrs[0];
                hal->selfid_ptrs[0] = current;
                j = 0;
                while (2 * j + 1 < i) {
                        if (2 * j + 2 >= i) {
                                if (IEEE1394_SELFID_PHYID(current) <
                                    IEEE1394_SELFID_PHYID(
                                        hal->selfid_ptrs[2 * j + 1])) {
                                        hal->selfid_ptrs[j] =
                                            hal->selfid_ptrs[2 * j + 1];
                                        hal->selfid_ptrs[2 * j + 1] = current;
                                        j = 2 * j + 1;
                                }
                                break;
                        }

                        if (IEEE1394_SELFID_PHYID(hal->selfid_ptrs[2 * j + 1]) >
                            IEEE1394_SELFID_PHYID(
                                hal->selfid_ptrs[2 * j + 2])) {
                                if (IEEE1394_SELFID_PHYID(current) <
                                    IEEE1394_SELFID_PHYID(
                                        hal->selfid_ptrs[2 * j + 1])) {
                                        hal->selfid_ptrs[j] =
                                            hal->selfid_ptrs[2 * j + 1];
                                        hal->selfid_ptrs[2 * j + 1] = current;
                                        j = 2 * j + 1;
                                } else {
                                        break;
                                }
                        } else {
                                if (IEEE1394_SELFID_PHYID(current) <
                                    IEEE1394_SELFID_PHYID(
                                        hal->selfid_ptrs[2 * j + 2])) {
                                        hal->selfid_ptrs[j] =
                                            hal->selfid_ptrs[2 * j + 2];
                                        hal->selfid_ptrs[2 * j + 2] = current;
                                        j = 2 * j + 2;
                                } else {
                                        break;
                                }
                        }
                }
        }
}

/*
 * selfid_speed()
 *    examines the "sp" bits for a given packet (see IEEE 1394-1995 4.3.4.1)
 *    and returns the node's speed capabilities.
 */
static uint8_t
selfid_speed(s1394_selfid_pkt_t *s)
{
        uint32_t sp;

        sp = ((s->spkt_data & IEEE1394_SELFID_SP_MASK) >>
            IEEE1394_SELFID_SP_SHIFT);

        switch (sp) {
        case IEEE1394_S100:
        case IEEE1394_S200:
        case IEEE1394_S400:
                return (sp);

        /*
         * To verify higher speeds we should look at PHY register #3
         * on this node.  This will need to be done to support P1394b
         */
        default:
                return (IEEE1394_S400);
        }
}

/*
 * selfid_num_ports()
 *    determines whether a packet is multi-part or single, and from this it
 *    calculates the number of ports which have been specified.
 *    (See IEEE 1394-1995 4.3.4.1)
 */
static int
selfid_num_ports(s1394_selfid_pkt_t *s)
{
        int     p = 3;

        while (IEEE1394_SELFID_ISMORE(s)) {
                p += 8;
                s++;
        }

        /* Threshold the number of ports at the P1394A defined maximum */
        /* (see P1394A Draft 3.0 - Section 8.5.1) */
        if (p > IEEE1394_MAX_NUM_PORTS)
                p = IEEE1394_MAX_NUM_PORTS;

        return (p);
}

/*
 * selfid_port_type()
 *    determines what type of node the specified port connects to.
 *    (See IEEE 1394-1995 4.3.4.1)
 */
static int
selfid_port_type(s1394_selfid_pkt_t *s, int port)
{
        int     block;
        int     offset = IEEE1394_SELFID_PORT_OFFSET_FIRST;

        if (port > 2) {
                /* Calculate which quadlet and bits for this port */
                port -= 3;
                block = (port >> 3) + 1;
                port = port % 8;
                /* Move to the correct quadlet */
                s += block;
                offset = IEEE1394_SELFID_PORT_OFFSET_OTHERS;
        }

        /* Shift by appropriate number of bits and mask */
        return ((s->spkt_data >> (offset - 2 * port)) & 0x00000003);
}

/*
 * s1394_init_topology_tree()
 *    frees any config rom's allocated in the topology tree before zapping it.
 *    If it gets a bus reset before the tree is marked processed, there will
 *    be memory allocated for cfgrom's being read. If there is no tree copy,
 *    topology would still be topology tree from the previous generation and
 *    if we bzero'd the tree, we will have a memory leak. To avoid this leak,
 *    walk through the tree and free any config roms in nodes that are NOT
 *    matched. (For matched nodes, we ensure that nodes in old and topology
 *    tree point to the same area of memory.)
 */
void
s1394_init_topology_tree(s1394_hal_t *hal, boolean_t copied,
    ushort_t number_of_nodes)
{
        s1394_node_t    *node;
        uint32_t        *config_rom;
        uint_t          tree_size;
        int             i;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        /*
         * if copied is false, we want to free any cfgrom memory that is
         * not referenced to in both topology and old trees. However, we
         * don't use hal->number_of_nodes as the number of nodes to look at.
         * The reason being we could be seeing the bus reset before the
         * state is appropriate for a tree copy (which need
         * toplogy_tree_processed to be true) and some nodes might have
         * departed in this generation and hal->number_of_nodes reflects
         * the number of nodes in this generation. Use number_of_nodes that
         * gets passed into this routine as the actual number of nodes to
         * look at.
         */
        if (copied == B_FALSE) {
                /* Free any cfgrom alloced and zap the node */
                for (i = 0; i < number_of_nodes; i++) {
                        node = &hal->topology_tree[i];
                        config_rom = node->cfgrom;
                        if (config_rom != NULL) {
                                if (CFGROM_NEW_ALLOC(node) == B_TRUE) {
                                        kmem_free((void *)config_rom,
                                            IEEE1394_CONFIG_ROM_SZ);
                                }
                        }
                }
        }

        tree_size = hal->number_of_nodes * sizeof (s1394_node_t);
        bzero((void *)hal->topology_tree, tree_size);
}

/*
 * s1394_topology_tree_build()
 *    takes the selfid_ptrs[] and builds the topology_tree[] by examining
 *    the node numbers (the order in which the nodes responded to SelfID).
 *    It sets the port pointers, leaf label, parent port, and
 *    s1394_selfid_packet_t pointer in each node.
 */
int
s1394_topology_tree_build(s1394_hal_t *hal)
{
        s1394_node_t    *tmp;
        uint32_t        number_of_nodes;
        boolean_t       push_to_orphan_stack = B_FALSE;
        boolean_t       found_parent = B_FALSE;
        boolean_t       found_connection = B_FALSE;
        int             i;
        int             j;

        /*
         * The method for building the tree is described in IEEE 1394-1995
         * (Annex E.3.4).  We use an "Orphan" stack to keep track of Child
         * nodes which have yet to find their Parent node.
         */

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;

        /* Flush the Stack */
        hal->hal_stack_depth = -1;

        /* For each node on the bus initialize its topology_tree entry */
        for (i = 0; i < number_of_nodes; i++) {
                /* Make sure that node numbers are correct */
                if (i != IEEE1394_SELFID_PHYID(hal->selfid_ptrs[i])) {
                        /* Initiate a bus reset */
                        s1394_initiate_hal_reset(hal, CRITICAL);

                        return (DDI_FAILURE);
                }

                hal->topology_tree[i].selfid_packet = hal->selfid_ptrs[i];
                hal->topology_tree[i].parent_port = (char)NO_PARENT;
                hal->topology_tree[i].is_a_leaf = 1;
                hal->topology_tree[i].node_num = (uchar_t)i;
        }

        for (i = 0; i < number_of_nodes; i++) {
                /* Current node has no parent yet */
                found_parent = B_FALSE;

                /* Current node has no connections yet */
                found_connection = B_FALSE;

                /* Initialize all ports on this node */
                for (j = 0; j < IEEE1394_MAX_NUM_PORTS; j++)
                        hal->topology_tree[i].phy_port[j] = NULL;

                /* For each port on the node - highest to lowest */
                for (j = selfid_num_ports(hal->selfid_ptrs[i]) - 1;
                    j >= 0; j--) {
                        if (selfid_port_type(hal->selfid_ptrs[i], j) ==
                            IEEE1394_SELFID_PORT_TO_PARENT) {

                                found_connection = B_TRUE;
                                if (found_parent == B_FALSE) {
                                        push_to_orphan_stack = B_TRUE;
                                        hal->topology_tree[i].parent_port =
                                            (char)j;
                                        found_parent = B_TRUE;

                                } else {
                                        /* Initiate a bus reset */
                                        s1394_initiate_hal_reset(hal, CRITICAL);

                                        return (DDI_FAILURE);
                                }
                        } else if (selfid_port_type(hal->selfid_ptrs[i], j) ==
                            IEEE1394_SELFID_PORT_TO_CHILD) {

                                found_connection = B_TRUE;
                                tmp = (s1394_node_t *)s1394_hal_stack_pop(hal);
                                if (tmp == NULL) {
                                        /* Initiate a bus reset */
                                        s1394_initiate_hal_reset(hal, CRITICAL);

                                        return (DDI_FAILURE);
                                }

                                hal->topology_tree[i].phy_port[j] = tmp;
                                hal->topology_tree[i].is_a_leaf = 0;
                                tmp->phy_port[tmp->parent_port] =
                                    &hal->topology_tree[i];
                        }
                }

                /* If current node has no parents or children - Invalid */
                if ((found_connection == B_FALSE) && (number_of_nodes > 1)) {
                        /* Initiate a bus reset */
                        s1394_initiate_hal_reset(hal, CRITICAL);

                        return (DDI_FAILURE);
                }

                /* Push it on the "Orphan" stack if it has no parent yet */
                if (push_to_orphan_stack == B_TRUE) {
                        push_to_orphan_stack = B_FALSE;
                        s1394_hal_stack_push(hal, &hal->topology_tree[i]);
                }
        }

        /* If the stack is not empty, then something has gone seriously wrong */
        if (hal->hal_stack_depth != -1) {
                /* Initiate a bus reset */
                s1394_initiate_hal_reset(hal, CRITICAL);

                return (DDI_FAILURE);
        }

        /* New topology tree is now valid */
        hal->topology_tree_valid = B_TRUE;

        return (DDI_SUCCESS);
}

/*
 * s1394_hal_stack_push()
 *    checks that the stack is not full, and puts the pointer on top of the
 *    HAL's stack if it isn't.  This routine is used only by the
 *    h1394_self_ids() interrupt.
 */
static void
s1394_hal_stack_push(s1394_hal_t *hal, void *obj)
{
        if (hal->hal_stack_depth < IEEE1394_MAX_NODES - 1) {
                hal->hal_stack_depth++;
                hal->hal_stack[hal->hal_stack_depth] = obj;
        }
}

/*
 * s1394_hal_stack_pop()
 *    checks that the stack is not empty, and pops and returns the pointer
 *    from the top of the HAL's stack if it isn't.  This routine is used
 *    only by the h1394_self_ids() interrupt.
 */
static void *
s1394_hal_stack_pop(s1394_hal_t *hal)
{
        if (hal->hal_stack_depth > -1) {
                hal->hal_stack_depth--;
                return (hal->hal_stack[hal->hal_stack_depth + 1]);

        } else {
                return (NULL);
        }
}

/*
 * s1394_hal_queue_insert()
 *    checks that the queue is not full, and puts the object in the front
 *    of the HAL's queue if it isn't.  This routine is used only by the
 *    h1394_self_ids() interrupt.
 */
static void
s1394_hal_queue_insert(s1394_hal_t *hal, void *obj)
{
        if (((hal->hal_queue_front + 1) % IEEE1394_MAX_NODES) ==
            hal->hal_queue_back) {
                return;
        } else {
                hal->hal_queue[hal->hal_queue_front] = obj;
                hal->hal_queue_front = (hal->hal_queue_front + 1) %
                    IEEE1394_MAX_NODES;
        }
}


/*
 * s1394_hal_queue_remove()
 *    checks that the queue is not empty, and pulls the object off the back
 *    of the HAL's queue (and returns it) if it isn't.  This routine is used
 *    only by the h1394_self_ids() interrupt.
 */
static void *
s1394_hal_queue_remove(s1394_hal_t *hal)
{
        void    *tmp;

        if (hal->hal_queue_back == hal->hal_queue_front) {
                return (NULL);
        } else {
                tmp = hal->hal_queue[hal->hal_queue_back];
                hal->hal_queue_back = (hal->hal_queue_back + 1) %
                    IEEE1394_MAX_NODES;
                return (tmp);
        }
}


/*
 * s1394_node_number_list_add()
 *    checks that the node_number_list is not full and puts the node number
 *    in the list.  The function is used primarily by s1394_speed_map_fill()
 *    to keep track of which connections need to be set in the speed_map[].
 *    This routine is used only by the h1394_self_ids() interrupt.
 */
static void
s1394_node_number_list_add(s1394_hal_t *hal, int node_num)
{
        if (hal->hal_node_number_list_size >= IEEE1394_MAX_NODES - 1) {
                return;
        }

        hal->hal_node_number_list[hal->hal_node_number_list_size] = node_num;
        hal->hal_node_number_list_size++;
}

/*
 * s1394_topology_tree_mark_all_unvisited()
 *    is used to initialize the topology_tree[] prior to tree traversals.
 *    It resets the "visited" flag for each node in the tree.
 */
void
s1394_topology_tree_mark_all_unvisited(s1394_hal_t *hal)
{
        uint_t  number_of_nodes;
        int     i;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;
        for (i = 0; i < number_of_nodes; i++)
                CLEAR_NODE_VISITED(&hal->topology_tree[i]);
}

/*
 * s1394_old_tree_mark_all_unvisited()
 *    is used to initialize the old_tree[] prior to tree traversals.  It
 *    resets the "visited" flag for each node in the tree.
 */
void
s1394_old_tree_mark_all_unvisited(s1394_hal_t *hal)
{
        uint_t  number_of_nodes;
        int     i;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->old_number_of_nodes;
        for (i = 0; i < number_of_nodes; i++)
                CLEAR_NODE_VISITED(&hal->old_tree[i]);
}

/*
 * s1394_old_tree_mark_all_unmatched()
 *    is used to initialize the old_tree[] prior to tree traversals.  It
 *    resets the "matched" flag for each node in the tree.
 */
void
s1394_old_tree_mark_all_unmatched(s1394_hal_t *hal)
{
        uint_t  number_of_nodes;
        int     i;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->old_number_of_nodes;

        for (i = 0; i < number_of_nodes; i++)
            CLEAR_NODE_MATCHED(&hal->old_tree[i]);
}

/*
 * s1394_copy_old_tree()
 *    switches the pointers for old_tree[] and topology_tree[].
 */
void
s1394_copy_old_tree(s1394_hal_t *hal)
{
        s1394_node_t    *temp;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        temp = hal->old_tree;
        hal->old_tree = hal->topology_tree;
        hal->topology_tree = temp;

        hal->old_number_of_nodes = hal->number_of_nodes;
        hal->old_node_id = hal->node_id;
        hal->old_generation_count = hal->generation_count;

        /* Old tree is now valid and filled also */
        hal->old_tree_valid = B_TRUE;
}


/*
 * s1394_match_tree_nodes()
 *    uses the information contained in the SelfID packets of the nodes in
 *    both the old_tree[] and the topology_tree[] to determine which new
 *    nodes correspond to old nodes.  Starting with the local node, we
 *    compare both old and new node's ports.  Assuming that only one bus
 *    reset has occurred, any node that was connected to another in the old
 *    bus and is still connected to another in the new bus must be connected
 *    (physically) to the same node.  Using this information, we can rebuild
 *    and match the old nodes to new ones.  Any nodes which aren't matched
 *    are either departing or arriving nodes and must be handled appropriately.
 */
void
s1394_match_tree_nodes(s1394_hal_t *hal)
{
        s1394_node_t    *tmp;
        uint_t          hal_node_num;
        uint_t          hal_node_num_old;
        int             i;
        int             port_type;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        /* To ensure that the queue is empty */
        hal->hal_queue_front = hal->hal_queue_back = 0;

        /* Set up the first matched nodes (which are our own local nodes) */
        hal_node_num = IEEE1394_NODE_NUM(hal->node_id);
        hal_node_num_old = IEEE1394_NODE_NUM(hal->old_node_id);
        hal->topology_tree[hal_node_num].old_node =
            &hal->old_tree[hal_node_num_old];
        hal->old_tree[hal_node_num_old].cur_node =
            &hal->topology_tree[hal_node_num];

        /* Put the node on the queue */
        s1394_hal_queue_insert(hal, &hal->topology_tree[hal_node_num]);

        /* While the queue is not empty, remove a node */
        while (hal->hal_queue_front != hal->hal_queue_back) {
                tmp = (s1394_node_t *)s1394_hal_queue_remove(hal);

            /* Mark both old and new nodes as "visited" */
            SET_NODE_VISITED(tmp);
            SET_NODE_VISITED(tmp->old_node);
            tmp->old_node->cur_node = tmp;

            /* Mark old and new nodes as "matched" */
            SET_NODE_MATCHED(tmp);
            SET_NODE_MATCHED(tmp->old_node);
            s1394_copy_cfgrom(tmp, tmp->old_node);

            /* s1394_copy_cfgrom() clears "matched" for some cases... */
            if ((tmp->cfgrom != NULL && CONFIG_ROM_GEN(tmp->cfgrom) <= 1) ||
                NODE_MATCHED(tmp) == B_TRUE) {
                /* Move the target list over to the new node and update */
                /* the node info. */
                        s1394_target_t *t;

                        rw_enter(&hal->target_list_rwlock, RW_WRITER);
                        t = tmp->target_list = tmp->old_node->target_list;
                        while (t != NULL) {
                                t->on_node = tmp;
                                t = t->target_sibling;
                        }
                        rw_exit(&hal->target_list_rwlock);
                }

                for (i = 0; i < selfid_num_ports(tmp->selfid_packet); i++) {
                        port_type = selfid_port_type(tmp->selfid_packet, i);

                        /* Is the new port connected? */
                        if ((port_type == IEEE1394_SELFID_PORT_TO_CHILD) ||
                            (port_type == IEEE1394_SELFID_PORT_TO_PARENT)) {
                                port_type = selfid_port_type(
                                    tmp->old_node->selfid_packet, i);

                                /* Is the old port connected? */
                                if ((port_type ==
                                        IEEE1394_SELFID_PORT_TO_CHILD) ||
                                    (port_type ==
                                        IEEE1394_SELFID_PORT_TO_PARENT)) {
                                        /* Found a match, check if */
                                        /* we've already visited it */
                                        if (!NODE_VISITED(tmp->phy_port[i])) {
                                                tmp->phy_port[i]->old_node =
                                                    tmp->old_node->phy_port[i];
                                                s1394_hal_queue_insert(hal,
                                                    tmp->phy_port[i]);
                                        }
                                }
                        }
                }
        }
}

/*
 * s1394_topology_tree_calculate_diameter()
 *    does a depth-first tree traversal, tracking at each branch the first
 *    and second deepest paths though that branch's children.  The diameter
 *    is given by the maximum of these over all branch nodes
 */
int
s1394_topology_tree_calculate_diameter(s1394_hal_t *hal)
{
        s1394_node_t    *current;
        uint_t          number_of_nodes;
        int             i;
        int             start;
        int             end;
        boolean_t       done;
        boolean_t       found_a_child;
        int             distance = 0;
        int             diameter = 0;
        int             local_diameter = 0;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;

        /* Initialize topology tree */
        for (i = 0; i < number_of_nodes; i++) {
                hal->topology_tree[i].max_1st = 0;
                hal->topology_tree[i].max_2nd = 0;
                hal->topology_tree[i].last_port_checked = 0;
        }

        /* Start at the root node */
        current = s1394_topology_tree_get_root_node(hal);

        /* Flush the stack before we start */
        hal->hal_stack_depth = -1;

        do {
                done            = B_FALSE;
                found_a_child   = B_FALSE;
                start           = current->last_port_checked;
                end             = selfid_num_ports(current->selfid_packet);

                /* Check every previously unchecked port for children */
                for (i = start; i < end; i++) {
                        current->last_port_checked++;
                        /* If there is a child push it on the stack */
                        if (selfid_port_type(current->selfid_packet, i) ==
                            IEEE1394_SELFID_PORT_TO_CHILD) {
                                found_a_child = B_TRUE;
                                s1394_hal_stack_push(hal, current);
                                current = current->phy_port[i];
                                break;
                        }
                }

                /* If we reach here and the stack is empty, we're done */
                if (hal->hal_stack_depth == -1) {
                        done = B_TRUE;
                        continue;
                }

                /* If no children were found, we're at a leaf */
                if (found_a_child == B_FALSE) {
                        distance = current->max_1st + 1;
                        /* Pop the child and set the appropriate fields */
                        current = s1394_hal_stack_pop(hal);
                        if (distance > current->max_1st) {
                                current->max_2nd = current->max_1st;
                                current->max_1st = (uchar_t)distance;

                        } else if (distance > current->max_2nd) {
                                current->max_2nd = (uchar_t)distance;
                        }

                        /* Update maximum distance (diameter), if necessary */
                        local_diameter = current->max_1st + current->max_2nd;
                        if (local_diameter > diameter)
                                diameter = local_diameter;
                }
        } while (done == B_FALSE);

        return (diameter);
}

/*
 * s1394_gap_count_optimize()
 *    looks in a table to find the appropriate gap_count for a given diameter.
 *    (See above - gap_count[])
 */
int
s1394_gap_count_optimize(int diameter)
{
        if ((diameter >= 0) && (diameter <= MAX_HOPS)) {
                return (gap_count[diameter]);
        } else {
                cmn_err(CE_NOTE, "Too may point-to-point links on the 1394"
                    " bus - If new devices have recently been added, remove"
                    " them.");
                return (gap_count[MAX_HOPS]);
        }
}

/*
 * s1394_get_current_gap_count()
 *    looks at all the SelfID packets to determine the current gap_count on
 *    the 1394 bus.  If the gap_counts differ from node to node, it initiates
 *    a bus reset and returns -1.
 */
int
s1394_get_current_gap_count(s1394_hal_t *hal)
{
        int     i;
        int     gap_count = -1;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        /* Grab the first gap_count in the SelfID packets */
        gap_count = IEEE1394_SELFID_GAP_CNT(hal->selfid_ptrs[0]);

        /* Compare it too all the rest */
        for (i = 1; i < hal->number_of_nodes; i++) {
                if (gap_count !=
                    IEEE1394_SELFID_GAP_CNT(hal->selfid_ptrs[i])) {

                        /* Inconsistent gap counts */
                        if (s1394_ignore_invalid_gap_cnt == 0) {
                                /* Initiate a bus reset */
                                s1394_initiate_hal_reset(hal, CRITICAL);
                        }

                        return (-1);
                }
        }

        return (gap_count);
}

/*
 * s1394_speed_map_fill()
 *    determines, for each pair of nodes, the maximum speed at which those
 *    nodes can communicate.  The speed of each node as well as the speed of
 *    any intermediate nodes on a given path must be accounted for, as the
 *    minimum speed on a given edge determines the maximum speed for all
 *    communications across that edge.
 *    In the method we implement below, a current minimum speed is selected.
 *    With this minimum speed in mind, we create subgraphs of the original
 *    bus which contain only edges that connect two nodes whose speeds are
 *    equal to or greater than the current minimum speed.  Then, for each of
 *    the subgraphs, we visit every node, keeping a list of the nodes we've
 *    visited.  When this list is completed, we can fill in the entries in
 *    the speed map which correspond to a pairs of these nodes.  Doing this
 *    for each subgraph and then for each speed we progressively fill in the
 *    parts of the speed map which weren't previously filled in.
 */
void
s1394_speed_map_fill(s1394_hal_t *hal)
{
        uint_t  number_of_nodes;
        int     i;
        int     j;
        int     node_num;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;
        s1394_speed_map_initialize(hal);

        /* Mark all speed = IEEE1394_S100 nodes in the Speed Map */
        for (i = 0; i < number_of_nodes; i++) {
                if (selfid_speed(hal->topology_tree[i].selfid_packet) ==
                    IEEE1394_S100) {
                        hal->slowest_node_speed = IEEE1394_S100;
                        node_num = IEEE1394_SELFID_PHYID(
                            hal->topology_tree[i].selfid_packet);
                        for (j = 0; j < number_of_nodes; j++) {
                                if (j != node_num) {
                                        hal->speed_map[node_num][j] =
                                            IEEE1394_S100;
                                        hal->speed_map[j][node_num] =
                                            IEEE1394_S100;
                                }
                        }
                }
        }

        s1394_speed_map_fill_speed_N(hal, IEEE1394_S200);
        s1394_speed_map_fill_speed_N(hal, IEEE1394_S400);

        /* Fill in the diagonal */
        for (i = 0; i < number_of_nodes; i++) {
                hal->speed_map[i][i] =
                    selfid_speed(hal->topology_tree[i].selfid_packet);
        }
}

/*
 * s1394_speed_map_fill_speed_N(),
 *    given a minimum link speed, creates subgraphs of the original bus which
 *    contain only the necessary edges (see speed_map_fill() above).  For each
 *    of the subgraphs, it visits and fills in the entries in the speed map
 *    which correspond to a pair of these nodes.
 */
static void
s1394_speed_map_fill_speed_N(s1394_hal_t *hal, int min_spd)
{
        s1394_node_t    *tmp;
        uint_t          number_of_nodes;
        int             i;
        int             j;
        int             k;
        int             size;
        int             ix_a, ix_b;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;

        /* Prepare the topology tree */
        s1394_topology_tree_mark_all_unvisited(hal);

        /* To ensure that the queue is empty */
        hal->hal_queue_front = hal->hal_queue_back = 0;

        for (i = 0; i < number_of_nodes; i++) {
                /* If the node's speed == min_spd and it hasn't been visited */
                if (!NODE_VISITED(&hal->topology_tree[i]) &&
                    (selfid_speed(hal->topology_tree[i].selfid_packet) ==
                        min_spd)) {

                        if (min_spd < hal->slowest_node_speed)
                                hal->slowest_node_speed = (uint8_t)min_spd;

                        SET_NODE_VISITED(&hal->topology_tree[i]);
                        s1394_hal_queue_insert(hal, &hal->topology_tree[i]);

                        while (hal->hal_queue_front != hal->hal_queue_back) {
                                tmp = (s1394_node_t *)s1394_hal_queue_remove(
                                    hal);
                                /* Add node number to the list */
                                s1394_node_number_list_add(hal,
                                    IEEE1394_SELFID_PHYID(tmp->selfid_packet));

                                for (j = 0; j < IEEE1394_MAX_NUM_PORTS; j++) {
                                        if ((tmp->phy_port[j] != NULL) &&
                                            (!NODE_VISITED(tmp->phy_port[j]))) {
                                                if (selfid_speed(
                                                    tmp->phy_port[j]->
                                                    selfid_packet) >= min_spd) {
                                                        SET_NODE_VISITED(
                                                            tmp->phy_port[j]);
                                                        s1394_hal_queue_insert(
                                                            hal,
                                                            tmp->phy_port[j]);
                                                }
                                        }
                                }
                        }

                        /* For each pair, mark speed_map as min_spd */
                        size = hal->hal_node_number_list_size;
                        for (j = 0; j < size; j++) {
                                for (k = 0; k < size; k++) {
                                        if (j != k) {
                                                ix_a = hal->
                                                    hal_node_number_list[j];
                                                ix_b = hal->
                                                    hal_node_number_list[k];
                                                hal->speed_map[ix_a][ix_b] =
                                                    (uint8_t)min_spd;
                                        }
                                }
                        }

                        /* Flush the Node Number List */
                        hal->hal_node_number_list_size = 0;
                }
        }
}

/*
 * s1394_speed_map_initialize()
 *    fills in the speed_map with IEEE1394_S100's and SPEED_MAP_INVALID's in
 *    the appropriate places.  These will be overwritten by
 *    s1394_speed_map_fill().
 */
static void
s1394_speed_map_initialize(s1394_hal_t *hal)
{
        uint_t  number_of_nodes;
        int     i, j;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;
        for (i = 0; i < number_of_nodes; i++) {
                for (j = 0; j < number_of_nodes; j++) {
                        if (i != j)
                                hal->speed_map[i][j] = IEEE1394_S100;
                        else
                                hal->speed_map[i][j] = SPEED_MAP_INVALID;
                }
        }
}

/*
 * s1394_speed_map_get()
 *    queries the speed_map[] for a given pair of nodes.
 */
uint8_t
s1394_speed_map_get(s1394_hal_t *hal, uint_t from_node, uint_t to_node)
{
        /* If it's not a valid node, then return slowest_node_speed */
        if (to_node >= hal->number_of_nodes) {
                /* Send at fastest speed everyone will see */
                return (hal->slowest_node_speed);
        }
        /* else return the correct maximum speed */
        return (hal->speed_map[from_node][to_node]);
}

/*
 * s1394_update_speed_map_link_speeds()
 *    takes into account information from Config ROM queries.  Any P1394A
 *    device can have a link with a different speed than its PHY.  In this
 *    case, the slower speed must be accounted for in order for communication
 *    with the remote node to work.
 */
void
s1394_update_speed_map_link_speeds(s1394_hal_t *hal)
{
        uint32_t bus_capabilities;
        uint8_t  link_speed;
        uint_t   number_of_nodes;
        int      i, j;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        number_of_nodes = hal->number_of_nodes;

        for (i = 0; i < number_of_nodes; i++) {

                /* Skip invalid config ROMs */
                if (CFGROM_VALID(&hal->topology_tree[i])) {

                        ASSERT(hal->topology_tree[i].cfgrom);

                        bus_capabilities = hal->topology_tree[i].
                            cfgrom[IEEE1212_NODE_CAP_QUAD];

                        /* Skip if Bus_Info_Block generation is 0 */
                        /* because it isn't a P1394a device */
                        if ((bus_capabilities & IEEE1394_BIB_GEN_MASK) != 0) {
                                link_speed = (bus_capabilities &
                                    IEEE1394_BIB_LNK_SPD_MASK);

                                for (j = 0; j < number_of_nodes; j++) {
                                        /* Update if link_speed is slower */
                                        if (hal->speed_map[i][j] > link_speed) {
                                                hal->speed_map[i][j] =
                                                    link_speed;
                                                hal->speed_map[j][i] =
                                                    link_speed;
                                        }

                                        if (link_speed <
                                            hal->slowest_node_speed)
                                                hal->slowest_node_speed =
                                                    link_speed;
                                }
                        }
                }
        }
}

/*
 * s1394_get_isoch_rsrc_mgr()
 *    looks at the SelfID packets to determine the Isochronous Resource
 *    Manager's node ID.  The IRM is the highest numbered node with both
 *    the "L"-bit and the "C"-bit in its SelfID packets turned on.  If no
 *    IRM is found on the bus, then -1 is returned.
 */
int
s1394_get_isoch_rsrc_mgr(s1394_hal_t *hal)
{
        int     i;

        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        for (i = hal->number_of_nodes - 1; i >= 0; i--) {
                /* Highest numbered node with L=1 and C=1 */
                if ((IEEE1394_SELFID_ISLINKON(hal->selfid_ptrs[i])) &&
                    (IEEE1394_SELFID_ISCONTENDER(hal->selfid_ptrs[i]))) {
                        return (i);
                }
        }

        /* No Isochronous Resource Manager */
        return (-1);
}

/*
 * s1394_physical_arreq_setup_all()
 *    is used to enable the physical filters for the link.  If a target has
 *    registered physical space allocations, then the corresponding node's
 *    bit is set.  This is done for all targets on a HAL (usually after bus
 *    reset).
 */
void
s1394_physical_arreq_setup_all(s1394_hal_t *hal)
{
        s1394_target_t  *curr_target;
        uint64_t        mask = 0;
        uint32_t        node_num;
        uint_t          generation;

        ASSERT(MUTEX_NOT_HELD(&hal->topology_tree_mutex));

        mutex_enter(&hal->topology_tree_mutex);
        generation = hal->generation_count;
        rw_enter(&hal->target_list_rwlock, RW_READER);

        curr_target = hal->target_head;
        while (curr_target != NULL) {
                if ((curr_target->on_node != NULL) &&
                    (curr_target->physical_arreq_enabled != 0)) {
                        node_num = curr_target->on_node->node_num;
                        mask = mask | (1 << node_num);
                }
                curr_target = curr_target->target_next;
        }
        rw_exit(&hal->target_list_rwlock);
        mutex_exit(&hal->topology_tree_mutex);

        /*
         * Since it is cleared to 0 on bus reset, set the bits for all
         * nodes.  This call returns DDI_FAILURE if the generation passed
         * is invalid or if the HAL is shutdown.  In either case, it is
         * acceptable to simply ignore the result and return.
         */
        (void) HAL_CALL(hal).physical_arreq_enable_set(
            hal->halinfo.hal_private, mask, generation);
}

/*
 * s1394_physical_arreq_set_one()
 *    is used to enable the physical filters for the link.  If a target has
 *    registered physical space allocations, then the corresponding node's
 *    bit is set.  This is done for one target.
 */
void
s1394_physical_arreq_set_one(s1394_target_t *target)
{
        s1394_hal_t     *hal;
        uint64_t        mask = 0;
        uint32_t        node_num;
        uint_t          generation;

        /* Find the HAL this target resides on */
        hal = target->on_hal;

        ASSERT(MUTEX_NOT_HELD(&hal->topology_tree_mutex));

        mutex_enter(&hal->topology_tree_mutex);
        rw_enter(&hal->target_list_rwlock, RW_READER);

        if ((target->on_node != NULL) &&
            (target->physical_arreq_enabled != 0)) {
                node_num = target->on_node->node_num;
                mask = mask | (1 << node_num);

                generation = hal->generation_count;

                rw_exit(&hal->target_list_rwlock);
                mutex_exit(&hal->topology_tree_mutex);

                /*
                 * Set the bit corresponding to this node.  This call
                 * returns DDI_FAILURE if the generation passed
                 * is invalid or if the HAL is shutdown.  In either case,
                 * it is acceptable to simply ignore the result and return.
                 */
                (void) HAL_CALL(hal).physical_arreq_enable_set(
                    hal->halinfo.hal_private, mask, generation);
        } else {
                rw_exit(&hal->target_list_rwlock);
                mutex_exit(&hal->topology_tree_mutex);
        }
}

/*
 * s1394_physical_arreq_clear_one()
 *    is used to disable the physical filters for OpenHCI.  If a target frees
 *    up the last of its registered physical space, then the corresponding
 *    node's bit is cleared.  This is done for one target.
 */
void
s1394_physical_arreq_clear_one(s1394_target_t *target)
{
        s1394_hal_t     *hal;
        uint64_t        mask = 0;
        uint32_t        node_num;
        uint_t          generation;

        /* Find the HAL this target resides on */
        hal = target->on_hal;

        ASSERT(MUTEX_NOT_HELD(&hal->topology_tree_mutex));

        mutex_enter(&hal->topology_tree_mutex);
        rw_enter(&hal->target_list_rwlock, RW_READER);

        if ((target->on_node != NULL) &&
            (target->physical_arreq_enabled == 0)) {
                node_num = target->on_node->node_num;
                mask = mask | (1 << node_num);

                generation = hal->generation_count;

                rw_exit(&hal->target_list_rwlock);
                mutex_exit(&hal->topology_tree_mutex);

                /*
                 * Set the bit corresponding to this node.  This call
                 * returns DDI_FAILURE if the generation passed
                 * is invalid or if the HAL is shutdown.  In either case,
                 * it is acceptable to simply ignore the result and return.
                 */
                (void) HAL_CALL(hal).physical_arreq_enable_clr(
                    hal->halinfo.hal_private, mask, generation);
        } else {
                rw_exit(&hal->target_list_rwlock);
                mutex_exit(&hal->topology_tree_mutex);
        }
}

/*
 * s1394_topology_tree_get_root_node()
 *    returns the last entry in topology_tree[] as this must always be the
 *    root node.
 */
s1394_node_t *
s1394_topology_tree_get_root_node(s1394_hal_t *hal)
{
        ASSERT(MUTEX_HELD(&hal->topology_tree_mutex));

        return (&hal->topology_tree[hal->number_of_nodes - 1]);
}