1 files changed, 290 insertions, 55 deletions

diff --git a/drivers/net/phy/sfp.c b/drivers/net/phy/sfp.c
index 89636dc71e48..d855a18308d7 100644
--- a/drivers/net/phy/sfp.c
+++ b/drivers/net/phy/sfp.c
@@ -24,14 +24,18 @@ enum {
 	GPIO_LOS,
 	GPIO_TX_FAULT,
 	GPIO_TX_DISABLE,
-	GPIO_RATE_SELECT,
+	GPIO_RS0,
+	GPIO_RS1,
 	GPIO_MAX,
 
 	SFP_F_PRESENT = BIT(GPIO_MODDEF0),
 	SFP_F_LOS = BIT(GPIO_LOS),
 	SFP_F_TX_FAULT = BIT(GPIO_TX_FAULT),
 	SFP_F_TX_DISABLE = BIT(GPIO_TX_DISABLE),
-	SFP_F_RATE_SELECT = BIT(GPIO_RATE_SELECT),
+	SFP_F_RS0 = BIT(GPIO_RS0),
+	SFP_F_RS1 = BIT(GPIO_RS1),
+
+	SFP_F_OUTPUTS = SFP_F_TX_DISABLE | SFP_F_RS0 | SFP_F_RS1,
 
 	SFP_E_INSERT = 0,
 	SFP_E_REMOVE,
@@ -148,6 +152,7 @@ static const char *gpio_names[] = {
 	"tx-fault",
 	"tx-disable",
 	"rate-select0",
+	"rate-select1",
 };
 
 static const enum gpiod_flags gpio_flags[] = {
@@ -156,6 +161,7 @@ static const enum gpiod_flags gpio_flags[] = {
 	GPIOD_IN,
 	GPIOD_ASIS,
 	GPIOD_ASIS,
+	GPIOD_ASIS,
 };
 
 /* t_start_up (SFF-8431) or t_init (SFF-8472) is the time required for a
@@ -164,7 +170,6 @@ static const enum gpiod_flags gpio_flags[] = {
  * on board (for a copper SFP) time to initialise.
  */
 #define T_WAIT			msecs_to_jiffies(50)
-#define T_WAIT_ROLLBALL		msecs_to_jiffies(25000)
 #define T_START_UP		msecs_to_jiffies(300)
 #define T_START_UP_BAD_GPON	msecs_to_jiffies(60000)
 
@@ -242,10 +247,18 @@ struct sfp {
 
 	bool need_poll;
 
+	/* Access rules:
+	 * state_hw_drive: st_mutex held
+	 * state_hw_mask: st_mutex held
+	 * state_soft_mask: st_mutex held
+	 * state: st_mutex held unless reading input bits
+	 */
 	struct mutex st_mutex;			/* Protects state */
+	unsigned int state_hw_drive;
 	unsigned int state_hw_mask;
 	unsigned int state_soft_mask;
 	unsigned int state;
+
 	struct delayed_work poll;
 	struct delayed_work timeout;
 	struct mutex sm_mutex;			/* Protects state machine */
@@ -262,6 +275,10 @@ struct sfp {
 	unsigned int module_t_start_up;
 	unsigned int module_t_wait;
 
+	unsigned int rate_kbd;
+	unsigned int rs_threshold_kbd;
+	unsigned int rs_state_mask;
+
 	bool have_a2;
 	bool tx_fault_ignore;
 
@@ -312,7 +329,7 @@ static bool sfp_module_supported(const struct sfp_eeprom_id *id)
 
 static const struct sff_data sfp_data = {
 	.gpios = SFP_F_PRESENT | SFP_F_LOS | SFP_F_TX_FAULT |
-		 SFP_F_TX_DISABLE | SFP_F_RATE_SELECT,
+		 SFP_F_TX_DISABLE | SFP_F_RS0 | SFP_F_RS1,
 	.module_supported = sfp_module_supported,
 };
 
@@ -333,6 +350,27 @@ static void sfp_fixup_ignore_tx_fault(struct sfp *sfp)
 	sfp->tx_fault_ignore = true;
 }
 
+// For 10GBASE-T short-reach modules
+static void sfp_fixup_10gbaset_30m(struct sfp *sfp)
+{
+	sfp->id.base.connector = SFF8024_CONNECTOR_RJ45;
+	sfp->id.base.extended_cc = SFF8024_ECC_10GBASE_T_SR;
+}
+
+static void sfp_fixup_rollball_proto(struct sfp *sfp, unsigned int secs)
+{
+	sfp->mdio_protocol = MDIO_I2C_ROLLBALL;
+	sfp->module_t_wait = msecs_to_jiffies(secs * 1000);
+}
+
+static void sfp_fixup_fs_10gt(struct sfp *sfp)
+{
+	sfp_fixup_10gbaset_30m(sfp);
+
+	// These SFPs need 4 seconds before the PHY can be accessed
+	sfp_fixup_rollball_proto(sfp, 4);
+}
+
 static void sfp_fixup_halny_gsfp(struct sfp *sfp)
 {
 	/* Ignore the TX_FAULT and LOS signals on this module.
@@ -344,8 +382,8 @@ static void sfp_fixup_halny_gsfp(struct sfp *sfp)
 
 static void sfp_fixup_rollball(struct sfp *sfp)
 {
-	sfp->mdio_protocol = MDIO_I2C_ROLLBALL;
-	sfp->module_t_wait = T_WAIT_ROLLBALL;
+	// Rollball SFPs need 25 seconds before the PHY can be accessed
+	sfp_fixup_rollball_proto(sfp, 25);
 }
 
 static void sfp_fixup_rollball_cc(struct sfp *sfp)
@@ -410,6 +448,10 @@ static const struct sfp_quirk sfp_quirks[] = {
 	SFP_QUIRK("ALCATELLUCENT", "3FE46541AA", sfp_quirk_2500basex,
 		  sfp_fixup_long_startup),
 
+	// Fiberstore SFP-10G-T doesn't identify as copper, and uses the
+	// Rollball protocol to talk to the PHY.
+	SFP_QUIRK_F("FS", "SFP-10G-T", sfp_fixup_fs_10gt),
+
 	SFP_QUIRK_F("HALNy", "HL-GSFP", sfp_fixup_halny_gsfp),
 
 	// HG MXPD-483II-F 2.5G supports 2500Base-X, but incorrectly reports
@@ -427,6 +469,11 @@ static const struct sfp_quirk sfp_quirks[] = {
 
 	SFP_QUIRK_M("UBNT", "UF-INSTANT", sfp_quirk_ubnt_uf_instant),
 
+	// Walsun HXSX-ATR[CI]-1 don't identify as copper, and use the
+	// Rollball protocol to talk to the PHY.
+	SFP_QUIRK_F("Walsun", "HXSX-ATRC-1", sfp_fixup_fs_10gt),
+	SFP_QUIRK_F("Walsun", "HXSX-ATRI-1", sfp_fixup_fs_10gt),
+
 	SFP_QUIRK_F("OEM", "SFP-10G-T", sfp_fixup_rollball_cc),
 	SFP_QUIRK_M("OEM", "SFP-2.5G-T", sfp_quirk_oem_2_5g),
 	SFP_QUIRK_F("OEM", "RTSFP-10", sfp_fixup_rollball_cc),
@@ -500,20 +547,37 @@ static unsigned int sff_gpio_get_state(struct sfp *sfp)
 
 static void sfp_gpio_set_state(struct sfp *sfp, unsigned int state)
 {
-	if (state & SFP_F_PRESENT) {
-		/* If the module is present, drive the signals */
-		if (sfp->gpio[GPIO_TX_DISABLE])
+	unsigned int drive;
+
+	if (state & SFP_F_PRESENT)
+		/* If the module is present, drive the requested signals */
+		drive = sfp->state_hw_drive;
+	else
+		/* Otherwise, let them float to the pull-ups */
+		drive = 0;
+
+	if (sfp->gpio[GPIO_TX_DISABLE]) {
+		if (drive & SFP_F_TX_DISABLE)
 			gpiod_direction_output(sfp->gpio[GPIO_TX_DISABLE],
 					       state & SFP_F_TX_DISABLE);
-		if (state & SFP_F_RATE_SELECT)
-			gpiod_direction_output(sfp->gpio[GPIO_RATE_SELECT],
-					       state & SFP_F_RATE_SELECT);
-	} else {
-		/* Otherwise, let them float to the pull-ups */
-		if (sfp->gpio[GPIO_TX_DISABLE])
+		else
 			gpiod_direction_input(sfp->gpio[GPIO_TX_DISABLE]);
-		if (state & SFP_F_RATE_SELECT)
-			gpiod_direction_input(sfp->gpio[GPIO_RATE_SELECT]);
+	}
+
+	if (sfp->gpio[GPIO_RS0]) {
+		if (drive & SFP_F_RS0)
+			gpiod_direction_output(sfp->gpio[GPIO_RS0],
+					       state & SFP_F_RS0);
+		else
+			gpiod_direction_input(sfp->gpio[GPIO_RS0]);
+	}
+
+	if (sfp->gpio[GPIO_RS1]) {
+		if (drive & SFP_F_RS1)
+			gpiod_direction_output(sfp->gpio[GPIO_RS1],
+					       state & SFP_F_RS1);
+		else
+			gpiod_direction_input(sfp->gpio[GPIO_RS1]);
 	}
 }
 
@@ -675,16 +739,33 @@ static unsigned int sfp_soft_get_state(struct sfp *sfp)
 	return state & sfp->state_soft_mask;
 }
 
-static void sfp_soft_set_state(struct sfp *sfp, unsigned int state)
+static void sfp_soft_set_state(struct sfp *sfp, unsigned int state,
+			       unsigned int soft)
 {
-	u8 mask = SFP_STATUS_TX_DISABLE_FORCE;
+	u8 mask = 0;
 	u8 val = 0;
 
+	if (soft & SFP_F_TX_DISABLE)
+		mask |= SFP_STATUS_TX_DISABLE_FORCE;
 	if (state & SFP_F_TX_DISABLE)
 		val |= SFP_STATUS_TX_DISABLE_FORCE;
 
+	if (soft & SFP_F_RS0)
+		mask |= SFP_STATUS_RS0_SELECT;
+	if (state & SFP_F_RS0)
+		val |= SFP_STATUS_RS0_SELECT;
+
+	if (mask)
+		sfp_modify_u8(sfp, true, SFP_STATUS, mask, val);
 
-	sfp_modify_u8(sfp, true, SFP_STATUS, mask, val);
+	val = mask = 0;
+	if (soft & SFP_F_RS1)
+		mask |= SFP_EXT_STATUS_RS1_SELECT;
+	if (state & SFP_F_RS1)
+		val |= SFP_EXT_STATUS_RS1_SELECT;
+
+	if (mask)
+		sfp_modify_u8(sfp, true, SFP_EXT_STATUS, mask, val);
 }
 
 static void sfp_soft_start_poll(struct sfp *sfp)
@@ -692,27 +773,35 @@ static void sfp_soft_start_poll(struct sfp *sfp)
 	const struct sfp_eeprom_id *id = &sfp->id;
 	unsigned int mask = 0;
 
-	sfp->state_soft_mask = 0;
 	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_DISABLE)
 		mask |= SFP_F_TX_DISABLE;
 	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_FAULT)
 		mask |= SFP_F_TX_FAULT;
 	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_RX_LOS)
 		mask |= SFP_F_LOS;
+	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_RATE_SELECT)
+		mask |= sfp->rs_state_mask;
 
+	mutex_lock(&sfp->st_mutex);
 	// Poll the soft state for hardware pins we want to ignore
 	sfp->state_soft_mask = ~sfp->state_hw_mask & mask;
 
 	if (sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT) &&
 	    !sfp->need_poll)
 		mod_delayed_work(system_wq, &sfp->poll, poll_jiffies);
+	mutex_unlock(&sfp->st_mutex);
 }
 
 static void sfp_soft_stop_poll(struct sfp *sfp)
 {
+	mutex_lock(&sfp->st_mutex);
 	sfp->state_soft_mask = 0;
+	mutex_unlock(&sfp->st_mutex);
 }
 
+/* sfp_get_state() - must be called with st_mutex held, or in the
+ * initialisation path.
+ */
 static unsigned int sfp_get_state(struct sfp *sfp)
 {
 	unsigned int soft = sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT);
@@ -725,13 +814,26 @@ static unsigned int sfp_get_state(struct sfp *sfp)
 	return state;
 }
 
+/* sfp_set_state() - must be called with st_mutex held, or in the
+ * initialisation path.
+ */
 static void sfp_set_state(struct sfp *sfp, unsigned int state)
 {
+	unsigned int soft;
+
 	sfp->set_state(sfp, state);
 
-	if (state & SFP_F_PRESENT &&
-	    sfp->state_soft_mask & SFP_F_TX_DISABLE)
-		sfp_soft_set_state(sfp, state);
+	soft = sfp->state_soft_mask & SFP_F_OUTPUTS;
+	if (state & SFP_F_PRESENT && soft)
+		sfp_soft_set_state(sfp, state, soft);
+}
+
+static void sfp_mod_state(struct sfp *sfp, unsigned int mask, unsigned int set)
+{
+	mutex_lock(&sfp->st_mutex);
+	sfp->state = (sfp->state & ~mask) | set;
+	sfp_set_state(sfp, sfp->state);
+	mutex_unlock(&sfp->st_mutex);
 }
 
 static unsigned int sfp_check(void *buf, size_t len)
@@ -1537,16 +1639,14 @@ static void sfp_module_tx_disable(struct sfp *sfp)
 {
 	dev_dbg(sfp->dev, "tx disable %u -> %u\n",
 		sfp->state & SFP_F_TX_DISABLE ? 1 : 0, 1);
-	sfp->state |= SFP_F_TX_DISABLE;
-	sfp_set_state(sfp, sfp->state);
+	sfp_mod_state(sfp, SFP_F_TX_DISABLE, SFP_F_TX_DISABLE);
 }
 
 static void sfp_module_tx_enable(struct sfp *sfp)
 {
 	dev_dbg(sfp->dev, "tx disable %u -> %u\n",
 		sfp->state & SFP_F_TX_DISABLE ? 1 : 0, 0);
-	sfp->state &= ~SFP_F_TX_DISABLE;
-	sfp_set_state(sfp, sfp->state);
+	sfp_mod_state(sfp, SFP_F_TX_DISABLE, 0);
 }
 
 #if IS_ENABLED(CONFIG_DEBUG_FS)
@@ -1567,10 +1667,15 @@ static int sfp_debug_state_show(struct seq_file *s, void *data)
 		   sfp->sm_fault_retries);
 	seq_printf(s, "PHY probe remaining retries: %d\n",
 		   sfp->sm_phy_retries);
+	seq_printf(s, "Signalling rate: %u kBd\n", sfp->rate_kbd);
+	seq_printf(s, "Rate select threshold: %u kBd\n",
+		   sfp->rs_threshold_kbd);
 	seq_printf(s, "moddef0: %d\n", !!(sfp->state & SFP_F_PRESENT));
 	seq_printf(s, "rx_los: %d\n", !!(sfp->state & SFP_F_LOS));
 	seq_printf(s, "tx_fault: %d\n", !!(sfp->state & SFP_F_TX_FAULT));
 	seq_printf(s, "tx_disable: %d\n", !!(sfp->state & SFP_F_TX_DISABLE));
+	seq_printf(s, "rs0: %d\n", !!(sfp->state & SFP_F_RS0));
+	seq_printf(s, "rs1: %d\n", !!(sfp->state & SFP_F_RS1));
 	return 0;
 }
 DEFINE_SHOW_ATTRIBUTE(sfp_debug_state);
@@ -1599,16 +1704,18 @@ static void sfp_debugfs_exit(struct sfp *sfp)
 
 static void sfp_module_tx_fault_reset(struct sfp *sfp)
 {
-	unsigned int state = sfp->state;
-
-	if (state & SFP_F_TX_DISABLE)
-		return;
+	unsigned int state;
 
-	sfp_set_state(sfp, state | SFP_F_TX_DISABLE);
+	mutex_lock(&sfp->st_mutex);
+	state = sfp->state;
+	if (!(state & SFP_F_TX_DISABLE)) {
+		sfp_set_state(sfp, state | SFP_F_TX_DISABLE);
 
-	udelay(T_RESET_US);
+		udelay(T_RESET_US);
 
-	sfp_set_state(sfp, state);
+		sfp_set_state(sfp, state);
+	}
+	mutex_unlock(&sfp->st_mutex);
 }
 
 /* SFP state machine */
@@ -1874,6 +1981,95 @@ static int sfp_sm_mod_hpower(struct sfp *sfp, bool enable)
 	return 0;
 }
 
+static void sfp_module_parse_rate_select(struct sfp *sfp)
+{
+	u8 rate_id;
+
+	sfp->rs_threshold_kbd = 0;
+	sfp->rs_state_mask = 0;
+
+	if (!(sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_RATE_SELECT)))
+		/* No support for RateSelect */
+		return;
+
+	/* Default to INF-8074 RateSelect operation. The signalling threshold
+	 * rate is not well specified, so always select "Full Bandwidth", but
+	 * SFF-8079 reveals that it is understood that RS0 will be low for
+	 * 1.0625Gb/s and high for 2.125Gb/s. Choose a value half-way between.
+	 * This method exists prior to SFF-8472.
+	 */
+	sfp->rs_state_mask = SFP_F_RS0;
+	sfp->rs_threshold_kbd = 1594;
+
+	/* Parse the rate identifier, which is complicated due to history:
+	 * SFF-8472 rev 9.5 marks this field as reserved.
+	 * SFF-8079 references SFF-8472 rev 9.5 and defines bit 0. SFF-8472
+	 *  compliance is not required.
+	 * SFF-8472 rev 10.2 defines this field using values 0..4
+	 * SFF-8472 rev 11.0 redefines this field with bit 0 for SFF-8079
+	 * and even values.
+	 */
+	rate_id = sfp->id.base.rate_id;
+	if (rate_id == 0)
+		/* Unspecified */
+		return;
+
+	/* SFF-8472 rev 10.0..10.4 did not account for SFF-8079 using bit 0,
+	 * and allocated value 3 to SFF-8431 independent tx/rx rate select.
+	 * Convert this to a SFF-8472 rev 11.0 rate identifier.
+	 */
+	if (sfp->id.ext.sff8472_compliance >= SFP_SFF8472_COMPLIANCE_REV10_2 &&
+	    sfp->id.ext.sff8472_compliance < SFP_SFF8472_COMPLIANCE_REV11_0 &&
+	    rate_id == 3)
+		rate_id = SFF_RID_8431;
+
+	if (rate_id & SFF_RID_8079) {
+		/* SFF-8079 RateSelect / Application Select in conjunction with
+		 * SFF-8472 rev 9.5. SFF-8079 defines rate_id as a bitfield
+		 * with only bit 0 used, which takes precedence over SFF-8472.
+		 */
+		if (!(sfp->id.ext.enhopts & SFP_ENHOPTS_APP_SELECT_SFF8079)) {
+			/* SFF-8079 Part 1 - rate selection between Fibre
+			 * Channel 1.0625/2.125/4.25 Gbd modes. Note that RS0
+			 * is high for 2125, so we have to subtract 1 to
+			 * include it.
+			 */
+			sfp->rs_threshold_kbd = 2125 - 1;
+			sfp->rs_state_mask = SFP_F_RS0;
+		}
+		return;
+	}
+
+	/* SFF-8472 rev 9.5 does not define the rate identifier */
+	if (sfp->id.ext.sff8472_compliance <= SFP_SFF8472_COMPLIANCE_REV9_5)
+		return;
+
+	/* SFF-8472 rev 11.0 defines rate_id as a numerical value which will
+	 * always have bit 0 clear due to SFF-8079's bitfield usage of rate_id.
+	 */
+	switch (rate_id) {
+	case SFF_RID_8431_RX_ONLY:
+		sfp->rs_threshold_kbd = 4250;
+		sfp->rs_state_mask = SFP_F_RS0;
+		break;
+
+	case SFF_RID_8431_TX_ONLY:
+		sfp->rs_threshold_kbd = 4250;
+		sfp->rs_state_mask = SFP_F_RS1;
+		break;
+
+	case SFF_RID_8431:
+		sfp->rs_threshold_kbd = 4250;
+		sfp->rs_state_mask = SFP_F_RS0 | SFP_F_RS1;
+		break;
+
+	case SFF_RID_10G8G:
+		sfp->rs_threshold_kbd = 9000;
+		sfp->rs_state_mask = SFP_F_RS0 | SFP_F_RS1;
+		break;
+	}
+}
+
 /* GPON modules based on Realtek RTL8672 and RTL9601C chips (e.g. V-SOL
  * V2801F, CarlitoxxPro CPGOS03-0490, Ubiquiti U-Fiber Instant, ...) do
  * not support multibyte reads from the EEPROM. Each multi-byte read
@@ -1953,6 +2149,7 @@ static int sfp_sm_mod_probe(struct sfp *sfp, bool report)
 	/* SFP module inserted - read I2C data */
 	struct sfp_eeprom_id id;
 	bool cotsworks_sfbg;
+	unsigned int mask;
 	bool cotsworks;
 	u8 check;
 	int ret;
@@ -2092,14 +2289,19 @@ static int sfp_sm_mod_probe(struct sfp *sfp, bool report)
 	if (ret < 0)
 		return ret;
 
-	/* Initialise state bits to use from hardware */
-	sfp->state_hw_mask = SFP_F_PRESENT;
+	sfp_module_parse_rate_select(sfp);
+
+	mask = SFP_F_PRESENT;
 	if (sfp->gpio[GPIO_TX_DISABLE])
-		sfp->state_hw_mask |= SFP_F_TX_DISABLE;
+		mask |= SFP_F_TX_DISABLE;
 	if (sfp->gpio[GPIO_TX_FAULT])
-		sfp->state_hw_mask |= SFP_F_TX_FAULT;
+		mask |= SFP_F_TX_FAULT;
 	if (sfp->gpio[GPIO_LOS])
-		sfp->state_hw_mask |= SFP_F_LOS;
+		mask |= SFP_F_LOS;
+	if (sfp->gpio[GPIO_RS0])
+		mask |= SFP_F_RS0;
+	if (sfp->gpio[GPIO_RS1])
+		mask |= SFP_F_RS1;
 
 	sfp->module_t_start_up = T_START_UP;
 	sfp->module_t_wait = T_WAIT;
@@ -2117,8 +2319,17 @@ static int sfp_sm_mod_probe(struct sfp *sfp, bool report)
 		sfp->mdio_protocol = MDIO_I2C_NONE;
 
 	sfp->quirk = sfp_lookup_quirk(&id);
+
+	mutex_lock(&sfp->st_mutex);
+	/* Initialise state bits to use from hardware */
+	sfp->state_hw_mask = mask;
+
+	/* We want to drive the rate select pins that the module is using */
+	sfp->state_hw_drive |= sfp->rs_state_mask;
+
 	if (sfp->quirk && sfp->quirk->fixup)
 		sfp->quirk->fixup(sfp);
+	mutex_unlock(&sfp->st_mutex);
 
 	return 0;
 }
@@ -2132,6 +2343,7 @@ static void sfp_sm_mod_remove(struct sfp *sfp)
 
 	memset(&sfp->id, 0, sizeof(sfp->id));
 	sfp->module_power_mW = 0;
+	sfp->state_hw_drive = SFP_F_TX_DISABLE;
 	sfp->have_a2 = false;
 
 	dev_info(sfp->dev, "module removed\n");
@@ -2452,10 +2664,8 @@ static void sfp_sm_main(struct sfp *sfp, unsigned int event)
 	}
 }
 
-static void sfp_sm_event(struct sfp *sfp, unsigned int event)
+static void __sfp_sm_event(struct sfp *sfp, unsigned int event)
 {
-	mutex_lock(&sfp->sm_mutex);
-
 	dev_dbg(sfp->dev, "SM: enter %s:%s:%s event %s\n",
 		mod_state_to_str(sfp->sm_mod_state),
 		dev_state_to_str(sfp->sm_dev_state),
@@ -2470,7 +2680,12 @@ static void sfp_sm_event(struct sfp *sfp, unsigned int event)
 		mod_state_to_str(sfp->sm_mod_state),
 		dev_state_to_str(sfp->sm_dev_state),
 		sm_state_to_str(sfp->sm_state));
+}
 
+static void sfp_sm_event(struct sfp *sfp, unsigned int event)
+{
+	mutex_lock(&sfp->sm_mutex);
+	__sfp_sm_event(sfp, event);
 	mutex_unlock(&sfp->sm_mutex);
 }
 
@@ -2494,6 +2709,20 @@ static void sfp_stop(struct sfp *sfp)
 	sfp_sm_event(sfp, SFP_E_DEV_DOWN);
 }
 
+static void sfp_set_signal_rate(struct sfp *sfp, unsigned int rate_kbd)
+{
+	unsigned int set;
+
+	sfp->rate_kbd = rate_kbd;
+
+	if (rate_kbd > sfp->rs_threshold_kbd)
+		set = sfp->rs_state_mask;
+	else
+		set = 0;
+
+	sfp_mod_state(sfp, SFP_F_RS0 | SFP_F_RS1, set);
+}
+
 static int sfp_module_info(struct sfp *sfp, struct ethtool_modinfo *modinfo)
 {
 	/* locking... and check module is present */
@@ -2578,6 +2807,7 @@ static const struct sfp_socket_ops sfp_module_ops = {
 	.detach = sfp_detach,
 	.start = sfp_start,
 	.stop = sfp_stop,
+	.set_signal_rate = sfp_set_signal_rate,
 	.module_info = sfp_module_info,
 	.module_eeprom = sfp_module_eeprom,
 	.module_eeprom_by_page = sfp_module_eeprom_by_page,
@@ -2596,6 +2826,7 @@ static void sfp_check_state(struct sfp *sfp)
 {
 	unsigned int state, i, changed;
 
+	rtnl_lock();
 	mutex_lock(&sfp->st_mutex);
 	state = sfp_get_state(sfp);
 	changed = state ^ sfp->state;
@@ -2609,23 +2840,24 @@ static void sfp_check_state(struct sfp *sfp)
 			dev_dbg(sfp->dev, "%s %u -> %u\n", gpio_names[i],
 				!!(sfp->state & BIT(i)), !!(state & BIT(i)));
 
-	state |= sfp->state & (SFP_F_TX_DISABLE | SFP_F_RATE_SELECT);
+	state |= sfp->state & SFP_F_OUTPUTS;
 	sfp->state = state;
+	mutex_unlock(&sfp->st_mutex);
 
-	rtnl_lock();
+	mutex_lock(&sfp->sm_mutex);
 	if (changed & SFP_F_PRESENT)
-		sfp_sm_event(sfp, state & SFP_F_PRESENT ?
-				SFP_E_INSERT : SFP_E_REMOVE);
+		__sfp_sm_event(sfp, state & SFP_F_PRESENT ?
+				    SFP_E_INSERT : SFP_E_REMOVE);
 
 	if (changed & SFP_F_TX_FAULT)
-		sfp_sm_event(sfp, state & SFP_F_TX_FAULT ?
-				SFP_E_TX_FAULT : SFP_E_TX_CLEAR);
+		__sfp_sm_event(sfp, state & SFP_F_TX_FAULT ?
+				    SFP_E_TX_FAULT : SFP_E_TX_CLEAR);
 
 	if (changed & SFP_F_LOS)
-		sfp_sm_event(sfp, state & SFP_F_LOS ?
-				SFP_E_LOS_HIGH : SFP_E_LOS_LOW);
+		__sfp_sm_event(sfp, state & SFP_F_LOS ?
+				    SFP_E_LOS_HIGH : SFP_E_LOS_LOW);
+	mutex_unlock(&sfp->sm_mutex);
 	rtnl_unlock();
-	mutex_unlock(&sfp->st_mutex);
 }
 
 static irqreturn_t sfp_irq(int irq, void *data)
@@ -2643,6 +2875,8 @@ static void sfp_poll(struct work_struct *work)
 
 	sfp_check_state(sfp);
 
+	// st_mutex doesn't need to be held here for state_soft_mask,
+	// it's unimportant if we race while reading this.
 	if (sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT) ||
 	    sfp->need_poll)
 		mod_delayed_work(system_wq, &sfp->poll, poll_jiffies);
@@ -2748,6 +2982,7 @@ static int sfp_probe(struct platform_device *pdev)
 		}
 
 	sfp->state_hw_mask = SFP_F_PRESENT;
+	sfp->state_hw_drive = SFP_F_TX_DISABLE;
 
 	sfp->get_state = sfp_gpio_get_state;
 	sfp->set_state = sfp_gpio_set_state;
@@ -2773,9 +3008,9 @@ static int sfp_probe(struct platform_device *pdev)
 	 */
 	sfp->state = sfp_get_state(sfp) | SFP_F_TX_DISABLE;
 
-	if (sfp->gpio[GPIO_RATE_SELECT] &&
-	    gpiod_get_value_cansleep(sfp->gpio[GPIO_RATE_SELECT]))
-		sfp->state |= SFP_F_RATE_SELECT;
+	if (sfp->gpio[GPIO_RS0] &&
+	    gpiod_get_value_cansleep(sfp->gpio[GPIO_RS0]))
+		sfp->state |= SFP_F_RS0;
 	sfp_set_state(sfp, sfp->state);
 	sfp_module_tx_disable(sfp);
 	if (sfp->state & SFP_F_PRESENT) {