Skip to content

Commit

Permalink
firewire: core: use helper functions for self ID sequence
Browse files Browse the repository at this point in the history 
This commit replaces the existing implementation with the helper
functions for self ID sequence.

Link: https://lore.kernel.org/r/[email protected]
Signed-off-by: Takashi Sakamoto <[email protected]>
  • Loading branch information
@takaswie
takaswie committed Jun 16, 2024
1 parent e404cac commit 24b7f8e
Showing 1 changed file with 69 additions and 120 deletions.
189 changes: 69 additions & 120 deletions drivers/firewire/core-topology.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,80 +20,15 @@
#include <asm/byteorder.h>

#include "core.h"
#include "phy-packet-definitions.h"
#include <trace/events/firewire.h>

#define SELF_ID_PHY_ID(q) (((q) >> 24) & 0x3f)
#define SELF_ID_EXTENDED(q) (((q) >> 23) & 0x01)
#define SELF_ID_LINK_ON(q) (((q) >> 22) & 0x01)
#define SELF_ID_GAP_COUNT(q) (((q) >> 16) & 0x3f)
#define SELF_ID_PHY_SPEED(q) (((q) >> 14) & 0x03)
#define SELF_ID_CONTENDER(q) (((q) >> 11) & 0x01)
#define SELF_ID_PHY_INITIATOR(q) (((q) >> 1) & 0x01)
#define SELF_ID_MORE_PACKETS(q) (((q) >> 0) & 0x01)

#define SELF_ID_EXT_SEQUENCE(q) (((q) >> 20) & 0x07)

#define SELFID_PORT_CHILD 0x3
#define SELFID_PORT_PARENT 0x2
#define SELFID_PORT_NCONN 0x1
#define SELFID_PORT_NONE 0x0

static const u32 *count_ports(const u32 *sid, int *total_port_count, int *child_port_count)
{
u32 q;
int port_type, shift, seq;

shift = 6;
q = *sid;
seq = 0;

while (1) {
port_type = (q >> shift) & 0x03;
switch (port_type) {
case SELFID_PORT_CHILD:
(*child_port_count)++;
fallthrough;
case SELFID_PORT_PARENT:
case SELFID_PORT_NCONN:
(*total_port_count)++;
fallthrough;
case SELFID_PORT_NONE:
break;
}

shift -= 2;
if (shift == 0) {
if (!SELF_ID_MORE_PACKETS(q))
return sid + 1;

shift = 16;
sid++;
q = *sid;

/*
* Check that the extra packets actually are
* extended self ID packets and that the
* sequence numbers in the extended self ID
* packets increase as expected.
*/

if (!SELF_ID_EXTENDED(q) ||
seq != SELF_ID_EXT_SEQUENCE(q))
return NULL;

seq++;
}
}
}

static int get_port_type(const u32 *sid, int port_index)
{
int index, shift;

index = (port_index + 5) / 8;
shift = 16 - ((port_index + 5) & 7) * 2;
return (sid[index] >> shift) & 0x03;
}

static struct fw_node *fw_node_create(u32 sid, int port_count, int color)
{
Expand Down Expand Up @@ -168,9 +103,12 @@ static inline struct fw_node *fw_node(struct list_head *l)
*/
static struct fw_node *build_tree(struct fw_card *card, const u32 *sid, int self_id_count)
{
struct self_id_sequence_enumerator enumerator = {
.cursor = sid,
.quadlet_count = self_id_count,
};
struct fw_node *node, *child, *local_node, *irm_node;
struct list_head stack;
const u32 *end;
int phy_id, stack_depth;
int gap_count;
bool beta_repeaters_present;
Expand All @@ -179,31 +117,54 @@ static struct fw_node *build_tree(struct fw_card *card, const u32 *sid, int self
node = NULL;
INIT_LIST_HEAD(&stack);
stack_depth = 0;
end = sid + self_id_count;
phy_id = 0;
irm_node = NULL;
gap_count = SELF_ID_GAP_COUNT(*sid);
beta_repeaters_present = false;

while (sid < end) {
int port_count = 0;
int child_port_count = 0;
int parent_count = 0;
const u32 *next_sid;
u32 q;
while (enumerator.quadlet_count > 0) {
unsigned int child_port_count = 0;
unsigned int total_port_count = 0;
unsigned int parent_count = 0;
unsigned int quadlet_count;
const u32 *self_id_sequence;
unsigned int port_capacity;
enum phy_packet_self_id_port_status port_status;
unsigned int port_index;
struct list_head *h;
int i;

next_sid = count_ports(sid, &port_count, &child_port_count);
if (next_sid == NULL) {
fw_err(card, "inconsistent extended self IDs\n");
return NULL;
self_id_sequence = self_id_sequence_enumerator_next(&enumerator, &quadlet_count);
if (IS_ERR(self_id_sequence)) {
if (PTR_ERR(self_id_sequence) != -ENODATA) {
fw_err(card, "inconsistent extended self IDs: %ld\n",
PTR_ERR(self_id_sequence));
return NULL;
}
break;
}

q = *sid;
if (phy_id != SELF_ID_PHY_ID(q)) {
port_capacity = self_id_sequence_get_port_capacity(quadlet_count);
for (port_index = 0; port_index < port_capacity; ++port_index) {
port_status = self_id_sequence_get_port_status(self_id_sequence, quadlet_count,
port_index);
switch (port_status) {
case PHY_PACKET_SELF_ID_PORT_STATUS_CHILD:
++child_port_count;
fallthrough;
case PHY_PACKET_SELF_ID_PORT_STATUS_PARENT:
case PHY_PACKET_SELF_ID_PORT_STATUS_NCONN:
++total_port_count;
fallthrough;
case PHY_PACKET_SELF_ID_PORT_STATUS_NONE:
default:
break;
}
}

if (phy_id != SELF_ID_PHY_ID(self_id_sequence[0])) {
fw_err(card, "PHY ID mismatch in self ID: %d != %d\n",
phy_id, SELF_ID_PHY_ID(q));
phy_id, SELF_ID_PHY_ID(self_id_sequence[0]));
return NULL;
}

Expand All @@ -224,7 +185,7 @@ static struct fw_node *build_tree(struct fw_card *card, const u32 *sid, int self
*/
child = fw_node(h);

node = fw_node_create(q, port_count, card->color);
node = fw_node_create(self_id_sequence[0], total_port_count, card->color);
if (node == NULL) {
fw_err(card, "out of memory while building topology\n");
return NULL;
Expand All @@ -233,48 +194,40 @@ static struct fw_node *build_tree(struct fw_card *card, const u32 *sid, int self
if (phy_id == (card->node_id & 0x3f))
local_node = node;

if (SELF_ID_CONTENDER(q))
if (SELF_ID_CONTENDER(self_id_sequence[0]))
irm_node = node;

parent_count = 0;

for (i = 0; i < port_count; i++) {
switch (get_port_type(sid, i)) {
case SELFID_PORT_PARENT:
/*
* Who's your daddy? We dont know the
* parent node at this time, so we
* temporarily abuse node->color for
* remembering the entry in the
* node->ports array where the parent
* node should be. Later, when we
* handle the parent node, we fix up
* the reference.
*/
parent_count++;
for (port_index = 0; port_index < total_port_count; ++port_index) {
port_status = self_id_sequence_get_port_status(self_id_sequence, quadlet_count,
port_index);
switch (port_status) {
case PHY_PACKET_SELF_ID_PORT_STATUS_PARENT:
// Who's your daddy? We dont know the parent node at this time, so
// we temporarily abuse node->color for remembering the entry in
// the node->ports array where the parent node should be. Later,
// when we handle the parent node, we fix up the reference.
++parent_count;
node->color = i;
break;

case SELFID_PORT_CHILD:
node->ports[i] = child;
/*
* Fix up parent reference for this
* child node.
*/
case PHY_PACKET_SELF_ID_PORT_STATUS_CHILD:
node->ports[port_index] = child;
// Fix up parent reference for this child node.
child->ports[child->color] = node;
child->color = card->color;
child = fw_node(child->link.next);
break;
case PHY_PACKET_SELF_ID_PORT_STATUS_NCONN:
case PHY_PACKET_SELF_ID_PORT_STATUS_NONE:
default:
break;
}
}

/*
* Check that the node reports exactly one parent
* port, except for the root, which of course should
* have no parents.
*/
if ((next_sid == end && parent_count != 0) ||
(next_sid < end && parent_count != 1)) {
// Check that the node reports exactly one parent port, except for the root, which
// of course should have no parents.
if ((enumerator.quadlet_count == 0 && parent_count != 0) ||
(enumerator.quadlet_count > 0 && parent_count != 1)) {
fw_err(card, "parent port inconsistency for node %d: "
"parent_count=%d\n", phy_id, parent_count);
return NULL;
Expand All @@ -285,20 +238,16 @@ static struct fw_node *build_tree(struct fw_card *card, const u32 *sid, int self
list_add_tail(&node->link, &stack);
stack_depth += 1 - child_port_count;

if (node->phy_speed == SCODE_BETA &&
parent_count + child_port_count > 1)
if (node->phy_speed == SCODE_BETA && parent_count + child_port_count > 1)
beta_repeaters_present = true;

/*
* If PHYs report different gap counts, set an invalid count
* which will force a gap count reconfiguration and a reset.
*/
if (SELF_ID_GAP_COUNT(q) != gap_count)
// If PHYs report different gap counts, set an invalid count which will force a gap
// count reconfiguration and a reset.
if (SELF_ID_GAP_COUNT(self_id_sequence[0]) != gap_count)
gap_count = 0;

update_hop_count(node);

sid = next_sid;
phy_id++;
}

Expand Down

0 comments on commit 24b7f8e

Please sign in to comment.