diff --git a/drivers/firewire/phy-packet-definitions.h b/drivers/firewire/phy-packet-definitions.h index 479bb3431afb..4ba8b18aa993 100644 --- a/drivers/firewire/phy-packet-definitions.h +++ b/drivers/firewire/phy-packet-definitions.h @@ -15,6 +15,8 @@ #define SELF_ID_EXTENDED_SEQUENCE_MASK 0x00700000 #define SELF_ID_EXTENDED_SEQUENCE_SHIFT 20 +#define SELF_ID_PORT_STATUS_MASK 0x3 + #define SELF_ID_SEQUENCE_MAXIMUM_QUADLET_COUNT 4 static inline bool phy_packet_self_id_get_extended(u32 quadlet) @@ -75,4 +77,45 @@ static inline const u32 *self_id_sequence_enumerator_next( return self_id_sequence; } +enum phy_packet_self_id_port_status { + PHY_PACKET_SELF_ID_PORT_STATUS_NONE = 0, + PHY_PACKET_SELF_ID_PORT_STATUS_NCONN = 1, + PHY_PACKET_SELF_ID_PORT_STATUS_PARENT = 2, + PHY_PACKET_SELF_ID_PORT_STATUS_CHILD = 3, +}; + +static inline unsigned int self_id_sequence_get_port_capacity(unsigned int quadlet_count) +{ + return quadlet_count * 8 - 5; +} + +static inline enum phy_packet_self_id_port_status self_id_sequence_get_port_status( + const u32 *self_id_sequence, unsigned int quadlet_count, unsigned int port_index) +{ + unsigned int index, shift; + + index = (port_index + 5) / 8; + shift = 16 - ((port_index + 5) % 8) * 2; + + if (index < quadlet_count && index < SELF_ID_SEQUENCE_MAXIMUM_QUADLET_COUNT) + return (self_id_sequence[index] >> shift) & SELF_ID_PORT_STATUS_MASK; + + return PHY_PACKET_SELF_ID_PORT_STATUS_NONE; +} + +static inline void self_id_sequence_set_port_status(u32 *self_id_sequence, unsigned int quadlet_count, + unsigned int port_index, + enum phy_packet_self_id_port_status status) +{ + unsigned int index, shift; + + index = (port_index + 5) / 8; + shift = 16 - ((port_index + 5) % 8) * 2; + + if (index < quadlet_count) { + self_id_sequence[index] &= ~(SELF_ID_PORT_STATUS_MASK << shift); + self_id_sequence[index] |= status << shift; + } +} + #endif // _FIREWIRE_PHY_PACKET_DEFINITIONS_H diff --git a/drivers/firewire/self-id-sequence-helper-test.c b/drivers/firewire/self-id-sequence-helper-test.c index e8a435e20241..eed7a2294e64 100644 --- a/drivers/firewire/self-id-sequence-helper-test.c +++ b/drivers/firewire/self-id-sequence-helper-test.c @@ -63,9 +63,82 @@ static void test_self_id_sequence_enumerator_invalid(struct kunit *test) KUNIT_EXPECT_EQ(test, PTR_ERR(entry), -EPROTO); } +static void test_self_id_sequence_get_port_status(struct kunit *test) +{ + static const u32 expected[] = { + 0x000000e5, + 0x00839e79, + 0x0091e79d, + 0x00a279e4, + }; + u32 quadlets [] = { + 0x00000001, + 0x00800001, + 0x00900001, + 0x00a00000, + }; + enum phy_packet_self_id_port_status port_status[28]; + unsigned int port_capacity; + unsigned int port_index; + + KUNIT_ASSERT_EQ(test, ARRAY_SIZE(expected), ARRAY_SIZE(quadlets)); + + // With an extra port. + port_capacity = self_id_sequence_get_port_capacity(ARRAY_SIZE(expected)) + 1; + KUNIT_ASSERT_EQ(test, port_capacity, ARRAY_SIZE(port_status)); + + for (port_index = 0; port_index < port_capacity; ++port_index) { + port_status[port_index] = + self_id_sequence_get_port_status(expected, ARRAY_SIZE(expected), port_index); + self_id_sequence_set_port_status(quadlets, ARRAY_SIZE(quadlets), port_index, + port_status[port_index]); + } + + // Self ID zero. + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[0]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[1]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[2]); + + // Self ID one. + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[3]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[4]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[5]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[6]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[7]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[8]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[9]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[10]); + + // Self ID two. + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[11]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[12]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[13]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[14]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[15]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[16]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[17]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[18]); + + // Self ID three. + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[19]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[20]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[21]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[22]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[23]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_CHILD, port_status[24]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_PARENT, port_status[25]); + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NCONN, port_status[26]); + + // Our of order. + KUNIT_EXPECT_EQ(test, PHY_PACKET_SELF_ID_PORT_STATUS_NONE, port_status[27]); + + KUNIT_EXPECT_MEMEQ(test, quadlets, expected, sizeof(expected)); +} + static struct kunit_case self_id_sequence_helper_test_cases[] = { KUNIT_CASE(test_self_id_sequence_enumerator_valid), KUNIT_CASE(test_self_id_sequence_enumerator_invalid), + KUNIT_CASE(test_self_id_sequence_get_port_status), {} };