From 9a84d699ddde0d4e272aa919ad8fd50271a3f932 Mon Sep 17 00:00:00 2001
From: "Russell King (Oracle)" <rmk+kernel@armlinux.org.uk>
Date: Fri, 26 Aug 2022 08:48:20 +0100
Subject: [PATCH 5/6] net: sfp: redo soft state polling

Signed-off-by: Russell King (Oracle) <rmk+kernel@armlinux.org.uk>
---
 drivers/net/phy/sfp.c | 35 ++++++++++++++++++++++++-----------
 1 file changed, 24 insertions(+), 11 deletions(-)

--- a/drivers/net/phy/sfp.c
+++ b/drivers/net/phy/sfp.c
@@ -234,6 +234,7 @@ struct sfp {
 	bool need_poll;
 
 	struct mutex st_mutex;			/* Protects state */
+	unsigned int state_ignore_hw_mask;
 	unsigned int state_soft_mask;
 	unsigned int state;
 	struct delayed_work poll;
@@ -623,17 +624,18 @@ static void sfp_soft_set_state(struct sf
 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 &&
-	    !sfp->gpio[GPIO_TX_DISABLE])
-		sfp->state_soft_mask |= SFP_F_TX_DISABLE;
-	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_TX_FAULT &&
-	    !sfp->gpio[GPIO_TX_FAULT])
-		sfp->state_soft_mask |= SFP_F_TX_FAULT;
-	if (id->ext.enhopts & SFP_ENHOPTS_SOFT_RX_LOS &&
-	    !sfp->gpio[GPIO_LOS])
-		sfp->state_soft_mask |= SFP_F_LOS;
+	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;
+
+	// Poll the soft state for hardware pins we want to ignore
+	sfp->state_soft_mask = sfp->state_ignore_hw_mask & mask;
 
 	if (sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT) &&
 	    !sfp->need_poll)
@@ -647,10 +649,12 @@ static void sfp_soft_stop_poll(struct sf
 
 static unsigned int sfp_get_state(struct sfp *sfp)
 {
+	unsigned int soft = sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT);
 	unsigned int state = sfp->get_state(sfp);
 
-	if (state & SFP_F_PRESENT &&
-	    sfp->state_soft_mask & (SFP_F_LOS | SFP_F_TX_FAULT))
+	state &= ~sfp->state_ignore_hw_mask;
+
+	if (state & SFP_F_PRESENT && soft)
 		state |= sfp_soft_get_state(sfp);
 
 	return state;
@@ -2064,6 +2068,15 @@ static int sfp_sm_mod_probe(struct sfp *
 	if (ret < 0)
 		return ret;
 
+	/* Initialise state bits to ignore from hardware */
+	sfp->state_ignore_hw_mask = 0;
+	if (!sfp->gpio[GPIO_TX_DISABLE])
+		sfp->state_ignore_hw_mask |= SFP_F_TX_DISABLE;
+	if (!sfp->gpio[GPIO_TX_FAULT])
+		sfp->state_ignore_hw_mask |= SFP_F_TX_FAULT;
+	if (!sfp->gpio[GPIO_LOS])
+		sfp->state_ignore_hw_mask |= SFP_F_LOS;
+
 	sfp->module_t_start_up = T_START_UP;
 
 	sfp->tx_fault_ignore = false;
