Commit 24b7f8e5 authored by Takashi Sakamoto's avatar Takashi Sakamoto
Browse files

firewire: core: use helper functions for self ID sequence 

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: default avatarTakashi Sakamoto <o-takashi@sakamocchi.jp>
parent e404cacf

drivers/firewire/core-topology.c

+69 −120
Original line number Diff line number Diff line
@@ -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");

		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;

		}



		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;

			}

		}



		q = *sid;

		if (phy_id != SELF_ID_PHY_ID(q)) {

		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++;

	}