Kristian Høgsberg | 3038e35 | 2006-12-19 19:58:27 -0500 | [diff] [blame^] | 1 | /* -*- c-basic-offset: 8 -*- |
| 2 | * |
| 3 | * fw-topology.c - Incremental bus scan, based on bus topology |
| 4 | * |
| 5 | * Copyright (C) 2004-2006 Kristian Hoegsberg <krh@bitplanet.net> |
| 6 | * |
| 7 | * This program is free software; you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU General Public License as published by |
| 9 | * the Free Software Foundation; either version 2 of the License, or |
| 10 | * (at your option) any later version. |
| 11 | * |
| 12 | * This program is distributed in the hope that it will be useful, |
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 15 | * GNU General Public License for more details. |
| 16 | * |
| 17 | * You should have received a copy of the GNU General Public License |
| 18 | * along with this program; if not, write to the Free Software Foundation, |
| 19 | * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
| 20 | */ |
| 21 | |
| 22 | #include <linux/module.h> |
| 23 | #include <linux/wait.h> |
| 24 | #include <linux/errno.h> |
| 25 | #include "fw-transaction.h" |
| 26 | #include "fw-topology.h" |
| 27 | |
| 28 | #define self_id_phy_id(q) (((q) >> 24) & 0x3f) |
| 29 | #define self_id_extended(q) (((q) >> 23) & 0x01) |
| 30 | #define self_id_link_on(q) (((q) >> 22) & 0x01) |
| 31 | #define self_id_gap_count(q) (((q) >> 16) & 0x3f) |
| 32 | #define self_id_phy_speed(q) (((q) >> 14) & 0x03) |
| 33 | #define self_id_contender(q) (((q) >> 11) & 0x01) |
| 34 | #define self_id_phy_initiator(q) (((q) >> 1) & 0x01) |
| 35 | #define self_id_more_packets(q) (((q) >> 0) & 0x01) |
| 36 | |
| 37 | #define self_id_ext_sequence(q) (((q) >> 20) & 0x07) |
| 38 | |
| 39 | static u32 *count_ports(u32 *sid, int *total_port_count, int *child_port_count) |
| 40 | { |
| 41 | u32 q; |
| 42 | int port_type, shift, seq; |
| 43 | |
| 44 | *total_port_count = 0; |
| 45 | *child_port_count = 0; |
| 46 | |
| 47 | shift = 6; |
| 48 | q = *sid; |
| 49 | seq = 0; |
| 50 | |
| 51 | while (1) { |
| 52 | port_type = (q >> shift) & 0x03; |
| 53 | switch (port_type) { |
| 54 | case SELFID_PORT_CHILD: |
| 55 | (*child_port_count)++; |
| 56 | case SELFID_PORT_PARENT: |
| 57 | case SELFID_PORT_NCONN: |
| 58 | (*total_port_count)++; |
| 59 | case SELFID_PORT_NONE: |
| 60 | break; |
| 61 | } |
| 62 | |
| 63 | shift -= 2; |
| 64 | if (shift == 0) { |
| 65 | if (!self_id_more_packets(q)) |
| 66 | return sid + 1; |
| 67 | |
| 68 | shift = 16; |
| 69 | sid++; |
| 70 | q = *sid; |
| 71 | |
| 72 | /* Check that the extra packets actually are |
| 73 | * extended self ID packets and that the |
| 74 | * sequence numbers in the extended self ID |
| 75 | * packets increase as expected. */ |
| 76 | |
| 77 | if (!self_id_extended(q) || |
| 78 | seq != self_id_ext_sequence(q)) |
| 79 | return NULL; |
| 80 | |
| 81 | seq++; |
| 82 | } |
| 83 | } |
| 84 | } |
| 85 | |
| 86 | static int get_port_type(u32 *sid, int port_index) |
| 87 | { |
| 88 | int index, shift; |
| 89 | |
| 90 | index = (port_index + 5) / 8; |
| 91 | shift = 16 - ((port_index + 5) & 7) * 2; |
| 92 | return (sid[index] >> shift) & 0x03; |
| 93 | } |
| 94 | |
| 95 | struct fw_node *fw_node_create(u32 sid, int port_count, int color) |
| 96 | { |
| 97 | struct fw_node *node; |
| 98 | |
| 99 | node = kzalloc(sizeof *node + port_count * sizeof node->ports[0], |
| 100 | GFP_ATOMIC); |
| 101 | if (node == NULL) |
| 102 | return NULL; |
| 103 | |
| 104 | node->color = color; |
| 105 | node->node_id = self_id_phy_id(sid); |
| 106 | node->link_on = self_id_link_on(sid); |
| 107 | node->phy_speed = self_id_phy_speed(sid); |
| 108 | node->port_count = port_count; |
| 109 | |
| 110 | atomic_set(&node->ref_count, 1); |
| 111 | INIT_LIST_HEAD(&node->link); |
| 112 | |
| 113 | return node; |
| 114 | } |
| 115 | |
| 116 | /** |
| 117 | * build_tree - Build the tree representation of the topology |
| 118 | * @self_ids: array of self IDs to create the tree from |
| 119 | * @self_id_count: the length of the self_ids array |
| 120 | * @local_id: the node ID of the local node |
| 121 | * |
| 122 | * This function builds the tree representation of the topology given |
| 123 | * by the self IDs from the latest bus reset. During the construction |
| 124 | * of the tree, the function checks that the self IDs are valid and |
| 125 | * internally consistent. On succcess this funtions returns the |
| 126 | * fw_node corresponding to the local card otherwise NULL. |
| 127 | */ |
| 128 | static struct fw_node *build_tree(struct fw_card *card) |
| 129 | { |
| 130 | struct fw_node *node, *child, *local_node; |
| 131 | struct list_head stack, *h; |
| 132 | u32 *sid, *next_sid, *end, q; |
| 133 | int i, port_count, child_port_count, phy_id, parent_count, stack_depth; |
| 134 | |
| 135 | local_node = NULL; |
| 136 | node = NULL; |
| 137 | INIT_LIST_HEAD(&stack); |
| 138 | stack_depth = 0; |
| 139 | sid = card->self_ids; |
| 140 | end = sid + card->self_id_count; |
| 141 | phy_id = 0; |
| 142 | card->irm_node = NULL; |
| 143 | |
| 144 | while (sid < end) { |
| 145 | next_sid = count_ports(sid, &port_count, &child_port_count); |
| 146 | |
| 147 | if (next_sid == NULL) { |
| 148 | fw_error("Inconsistent extended self IDs.\n"); |
| 149 | return NULL; |
| 150 | } |
| 151 | |
| 152 | q = *sid; |
| 153 | if (phy_id != self_id_phy_id(q)) { |
| 154 | fw_error("PHY ID mismatch in self ID: %d != %d.\n", |
| 155 | phy_id, self_id_phy_id(q)); |
| 156 | return NULL; |
| 157 | } |
| 158 | |
| 159 | if (child_port_count > stack_depth) { |
| 160 | fw_error("Topology stack underflow\n"); |
| 161 | return NULL; |
| 162 | } |
| 163 | |
| 164 | /* Seek back from the top of our stack to find the |
| 165 | * start of the child nodes for this node. */ |
| 166 | for (i = 0, h = &stack; i < child_port_count; i++) |
| 167 | h = h->prev; |
| 168 | child = fw_node(h); |
| 169 | |
| 170 | node = fw_node_create(q, port_count, card->color); |
| 171 | if (node == NULL) { |
| 172 | fw_error("Out of memory while building topology."); |
| 173 | return NULL; |
| 174 | } |
| 175 | |
| 176 | if (phy_id == (card->node_id & 0x3f)) |
| 177 | local_node = node; |
| 178 | |
| 179 | if (self_id_contender(q)) |
| 180 | card->irm_node = node; |
| 181 | |
| 182 | parent_count = 0; |
| 183 | |
| 184 | for (i = 0; i < port_count; i++) { |
| 185 | switch (get_port_type(sid, i)) { |
| 186 | case SELFID_PORT_PARENT: |
| 187 | /* Who's your daddy? We dont know the |
| 188 | * parent node at this time, so we |
| 189 | * temporarily abuse node->color for |
| 190 | * remembering the entry in the |
| 191 | * node->ports array where the parent |
| 192 | * node should be. Later, when we |
| 193 | * handle the parent node, we fix up |
| 194 | * the reference. |
| 195 | */ |
| 196 | parent_count++; |
| 197 | node->color = i; |
| 198 | break; |
| 199 | |
| 200 | case SELFID_PORT_CHILD: |
| 201 | node->ports[i].node = child; |
| 202 | /* Fix up parent reference for this |
| 203 | * child node. */ |
| 204 | child->ports[child->color].node = node; |
| 205 | child->color = card->color; |
| 206 | child = fw_node(child->link.next); |
| 207 | break; |
| 208 | } |
| 209 | } |
| 210 | |
| 211 | /* Check that the node reports exactly one parent |
| 212 | * port, except for the root, which of course should |
| 213 | * have no parents. */ |
| 214 | if ((next_sid == end && parent_count != 0) || |
| 215 | (next_sid < end && parent_count != 1)) { |
| 216 | fw_error("Parent port inconsistency for node %d: " |
| 217 | "parent_count=%d\n", phy_id, parent_count); |
| 218 | return NULL; |
| 219 | } |
| 220 | |
| 221 | /* Pop the child nodes off the stack and push the new node. */ |
| 222 | __list_del(h->prev, &stack); |
| 223 | list_add_tail(&node->link, &stack); |
| 224 | stack_depth += 1 - child_port_count; |
| 225 | |
| 226 | sid = next_sid; |
| 227 | phy_id++; |
| 228 | } |
| 229 | |
| 230 | card->root_node = node; |
| 231 | |
| 232 | return local_node; |
| 233 | } |
| 234 | |
| 235 | typedef void (*fw_node_callback_t) (struct fw_card * card, |
| 236 | struct fw_node * node, |
| 237 | struct fw_node * parent); |
| 238 | |
| 239 | static void |
| 240 | for_each_fw_node(struct fw_card *card, struct fw_node *root, |
| 241 | fw_node_callback_t callback) |
| 242 | { |
| 243 | struct list_head list; |
| 244 | struct fw_node *node, *next, *child, *parent; |
| 245 | int i; |
| 246 | |
| 247 | INIT_LIST_HEAD(&list); |
| 248 | |
| 249 | fw_node_get(root); |
| 250 | list_add_tail(&root->link, &list); |
| 251 | parent = NULL; |
| 252 | list_for_each_entry(node, &list, link) { |
| 253 | node->color = card->color; |
| 254 | |
| 255 | for (i = 0; i < node->port_count; i++) { |
| 256 | child = node->ports[i].node; |
| 257 | if (!child) |
| 258 | continue; |
| 259 | if (child->color == card->color) |
| 260 | parent = child; |
| 261 | else { |
| 262 | fw_node_get(child); |
| 263 | list_add_tail(&child->link, &list); |
| 264 | } |
| 265 | } |
| 266 | |
| 267 | callback(card, node, parent); |
| 268 | } |
| 269 | |
| 270 | list_for_each_entry_safe(node, next, &list, link) |
| 271 | fw_node_put(node); |
| 272 | } |
| 273 | |
| 274 | static void |
| 275 | report_lost_node(struct fw_card *card, |
| 276 | struct fw_node *node, struct fw_node *parent) |
| 277 | { |
| 278 | fw_node_event(card, node, FW_NODE_DESTROYED); |
| 279 | fw_node_put(node); |
| 280 | } |
| 281 | |
| 282 | static void |
| 283 | report_found_node(struct fw_card *card, |
| 284 | struct fw_node *node, struct fw_node *parent) |
| 285 | { |
| 286 | int b_path = (node->phy_speed == SCODE_BETA); |
| 287 | |
| 288 | if (parent != NULL) { |
| 289 | node->max_speed = min(parent->max_speed, node->phy_speed); |
| 290 | node->b_path = parent->b_path && b_path; |
| 291 | } else { |
| 292 | node->max_speed = node->phy_speed; |
| 293 | node->b_path = b_path; |
| 294 | } |
| 295 | |
| 296 | fw_node_event(card, node, FW_NODE_CREATED); |
| 297 | } |
| 298 | |
| 299 | void fw_destroy_nodes(struct fw_card *card) |
| 300 | { |
| 301 | unsigned long flags; |
| 302 | |
| 303 | spin_lock_irqsave(&card->lock, flags); |
| 304 | card->color++; |
| 305 | if (card->local_node != NULL) |
| 306 | for_each_fw_node(card, card->local_node, report_lost_node); |
| 307 | spin_unlock_irqrestore(&card->lock, flags); |
| 308 | } |
| 309 | |
| 310 | static void move_tree(struct fw_node *node0, struct fw_node *node1, int port) |
| 311 | { |
| 312 | struct fw_node *tree; |
| 313 | int i; |
| 314 | |
| 315 | tree = node1->ports[port].node; |
| 316 | node0->ports[port].node = tree; |
| 317 | for (i = 0; i < tree->port_count; i++) { |
| 318 | if (tree->ports[i].node == node1) { |
| 319 | tree->ports[i].node = node0; |
| 320 | break; |
| 321 | } |
| 322 | } |
| 323 | } |
| 324 | |
| 325 | /** |
| 326 | * update_tree - compare the old topology tree for card with the new |
| 327 | * one specified by root. Queue the nodes and mark them as either |
| 328 | * found, lost or updated. Update the nodes in the card topology tree |
| 329 | * as we go. |
| 330 | */ |
| 331 | static void |
| 332 | update_tree(struct fw_card *card, struct fw_node *root, int *changed) |
| 333 | { |
| 334 | struct list_head list0, list1; |
| 335 | struct fw_node *node0, *node1; |
| 336 | int i, event; |
| 337 | |
| 338 | INIT_LIST_HEAD(&list0); |
| 339 | list_add_tail(&card->local_node->link, &list0); |
| 340 | INIT_LIST_HEAD(&list1); |
| 341 | list_add_tail(&root->link, &list1); |
| 342 | |
| 343 | node0 = fw_node(list0.next); |
| 344 | node1 = fw_node(list1.next); |
| 345 | *changed = 0; |
| 346 | |
| 347 | while (&node0->link != &list0) { |
| 348 | |
| 349 | /* assert(node0->port_count == node1->port_count); */ |
| 350 | if (node0->link_on && !node1->link_on) |
| 351 | event = FW_NODE_LINK_OFF; |
| 352 | else if (!node0->link_on && node1->link_on) |
| 353 | event = FW_NODE_LINK_ON; |
| 354 | else |
| 355 | event = FW_NODE_UPDATED; |
| 356 | |
| 357 | node0->node_id = node1->node_id; |
| 358 | node0->color = card->color; |
| 359 | node0->link_on = node1->link_on; |
| 360 | node0->initiated_reset = node1->initiated_reset; |
| 361 | node1->color = card->color; |
| 362 | fw_node_event(card, node0, event); |
| 363 | |
| 364 | if (card->root_node == node1) |
| 365 | card->root_node = node0; |
| 366 | if (card->irm_node == node1) |
| 367 | card->irm_node = node0; |
| 368 | |
| 369 | for (i = 0; i < node0->port_count; i++) { |
| 370 | if (node0->ports[i].node && node1->ports[i].node) { |
| 371 | /* This port didn't change, queue the |
| 372 | * connected node for further |
| 373 | * investigation. */ |
| 374 | if (node0->ports[i].node->color == card->color) |
| 375 | continue; |
| 376 | list_add_tail(&node0->ports[i].node->link, |
| 377 | &list0); |
| 378 | list_add_tail(&node1->ports[i].node->link, |
| 379 | &list1); |
| 380 | } else if (node0->ports[i].node) { |
| 381 | /* The nodes connected here were |
| 382 | * unplugged; unref the lost nodes and |
| 383 | * queue FW_NODE_LOST callbacks for |
| 384 | * them. */ |
| 385 | |
| 386 | for_each_fw_node(card, node0->ports[i].node, |
| 387 | report_lost_node); |
| 388 | node0->ports[i].node = NULL; |
| 389 | *changed = 1; |
| 390 | } else if (node1->ports[i].node) { |
| 391 | /* One or more node were connected to |
| 392 | * this port. Move the new nodes into |
| 393 | * the tree and queue FW_NODE_CREATED |
| 394 | * callbacks for them. */ |
| 395 | move_tree(node0, node1, i); |
| 396 | for_each_fw_node(card, node0->ports[i].node, |
| 397 | report_found_node); |
| 398 | *changed = 1; |
| 399 | } |
| 400 | } |
| 401 | |
| 402 | node0 = fw_node(node0->link.next); |
| 403 | node1 = fw_node(node1->link.next); |
| 404 | } |
| 405 | } |
| 406 | |
| 407 | void |
| 408 | fw_core_handle_bus_reset(struct fw_card *card, |
| 409 | int node_id, int generation, |
| 410 | int self_id_count, u32 * self_ids) |
| 411 | { |
| 412 | struct fw_node *local_node; |
| 413 | unsigned long flags; |
| 414 | int changed; |
| 415 | |
| 416 | fw_flush_transactions(card); |
| 417 | |
| 418 | spin_lock_irqsave(&card->lock, flags); |
| 419 | |
| 420 | card->node_id = node_id; |
| 421 | card->self_id_count = self_id_count; |
| 422 | card->generation = generation; |
| 423 | memcpy(card->self_ids, self_ids, self_id_count * 4); |
| 424 | |
| 425 | local_node = build_tree(card); |
| 426 | |
| 427 | card->color++; |
| 428 | |
| 429 | if (local_node == NULL) { |
| 430 | fw_error("topology build failed\n"); |
| 431 | /* FIXME: We need to issue a bus reset in this case. */ |
| 432 | } else if (card->local_node == NULL) { |
| 433 | card->local_node = local_node; |
| 434 | for_each_fw_node(card, local_node, report_found_node); |
| 435 | } else { |
| 436 | update_tree(card, local_node, &changed); |
| 437 | } |
| 438 | |
| 439 | spin_unlock_irqrestore(&card->lock, flags); |
| 440 | } |
| 441 | |
| 442 | EXPORT_SYMBOL(fw_core_handle_bus_reset); |
| 443 | |
| 444 | void fw_node_event(struct fw_card *card, struct fw_node *node, int event) |
| 445 | { |
| 446 | } |