diff options
Diffstat (limited to 'drivers/firewire/core-topology.c')
-rw-r--r-- | drivers/firewire/core-topology.c | 572 |
1 files changed, 572 insertions, 0 deletions
diff --git a/drivers/firewire/core-topology.c b/drivers/firewire/core-topology.c new file mode 100644 index 000000000000..fddf2b358936 --- /dev/null +++ b/drivers/firewire/core-topology.c | |||
@@ -0,0 +1,572 @@ | |||
1 | /* | ||
2 | * Incremental bus scan, based on bus topology | ||
3 | * | ||
4 | * Copyright (C) 2004-2006 Kristian Hoegsberg <krh@bitplanet.net> | ||
5 | * | ||
6 | * This program is free software; you can redistribute it and/or modify | ||
7 | * it under the terms of the GNU General Public License as published by | ||
8 | * the Free Software Foundation; either version 2 of the License, or | ||
9 | * (at your option) any later version. | ||
10 | * | ||
11 | * This program is distributed in the hope that it will be useful, | ||
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
14 | * GNU General Public License for more details. | ||
15 | * | ||
16 | * You should have received a copy of the GNU General Public License | ||
17 | * along with this program; if not, write to the Free Software Foundation, | ||
18 | * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. | ||
19 | */ | ||
20 | |||
21 | #include <linux/bug.h> | ||
22 | #include <linux/errno.h> | ||
23 | #include <linux/firewire.h> | ||
24 | #include <linux/firewire-constants.h> | ||
25 | #include <linux/jiffies.h> | ||
26 | #include <linux/kernel.h> | ||
27 | #include <linux/list.h> | ||
28 | #include <linux/module.h> | ||
29 | #include <linux/slab.h> | ||
30 | #include <linux/spinlock.h> | ||
31 | #include <linux/string.h> | ||
32 | |||
33 | #include <asm/atomic.h> | ||
34 | #include <asm/system.h> | ||
35 | |||
36 | #include "core.h" | ||
37 | |||
38 | #define SELF_ID_PHY_ID(q) (((q) >> 24) & 0x3f) | ||
39 | #define SELF_ID_EXTENDED(q) (((q) >> 23) & 0x01) | ||
40 | #define SELF_ID_LINK_ON(q) (((q) >> 22) & 0x01) | ||
41 | #define SELF_ID_GAP_COUNT(q) (((q) >> 16) & 0x3f) | ||
42 | #define SELF_ID_PHY_SPEED(q) (((q) >> 14) & 0x03) | ||
43 | #define SELF_ID_CONTENDER(q) (((q) >> 11) & 0x01) | ||
44 | #define SELF_ID_PHY_INITIATOR(q) (((q) >> 1) & 0x01) | ||
45 | #define SELF_ID_MORE_PACKETS(q) (((q) >> 0) & 0x01) | ||
46 | |||
47 | #define SELF_ID_EXT_SEQUENCE(q) (((q) >> 20) & 0x07) | ||
48 | |||
49 | #define SELFID_PORT_CHILD 0x3 | ||
50 | #define SELFID_PORT_PARENT 0x2 | ||
51 | #define SELFID_PORT_NCONN 0x1 | ||
52 | #define SELFID_PORT_NONE 0x0 | ||
53 | |||
54 | static u32 *count_ports(u32 *sid, int *total_port_count, int *child_port_count) | ||
55 | { | ||
56 | u32 q; | ||
57 | int port_type, shift, seq; | ||
58 | |||
59 | *total_port_count = 0; | ||
60 | *child_port_count = 0; | ||
61 | |||
62 | shift = 6; | ||
63 | q = *sid; | ||
64 | seq = 0; | ||
65 | |||
66 | while (1) { | ||
67 | port_type = (q >> shift) & 0x03; | ||
68 | switch (port_type) { | ||
69 | case SELFID_PORT_CHILD: | ||
70 | (*child_port_count)++; | ||
71 | case SELFID_PORT_PARENT: | ||
72 | case SELFID_PORT_NCONN: | ||
73 | (*total_port_count)++; | ||
74 | case SELFID_PORT_NONE: | ||
75 | break; | ||
76 | } | ||
77 | |||
78 | shift -= 2; | ||
79 | if (shift == 0) { | ||
80 | if (!SELF_ID_MORE_PACKETS(q)) | ||
81 | return sid + 1; | ||
82 | |||
83 | shift = 16; | ||
84 | sid++; | ||
85 | q = *sid; | ||
86 | |||
87 | /* | ||
88 | * Check that the extra packets actually are | ||
89 | * extended self ID packets and that the | ||
90 | * sequence numbers in the extended self ID | ||
91 | * packets increase as expected. | ||
92 | */ | ||
93 | |||
94 | if (!SELF_ID_EXTENDED(q) || | ||
95 | seq != SELF_ID_EXT_SEQUENCE(q)) | ||
96 | return NULL; | ||
97 | |||
98 | seq++; | ||
99 | } | ||
100 | } | ||
101 | } | ||
102 | |||
103 | static int get_port_type(u32 *sid, int port_index) | ||
104 | { | ||
105 | int index, shift; | ||
106 | |||
107 | index = (port_index + 5) / 8; | ||
108 | shift = 16 - ((port_index + 5) & 7) * 2; | ||
109 | return (sid[index] >> shift) & 0x03; | ||
110 | } | ||
111 | |||
112 | static struct fw_node *fw_node_create(u32 sid, int port_count, int color) | ||
113 | { | ||
114 | struct fw_node *node; | ||
115 | |||
116 | node = kzalloc(sizeof(*node) + port_count * sizeof(node->ports[0]), | ||
117 | GFP_ATOMIC); | ||
118 | if (node == NULL) | ||
119 | return NULL; | ||
120 | |||
121 | node->color = color; | ||
122 | node->node_id = LOCAL_BUS | SELF_ID_PHY_ID(sid); | ||
123 | node->link_on = SELF_ID_LINK_ON(sid); | ||
124 | node->phy_speed = SELF_ID_PHY_SPEED(sid); | ||
125 | node->initiated_reset = SELF_ID_PHY_INITIATOR(sid); | ||
126 | node->port_count = port_count; | ||
127 | |||
128 | atomic_set(&node->ref_count, 1); | ||
129 | INIT_LIST_HEAD(&node->link); | ||
130 | |||
131 | return node; | ||
132 | } | ||
133 | |||
134 | /* | ||
135 | * Compute the maximum hop count for this node and it's children. The | ||
136 | * maximum hop count is the maximum number of connections between any | ||
137 | * two nodes in the subtree rooted at this node. We need this for | ||
138 | * setting the gap count. As we build the tree bottom up in | ||
139 | * build_tree() below, this is fairly easy to do: for each node we | ||
140 | * maintain the max hop count and the max depth, ie the number of hops | ||
141 | * to the furthest leaf. Computing the max hop count breaks down into | ||
142 | * two cases: either the path goes through this node, in which case | ||
143 | * the hop count is the sum of the two biggest child depths plus 2. | ||
144 | * Or it could be the case that the max hop path is entirely | ||
145 | * containted in a child tree, in which case the max hop count is just | ||
146 | * the max hop count of this child. | ||
147 | */ | ||
148 | static void update_hop_count(struct fw_node *node) | ||
149 | { | ||
150 | int depths[2] = { -1, -1 }; | ||
151 | int max_child_hops = 0; | ||
152 | int i; | ||
153 | |||
154 | for (i = 0; i < node->port_count; i++) { | ||
155 | if (node->ports[i] == NULL) | ||
156 | continue; | ||
157 | |||
158 | if (node->ports[i]->max_hops > max_child_hops) | ||
159 | max_child_hops = node->ports[i]->max_hops; | ||
160 | |||
161 | if (node->ports[i]->max_depth > depths[0]) { | ||
162 | depths[1] = depths[0]; | ||
163 | depths[0] = node->ports[i]->max_depth; | ||
164 | } else if (node->ports[i]->max_depth > depths[1]) | ||
165 | depths[1] = node->ports[i]->max_depth; | ||
166 | } | ||
167 | |||
168 | node->max_depth = depths[0] + 1; | ||
169 | node->max_hops = max(max_child_hops, depths[0] + depths[1] + 2); | ||
170 | } | ||
171 | |||
172 | static inline struct fw_node *fw_node(struct list_head *l) | ||
173 | { | ||
174 | return list_entry(l, struct fw_node, link); | ||
175 | } | ||
176 | |||
177 | /** | ||
178 | * build_tree - Build the tree representation of the topology | ||
179 | * @self_ids: array of self IDs to create the tree from | ||
180 | * @self_id_count: the length of the self_ids array | ||
181 | * @local_id: the node ID of the local node | ||
182 | * | ||
183 | * This function builds the tree representation of the topology given | ||
184 | * by the self IDs from the latest bus reset. During the construction | ||
185 | * of the tree, the function checks that the self IDs are valid and | ||
186 | * internally consistent. On succcess this function returns the | ||
187 | * fw_node corresponding to the local card otherwise NULL. | ||
188 | */ | ||
189 | static struct fw_node *build_tree(struct fw_card *card, | ||
190 | u32 *sid, int self_id_count) | ||
191 | { | ||
192 | struct fw_node *node, *child, *local_node, *irm_node; | ||
193 | struct list_head stack, *h; | ||
194 | u32 *next_sid, *end, q; | ||
195 | int i, port_count, child_port_count, phy_id, parent_count, stack_depth; | ||
196 | int gap_count; | ||
197 | bool beta_repeaters_present; | ||
198 | |||
199 | local_node = NULL; | ||
200 | node = NULL; | ||
201 | INIT_LIST_HEAD(&stack); | ||
202 | stack_depth = 0; | ||
203 | end = sid + self_id_count; | ||
204 | phy_id = 0; | ||
205 | irm_node = NULL; | ||
206 | gap_count = SELF_ID_GAP_COUNT(*sid); | ||
207 | beta_repeaters_present = false; | ||
208 | |||
209 | while (sid < end) { | ||
210 | next_sid = count_ports(sid, &port_count, &child_port_count); | ||
211 | |||
212 | if (next_sid == NULL) { | ||
213 | fw_error("Inconsistent extended self IDs.\n"); | ||
214 | return NULL; | ||
215 | } | ||
216 | |||
217 | q = *sid; | ||
218 | if (phy_id != SELF_ID_PHY_ID(q)) { | ||
219 | fw_error("PHY ID mismatch in self ID: %d != %d.\n", | ||
220 | phy_id, SELF_ID_PHY_ID(q)); | ||
221 | return NULL; | ||
222 | } | ||
223 | |||
224 | if (child_port_count > stack_depth) { | ||
225 | fw_error("Topology stack underflow\n"); | ||
226 | return NULL; | ||
227 | } | ||
228 | |||
229 | /* | ||
230 | * Seek back from the top of our stack to find the | ||
231 | * start of the child nodes for this node. | ||
232 | */ | ||
233 | for (i = 0, h = &stack; i < child_port_count; i++) | ||
234 | h = h->prev; | ||
235 | /* | ||
236 | * When the stack is empty, this yields an invalid value, | ||
237 | * but that pointer will never be dereferenced. | ||
238 | */ | ||
239 | child = fw_node(h); | ||
240 | |||
241 | node = fw_node_create(q, port_count, card->color); | ||
242 | if (node == NULL) { | ||
243 | fw_error("Out of memory while building topology.\n"); | ||
244 | return NULL; | ||
245 | } | ||
246 | |||
247 | if (phy_id == (card->node_id & 0x3f)) | ||
248 | local_node = node; | ||
249 | |||
250 | if (SELF_ID_CONTENDER(q)) | ||
251 | irm_node = node; | ||
252 | |||
253 | parent_count = 0; | ||
254 | |||
255 | for (i = 0; i < port_count; i++) { | ||
256 | switch (get_port_type(sid, i)) { | ||
257 | case SELFID_PORT_PARENT: | ||
258 | /* | ||
259 | * Who's your daddy? We dont know the | ||
260 | * parent node at this time, so we | ||
261 | * temporarily abuse node->color for | ||
262 | * remembering the entry in the | ||
263 | * node->ports array where the parent | ||
264 | * node should be. Later, when we | ||
265 | * handle the parent node, we fix up | ||
266 | * the reference. | ||
267 | */ | ||
268 | parent_count++; | ||
269 | node->color = i; | ||
270 | break; | ||
271 | |||
272 | case SELFID_PORT_CHILD: | ||
273 | node->ports[i] = child; | ||
274 | /* | ||
275 | * Fix up parent reference for this | ||
276 | * child node. | ||
277 | */ | ||
278 | child->ports[child->color] = node; | ||
279 | child->color = card->color; | ||
280 | child = fw_node(child->link.next); | ||
281 | break; | ||
282 | } | ||
283 | } | ||
284 | |||
285 | /* | ||
286 | * Check that the node reports exactly one parent | ||
287 | * port, except for the root, which of course should | ||
288 | * have no parents. | ||
289 | */ | ||
290 | if ((next_sid == end && parent_count != 0) || | ||
291 | (next_sid < end && parent_count != 1)) { | ||
292 | fw_error("Parent port inconsistency for node %d: " | ||
293 | "parent_count=%d\n", phy_id, parent_count); | ||
294 | return NULL; | ||
295 | } | ||
296 | |||
297 | /* Pop the child nodes off the stack and push the new node. */ | ||
298 | __list_del(h->prev, &stack); | ||
299 | list_add_tail(&node->link, &stack); | ||
300 | stack_depth += 1 - child_port_count; | ||
301 | |||
302 | if (node->phy_speed == SCODE_BETA && | ||
303 | parent_count + child_port_count > 1) | ||
304 | beta_repeaters_present = true; | ||
305 | |||
306 | /* | ||
307 | * If PHYs report different gap counts, set an invalid count | ||
308 | * which will force a gap count reconfiguration and a reset. | ||
309 | */ | ||
310 | if (SELF_ID_GAP_COUNT(q) != gap_count) | ||
311 | gap_count = 0; | ||
312 | |||
313 | update_hop_count(node); | ||
314 | |||
315 | sid = next_sid; | ||
316 | phy_id++; | ||
317 | } | ||
318 | |||
319 | card->root_node = node; | ||
320 | card->irm_node = irm_node; | ||
321 | card->gap_count = gap_count; | ||
322 | card->beta_repeaters_present = beta_repeaters_present; | ||
323 | |||
324 | return local_node; | ||
325 | } | ||
326 | |||
327 | typedef void (*fw_node_callback_t)(struct fw_card * card, | ||
328 | struct fw_node * node, | ||
329 | struct fw_node * parent); | ||
330 | |||
331 | static void for_each_fw_node(struct fw_card *card, struct fw_node *root, | ||
332 | fw_node_callback_t callback) | ||
333 | { | ||
334 | struct list_head list; | ||
335 | struct fw_node *node, *next, *child, *parent; | ||
336 | int i; | ||
337 | |||
338 | INIT_LIST_HEAD(&list); | ||
339 | |||
340 | fw_node_get(root); | ||
341 | list_add_tail(&root->link, &list); | ||
342 | parent = NULL; | ||
343 | list_for_each_entry(node, &list, link) { | ||
344 | node->color = card->color; | ||
345 | |||
346 | for (i = 0; i < node->port_count; i++) { | ||
347 | child = node->ports[i]; | ||
348 | if (!child) | ||
349 | continue; | ||
350 | if (child->color == card->color) | ||
351 | parent = child; | ||
352 | else { | ||
353 | fw_node_get(child); | ||
354 | list_add_tail(&child->link, &list); | ||
355 | } | ||
356 | } | ||
357 | |||
358 | callback(card, node, parent); | ||
359 | } | ||
360 | |||
361 | list_for_each_entry_safe(node, next, &list, link) | ||
362 | fw_node_put(node); | ||
363 | } | ||
364 | |||
365 | static void report_lost_node(struct fw_card *card, | ||
366 | struct fw_node *node, struct fw_node *parent) | ||
367 | { | ||
368 | fw_node_event(card, node, FW_NODE_DESTROYED); | ||
369 | fw_node_put(node); | ||
370 | |||
371 | /* Topology has changed - reset bus manager retry counter */ | ||
372 | card->bm_retries = 0; | ||
373 | } | ||
374 | |||
375 | static void report_found_node(struct fw_card *card, | ||
376 | struct fw_node *node, struct fw_node *parent) | ||
377 | { | ||
378 | int b_path = (node->phy_speed == SCODE_BETA); | ||
379 | |||
380 | if (parent != NULL) { | ||
381 | /* min() macro doesn't work here with gcc 3.4 */ | ||
382 | node->max_speed = parent->max_speed < node->phy_speed ? | ||
383 | parent->max_speed : node->phy_speed; | ||
384 | node->b_path = parent->b_path && b_path; | ||
385 | } else { | ||
386 | node->max_speed = node->phy_speed; | ||
387 | node->b_path = b_path; | ||
388 | } | ||
389 | |||
390 | fw_node_event(card, node, FW_NODE_CREATED); | ||
391 | |||
392 | /* Topology has changed - reset bus manager retry counter */ | ||
393 | card->bm_retries = 0; | ||
394 | } | ||
395 | |||
396 | void fw_destroy_nodes(struct fw_card *card) | ||
397 | { | ||
398 | unsigned long flags; | ||
399 | |||
400 | spin_lock_irqsave(&card->lock, flags); | ||
401 | card->color++; | ||
402 | if (card->local_node != NULL) | ||
403 | for_each_fw_node(card, card->local_node, report_lost_node); | ||
404 | card->local_node = NULL; | ||
405 | spin_unlock_irqrestore(&card->lock, flags); | ||
406 | } | ||
407 | |||
408 | static void move_tree(struct fw_node *node0, struct fw_node *node1, int port) | ||
409 | { | ||
410 | struct fw_node *tree; | ||
411 | int i; | ||
412 | |||
413 | tree = node1->ports[port]; | ||
414 | node0->ports[port] = tree; | ||
415 | for (i = 0; i < tree->port_count; i++) { | ||
416 | if (tree->ports[i] == node1) { | ||
417 | tree->ports[i] = node0; | ||
418 | break; | ||
419 | } | ||
420 | } | ||
421 | } | ||
422 | |||
423 | /** | ||
424 | * update_tree - compare the old topology tree for card with the new | ||
425 | * one specified by root. Queue the nodes and mark them as either | ||
426 | * found, lost or updated. Update the nodes in the card topology tree | ||
427 | * as we go. | ||
428 | */ | ||
429 | static void update_tree(struct fw_card *card, struct fw_node *root) | ||
430 | { | ||
431 | struct list_head list0, list1; | ||
432 | struct fw_node *node0, *node1, *next1; | ||
433 | int i, event; | ||
434 | |||
435 | INIT_LIST_HEAD(&list0); | ||
436 | list_add_tail(&card->local_node->link, &list0); | ||
437 | INIT_LIST_HEAD(&list1); | ||
438 | list_add_tail(&root->link, &list1); | ||
439 | |||
440 | node0 = fw_node(list0.next); | ||
441 | node1 = fw_node(list1.next); | ||
442 | |||
443 | while (&node0->link != &list0) { | ||
444 | WARN_ON(node0->port_count != node1->port_count); | ||
445 | |||
446 | if (node0->link_on && !node1->link_on) | ||
447 | event = FW_NODE_LINK_OFF; | ||
448 | else if (!node0->link_on && node1->link_on) | ||
449 | event = FW_NODE_LINK_ON; | ||
450 | else if (node1->initiated_reset && node1->link_on) | ||
451 | event = FW_NODE_INITIATED_RESET; | ||
452 | else | ||
453 | event = FW_NODE_UPDATED; | ||
454 | |||
455 | node0->node_id = node1->node_id; | ||
456 | node0->color = card->color; | ||
457 | node0->link_on = node1->link_on; | ||
458 | node0->initiated_reset = node1->initiated_reset; | ||
459 | node0->max_hops = node1->max_hops; | ||
460 | node1->color = card->color; | ||
461 | fw_node_event(card, node0, event); | ||
462 | |||
463 | if (card->root_node == node1) | ||
464 | card->root_node = node0; | ||
465 | if (card->irm_node == node1) | ||
466 | card->irm_node = node0; | ||
467 | |||
468 | for (i = 0; i < node0->port_count; i++) { | ||
469 | if (node0->ports[i] && node1->ports[i]) { | ||
470 | /* | ||
471 | * This port didn't change, queue the | ||
472 | * connected node for further | ||
473 | * investigation. | ||
474 | */ | ||
475 | if (node0->ports[i]->color == card->color) | ||
476 | continue; | ||
477 | list_add_tail(&node0->ports[i]->link, &list0); | ||
478 | list_add_tail(&node1->ports[i]->link, &list1); | ||
479 | } else if (node0->ports[i]) { | ||
480 | /* | ||
481 | * The nodes connected here were | ||
482 | * unplugged; unref the lost nodes and | ||
483 | * queue FW_NODE_LOST callbacks for | ||
484 | * them. | ||
485 | */ | ||
486 | |||
487 | for_each_fw_node(card, node0->ports[i], | ||
488 | report_lost_node); | ||
489 | node0->ports[i] = NULL; | ||
490 | } else if (node1->ports[i]) { | ||
491 | /* | ||
492 | * One or more node were connected to | ||
493 | * this port. Move the new nodes into | ||
494 | * the tree and queue FW_NODE_CREATED | ||
495 | * callbacks for them. | ||
496 | */ | ||
497 | move_tree(node0, node1, i); | ||
498 | for_each_fw_node(card, node0->ports[i], | ||
499 | report_found_node); | ||
500 | } | ||
501 | } | ||
502 | |||
503 | node0 = fw_node(node0->link.next); | ||
504 | next1 = fw_node(node1->link.next); | ||
505 | fw_node_put(node1); | ||
506 | node1 = next1; | ||
507 | } | ||
508 | } | ||
509 | |||
510 | static void update_topology_map(struct fw_card *card, | ||
511 | u32 *self_ids, int self_id_count) | ||
512 | { | ||
513 | int node_count; | ||
514 | |||
515 | card->topology_map[1]++; | ||
516 | node_count = (card->root_node->node_id & 0x3f) + 1; | ||
517 | card->topology_map[2] = (node_count << 16) | self_id_count; | ||
518 | card->topology_map[0] = (self_id_count + 2) << 16; | ||
519 | memcpy(&card->topology_map[3], self_ids, self_id_count * 4); | ||
520 | fw_compute_block_crc(card->topology_map); | ||
521 | } | ||
522 | |||
523 | void fw_core_handle_bus_reset(struct fw_card *card, int node_id, int generation, | ||
524 | int self_id_count, u32 *self_ids) | ||
525 | { | ||
526 | struct fw_node *local_node; | ||
527 | unsigned long flags; | ||
528 | |||
529 | /* | ||
530 | * If the selfID buffer is not the immediate successor of the | ||
531 | * previously processed one, we cannot reliably compare the | ||
532 | * old and new topologies. | ||
533 | */ | ||
534 | if (!is_next_generation(generation, card->generation) && | ||
535 | card->local_node != NULL) { | ||
536 | fw_notify("skipped bus generations, destroying all nodes\n"); | ||
537 | fw_destroy_nodes(card); | ||
538 | card->bm_retries = 0; | ||
539 | } | ||
540 | |||
541 | spin_lock_irqsave(&card->lock, flags); | ||
542 | |||
543 | card->broadcast_channel_allocated = false; | ||
544 | card->node_id = node_id; | ||
545 | /* | ||
546 | * Update node_id before generation to prevent anybody from using | ||
547 | * a stale node_id together with a current generation. | ||
548 | */ | ||
549 | smp_wmb(); | ||
550 | card->generation = generation; | ||
551 | card->reset_jiffies = jiffies; | ||
552 | fw_schedule_bm_work(card, 0); | ||
553 | |||
554 | local_node = build_tree(card, self_ids, self_id_count); | ||
555 | |||
556 | update_topology_map(card, self_ids, self_id_count); | ||
557 | |||
558 | card->color++; | ||
559 | |||
560 | if (local_node == NULL) { | ||
561 | fw_error("topology build failed\n"); | ||
562 | /* FIXME: We need to issue a bus reset in this case. */ | ||
563 | } else if (card->local_node == NULL) { | ||
564 | card->local_node = local_node; | ||
565 | for_each_fw_node(card, local_node, report_found_node); | ||
566 | } else { | ||
567 | update_tree(card, local_node); | ||
568 | } | ||
569 | |||
570 | spin_unlock_irqrestore(&card->lock, flags); | ||
571 | } | ||
572 | EXPORT_SYMBOL(fw_core_handle_bus_reset); | ||