kevin/linux
1
Fork 0
Code

firewire: core: use helper functions for self ID sequence

Browse Source 
This commit replaces the existing implementation with the helper
functions for self ID sequence.

Link: https://lore.kernel.org/r/20240605235155.116468-6-o-takashi@sakamocchi.jp
Signed-off-by: Takashi Sakamoto <o-takashi@sakamocchi.jp>
This commit is contained in:
Takashi Sakamoto 2024-06-06 08:51:49 +09:00
parent e404cacfc5
commit 24b7f8e5cd
1 changed files with 69 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

189
drivers/firewire/core-topology.c
View File

@ -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)
{
@ -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;
@ -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;
}
@ -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;
@ -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;
@ -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++;
}