diff options
Diffstat (limited to 'drivers/net/phy/sfp.c')
| -rw-r--r-- | drivers/net/phy/sfp.c | 60 |
1 files changed, 46 insertions, 14 deletions
diff --git a/drivers/net/phy/sfp.c b/drivers/net/phy/sfp.c index 3165bc7b8e1e..9cef89fe410d 100644 --- a/drivers/net/phy/sfp.c +++ b/drivers/net/phy/sfp.c @@ -114,10 +114,12 @@ struct sfp { struct gpio_desc *gpio[GPIO_MAX]; + bool attached; + struct mutex st_mutex; /* Protects state */ unsigned int state; struct delayed_work poll; struct delayed_work timeout; - struct mutex sm_mutex; + struct mutex sm_mutex; /* Protects state machine */ unsigned char sm_mod_state; unsigned char sm_dev_state; unsigned short sm_state; @@ -167,6 +169,7 @@ static int sfp__i2c_read(struct i2c_adapter *i2c, u8 bus_addr, u8 dev_addr, void *buf, size_t len) { struct i2c_msg msgs[2]; + size_t this_len; int ret; msgs[0].addr = bus_addr; @@ -178,11 +181,26 @@ static int sfp__i2c_read(struct i2c_adapter *i2c, u8 bus_addr, u8 dev_addr, msgs[1].len = len; msgs[1].buf = buf; - ret = i2c_transfer(i2c, msgs, ARRAY_SIZE(msgs)); - if (ret < 0) - return ret; + while (len) { + this_len = len; + if (this_len > 16) + this_len = 16; + + msgs[1].len = this_len; + + ret = i2c_transfer(i2c, msgs, ARRAY_SIZE(msgs)); + if (ret < 0) + return ret; + + if (ret != ARRAY_SIZE(msgs)) + break; + + msgs[1].buf += this_len; + dev_addr += this_len; + len -= this_len; + } - return ret == ARRAY_SIZE(msgs) ? len : 0; + return msgs[1].buf - (u8 *)buf; } static int sfp_i2c_read(struct sfp *sfp, bool a2, u8 addr, void *buf, @@ -500,7 +518,7 @@ static void sfp_sm_event(struct sfp *sfp, unsigned int event) */ switch (sfp->sm_mod_state) { default: - if (event == SFP_E_INSERT) { + if (event == SFP_E_INSERT && sfp->attached) { sfp_module_tx_disable(sfp); sfp_sm_ins_next(sfp, SFP_MOD_PROBE, T_PROBE_INIT); } @@ -628,6 +646,19 @@ static void sfp_sm_event(struct sfp *sfp, unsigned int event) mutex_unlock(&sfp->sm_mutex); } +static void sfp_attach(struct sfp *sfp) +{ + sfp->attached = true; + if (sfp->state & SFP_F_PRESENT) + sfp_sm_event(sfp, SFP_E_INSERT); +} + +static void sfp_detach(struct sfp *sfp) +{ + sfp->attached = false; + sfp_sm_event(sfp, SFP_E_REMOVE); +} + static void sfp_start(struct sfp *sfp) { sfp_sm_event(sfp, SFP_E_DEV_UP); @@ -687,6 +718,8 @@ static int sfp_module_eeprom(struct sfp *sfp, struct ethtool_eeprom *ee, } static const struct sfp_socket_ops sfp_module_ops = { + .attach = sfp_attach, + .detach = sfp_detach, .start = sfp_start, .stop = sfp_stop, .module_info = sfp_module_info, @@ -706,6 +739,7 @@ static void sfp_check_state(struct sfp *sfp) { unsigned int state, i, changed; + mutex_lock(&sfp->st_mutex); state = sfp_get_state(sfp); changed = state ^ sfp->state; changed &= SFP_F_PRESENT | SFP_F_LOS | SFP_F_TX_FAULT; @@ -731,6 +765,7 @@ static void sfp_check_state(struct sfp *sfp) sfp_sm_event(sfp, state & SFP_F_LOS ? SFP_E_LOS_HIGH : SFP_E_LOS_LOW); rtnl_unlock(); + mutex_unlock(&sfp->st_mutex); } static irqreturn_t sfp_irq(int irq, void *data) @@ -761,6 +796,7 @@ static struct sfp *sfp_alloc(struct device *dev) sfp->dev = dev; mutex_init(&sfp->sm_mutex); + mutex_init(&sfp->st_mutex); INIT_DELAYED_WORK(&sfp->poll, sfp_poll); INIT_DELAYED_WORK(&sfp->timeout, sfp_timeout); @@ -829,10 +865,6 @@ static int sfp_probe(struct platform_device *pdev) sfp->set_state = sfp_gpio_set_state; } - sfp->sfp_bus = sfp_register_socket(sfp->dev, sfp, &sfp_module_ops); - if (!sfp->sfp_bus) - return -ENOMEM; - /* Get the initial state, and always signal TX disable, * since the network interface will not be up. */ @@ -843,10 +875,6 @@ static int sfp_probe(struct platform_device *pdev) sfp->state |= SFP_F_RATE_SELECT; sfp_set_state(sfp, sfp->state); sfp_module_tx_disable(sfp); - rtnl_lock(); - if (sfp->state & SFP_F_PRESENT) - sfp_sm_event(sfp, SFP_E_INSERT); - rtnl_unlock(); for (i = 0; i < GPIO_MAX; i++) { if (gpio_flags[i] != GPIOD_IN || !sfp->gpio[i]) @@ -870,6 +898,10 @@ static int sfp_probe(struct platform_device *pdev) if (poll) mod_delayed_work(system_wq, &sfp->poll, poll_jiffies); + sfp->sfp_bus = sfp_register_socket(sfp->dev, sfp, &sfp_module_ops); + if (!sfp->sfp_bus) + return -ENOMEM; + return 0; } |