drivers/net/phy/sfp.c

   1 #include <linux/delay.h>
   2 #include <linux/gpio/consumer.h>
   3 #include <linux/i2c.h>
   4 #include <linux/interrupt.h>
   5 #include <linux/jiffies.h>
   6 #include <linux/module.h>
   7 #include <linux/mutex.h>
   8 #include <linux/of.h>
   9 #include <linux/phy.h>
  10 #include <linux/platform_device.h>
  11 #include <linux/rtnetlink.h>
  12 #include <linux/slab.h>
  13 #include <linux/workqueue.h>
  14
  15 #include "mdio-i2c.h"
  16 #include "sfp.h"
  17 #include "swphy.h"
  18
  19 enum {
  20         GPIO_MODDEF0,
  21         GPIO_LOS,
  22         GPIO_TX_FAULT,
  23         GPIO_TX_DISABLE,
  24         GPIO_RATE_SELECT,
  25         GPIO_MAX,
  26
  27         SFP_F_PRESENT = BIT(GPIO_MODDEF0),
  28         SFP_F_LOS = BIT(GPIO_LOS),
  29         SFP_F_TX_FAULT = BIT(GPIO_TX_FAULT),
  30         SFP_F_TX_DISABLE = BIT(GPIO_TX_DISABLE),
  31         SFP_F_RATE_SELECT = BIT(GPIO_RATE_SELECT),
  32
  33         SFP_E_INSERT = 0,
  34         SFP_E_REMOVE,
  35         SFP_E_DEV_DOWN,
  36         SFP_E_DEV_UP,
  37         SFP_E_TX_FAULT,
  38         SFP_E_TX_CLEAR,
  39         SFP_E_LOS_HIGH,
  40         SFP_E_LOS_LOW,
  41         SFP_E_TIMEOUT,
  42
  43         SFP_MOD_EMPTY = 0,
  44         SFP_MOD_PROBE,
  45         SFP_MOD_PRESENT,
  46         SFP_MOD_ERROR,
  47
  48         SFP_DEV_DOWN = 0,
  49         SFP_DEV_UP,
  50
  51         SFP_S_DOWN = 0,
  52         SFP_S_INIT,
  53         SFP_S_WAIT_LOS,
  54         SFP_S_LINK_UP,
  55         SFP_S_TX_FAULT,
  56         SFP_S_REINIT,
  57         SFP_S_TX_DISABLE,
  58 };
  59
  60 static const char *gpio_of_names[] = {
  61         "mod-def0",
  62         "los",
  63         "tx-fault",
  64         "tx-disable",
  65         "rate-select0",
  66 };
  67
  68 static const enum gpiod_flags gpio_flags[] = {
  69         GPIOD_IN,
  70         GPIOD_IN,
  71         GPIOD_IN,
  72         GPIOD_ASIS,
  73         GPIOD_ASIS,
  74 };
  75
  76 #define T_INIT_JIFFIES  msecs_to_jiffies(300)
  77 #define T_RESET_US      10
  78 #define T_FAULT_RECOVER msecs_to_jiffies(1000)
  79
  80 /* SFP module presence detection is poor: the three MOD DEF signals are
  81  * the same length on the PCB, which means it's possible for MOD DEF 0 to
  82  * connect before the I2C bus on MOD DEF 1/2.
  83  *
  84  * The SFP MSA specifies 300ms as t_init (the time taken for TX_FAULT to
  85  * be deasserted) but makes no mention of the earliest time before we can
  86  * access the I2C EEPROM.  However, Avago modules require 300ms.
  87  */
  88 #define T_PROBE_INIT    msecs_to_jiffies(300)
  89 #define T_PROBE_RETRY   msecs_to_jiffies(100)
  90
  91 /* SFP modules appear to always have their PHY configured for bus address
  92  * 0x56 (which with mdio-i2c, translates to a PHY address of 22).
  93  */
  94 #define SFP_PHY_ADDR    22
  95
  96 /* Give this long for the PHY to reset. */
  97 #define T_PHY_RESET_MS  50
  98
  99 static DEFINE_MUTEX(sfp_mutex);
 100
 101 struct sff_data {
 102         unsigned int gpios;
 103         bool (*module_supported)(const struct sfp_eeprom_id *id);
 104 };
 105
 106 struct sfp {
 107         struct device *dev;
 108         struct i2c_adapter *i2c;
 109         struct mii_bus *i2c_mii;
 110         struct sfp_bus *sfp_bus;
 111         struct phy_device *mod_phy;
 112         const struct sff_data *type;
 113
 114         unsigned int (*get_state)(struct sfp *);
 115         void (*set_state)(struct sfp *, unsigned int);
 116         int (*read)(struct sfp *, bool, u8, void *, size_t);
 117
 118         struct gpio_desc *gpio[GPIO_MAX];
 119
 120         unsigned int state;
 121         struct delayed_work poll;
 122         struct delayed_work timeout;
 123         struct mutex sm_mutex;
 124         unsigned char sm_mod_state;
 125         unsigned char sm_dev_state;
 126         unsigned short sm_state;
 127         unsigned int sm_retries;
 128
 129         struct sfp_eeprom_id id;
 130 };
 131
 132 static bool sff_module_supported(const struct sfp_eeprom_id *id)
 133 {
 134         return id->base.phys_id == SFP_PHYS_ID_SFF &&
 135                id->base.phys_ext_id == SFP_PHYS_EXT_ID_SFP;
 136 }
 137
 138 static const struct sff_data sff_data = {
 139         .gpios = SFP_F_LOS | SFP_F_TX_FAULT | SFP_F_TX_DISABLE,
 140         .module_supported = sff_module_supported,
 141 };
 142
 143 static bool sfp_module_supported(const struct sfp_eeprom_id *id)
 144 {
 145         return id->base.phys_id == SFP_PHYS_ID_SFP &&
 146                id->base.phys_ext_id == SFP_PHYS_EXT_ID_SFP;
 147 }
 148
 149 static const struct sff_data sfp_data = {
 150         .gpios = SFP_F_PRESENT | SFP_F_LOS | SFP_F_TX_FAULT |
 151                  SFP_F_TX_DISABLE | SFP_F_RATE_SELECT,
 152         .module_supported = sfp_module_supported,
 153 };
 154
 155 static const struct of_device_id sfp_of_match[] = {
 156         { .compatible = "sff,sff", .data = &sff_data, },
 157         { .compatible = "sff,sfp", .data = &sfp_data, },
 158         { },
 159 };
 160 MODULE_DEVICE_TABLE(of, sfp_of_match);
 161
 162 static unsigned long poll_jiffies;
 163
 164 static unsigned int sfp_gpio_get_state(struct sfp *sfp)
 165 {
 166         unsigned int i, state, v;
 167
 168         for (i = state = 0; i < GPIO_MAX; i++) {
 169                 if (gpio_flags[i] != GPIOD_IN || !sfp->gpio[i])
 170                         continue;
 171
 172                 v = gpiod_get_value_cansleep(sfp->gpio[i]);
 173                 if (v)
 174                         state |= BIT(i);
 175         }
 176
 177         return state;
 178 }
 179
 180 static unsigned int sff_gpio_get_state(struct sfp *sfp)
 181 {
 182         return sfp_gpio_get_state(sfp) | SFP_F_PRESENT;
 183 }
 184
 185 static void sfp_gpio_set_state(struct sfp *sfp, unsigned int state)
 186 {
 187         if (state & SFP_F_PRESENT) {
 188                 /* If the module is present, drive the signals */
 189                 if (sfp->gpio[GPIO_TX_DISABLE])
 190                         gpiod_direction_output(sfp->gpio[GPIO_TX_DISABLE],
 191                                                state & SFP_F_TX_DISABLE);
 192                 if (state & SFP_F_RATE_SELECT)
 193                         gpiod_direction_output(sfp->gpio[GPIO_RATE_SELECT],
 194                                                state & SFP_F_RATE_SELECT);
 195         } else {
 196                 /* Otherwise, let them float to the pull-ups */
 197                 if (sfp->gpio[GPIO_TX_DISABLE])
 198                         gpiod_direction_input(sfp->gpio[GPIO_TX_DISABLE]);
 199                 if (state & SFP_F_RATE_SELECT)
 200                         gpiod_direction_input(sfp->gpio[GPIO_RATE_SELECT]);
 201         }
 202 }
 203
 204 static int sfp__i2c_read(struct i2c_adapter *i2c, u8 bus_addr, u8 dev_addr,
 205                          void *buf, size_t len)
 206 {
 207         struct i2c_msg msgs[2];
 208         int ret;
 209
 210         msgs[0].addr = bus_addr;
 211         msgs[0].flags = 0;
 212         msgs[0].len = 1;
 213         msgs[0].buf = &dev_addr;
 214         msgs[1].addr = bus_addr;
 215         msgs[1].flags = I2C_M_RD;
 216         msgs[1].len = len;
 217         msgs[1].buf = buf;
 218
 219         ret = i2c_transfer(i2c, msgs, ARRAY_SIZE(msgs));
 220         if (ret < 0)
 221                 return ret;
 222
 223         return ret == ARRAY_SIZE(msgs) ? len : 0;
 224 }
 225
 226 static int sfp_i2c_read(struct sfp *sfp, bool a2, u8 addr, void *buf,
 227                         size_t len)
 228 {
 229         return sfp__i2c_read(sfp->i2c, a2 ? 0x51 : 0x50, addr, buf, len);
 230 }
 231
 232 static int sfp_i2c_configure(struct sfp *sfp, struct i2c_adapter *i2c)
 233 {
 234         struct mii_bus *i2c_mii;
 235         int ret;
 236
 237         if (!i2c_check_functionality(i2c, I2C_FUNC_I2C))
 238                 return -EINVAL;
 239
 240         sfp->i2c = i2c;
 241         sfp->read = sfp_i2c_read;
 242
 243         i2c_mii = mdio_i2c_alloc(sfp->dev, i2c);
 244         if (IS_ERR(i2c_mii))
 245                 return PTR_ERR(i2c_mii);
 246
 247         i2c_mii->name = "SFP I2C Bus";
 248         i2c_mii->phy_mask = ~0;
 249
 250         ret = mdiobus_register(i2c_mii);
 251         if (ret < 0) {
 252                 mdiobus_free(i2c_mii);
 253                 return ret;
 254         }
 255
 256         sfp->i2c_mii = i2c_mii;
 257
 258         return 0;
 259 }
 260
 261 /* Interface */
 262 static unsigned int sfp_get_state(struct sfp *sfp)
 263 {
 264         return sfp->get_state(sfp);
 265 }
 266
 267 static void sfp_set_state(struct sfp *sfp, unsigned int state)
 268 {
 269         sfp->set_state(sfp, state);
 270 }
 271
 272 static int sfp_read(struct sfp *sfp, bool a2, u8 addr, void *buf, size_t len)
 273 {
 274         return sfp->read(sfp, a2, addr, buf, len);
 275 }
 276
 277 static unsigned int sfp_check(void *buf, size_t len)
 278 {
 279         u8 *p, check;
 280
 281         for (p = buf, check = 0; len; p++, len--)
 282                 check += *p;
 283
 284         return check;
 285 }
 286
 287 /* Helpers */
 288 static void sfp_module_tx_disable(struct sfp *sfp)
 289 {
 290         dev_dbg(sfp->dev, "tx disable %u -> %u\n",
 291                 sfp->state & SFP_F_TX_DISABLE ? 1 : 0, 1);
 292         sfp->state |= SFP_F_TX_DISABLE;
 293         sfp_set_state(sfp, sfp->state);
 294 }
 295
 296 static void sfp_module_tx_enable(struct sfp *sfp)
 297 {
 298         dev_dbg(sfp->dev, "tx disable %u -> %u\n",
 299                 sfp->state & SFP_F_TX_DISABLE ? 1 : 0, 0);
 300         sfp->state &= ~SFP_F_TX_DISABLE;
 301         sfp_set_state(sfp, sfp->state);
 302 }
 303
 304 static void sfp_module_tx_fault_reset(struct sfp *sfp)
 305 {
 306         unsigned int state = sfp->state;
 307
 308         if (state & SFP_F_TX_DISABLE)
 309                 return;
 310
 311         sfp_set_state(sfp, state | SFP_F_TX_DISABLE);
 312
 313         udelay(T_RESET_US);
 314
 315         sfp_set_state(sfp, state);
 316 }
 317
 318 /* SFP state machine */
 319 static void sfp_sm_set_timer(struct sfp *sfp, unsigned int timeout)
 320 {
 321         if (timeout)
 322                 mod_delayed_work(system_power_efficient_wq, &sfp->timeout,
 323                                  timeout);
 324         else
 325                 cancel_delayed_work(&sfp->timeout);
 326 }
 327
 328 static void sfp_sm_next(struct sfp *sfp, unsigned int state,
 329                         unsigned int timeout)
 330 {
 331         sfp->sm_state = state;
 332         sfp_sm_set_timer(sfp, timeout);
 333 }
 334
 335 static void sfp_sm_ins_next(struct sfp *sfp, unsigned int state,
 336                             unsigned int timeout)
 337 {
 338         sfp->sm_mod_state = state;
 339         sfp_sm_set_timer(sfp, timeout);
 340 }
 341
 342 static void sfp_sm_phy_detach(struct sfp *sfp)
 343 {
 344         phy_stop(sfp->mod_phy);
 345         sfp_remove_phy(sfp->sfp_bus);
 346         phy_device_remove(sfp->mod_phy);
 347         phy_device_free(sfp->mod_phy);
 348         sfp->mod_phy = NULL;
 349 }
 350
 351 static void sfp_sm_probe_phy(struct sfp *sfp)
 352 {
 353         struct phy_device *phy;
 354         int err;
 355
 356         msleep(T_PHY_RESET_MS);
 357
 358         phy = mdiobus_scan(sfp->i2c_mii, SFP_PHY_ADDR);
 359         if (phy == ERR_PTR(-ENODEV)) {
 360                 dev_info(sfp->dev, "no PHY detected\n");
 361                 return;
 362         }
 363         if (IS_ERR(phy)) {
 364                 dev_err(sfp->dev, "mdiobus scan returned %ld\n", PTR_ERR(phy));
 365                 return;
 366         }
 367
 368         err = sfp_add_phy(sfp->sfp_bus, phy);
 369         if (err) {
 370                 phy_device_remove(phy);
 371                 phy_device_free(phy);
 372                 dev_err(sfp->dev, "sfp_add_phy failed: %d\n", err);
 373                 return;
 374         }
 375
 376         sfp->mod_phy = phy;
 377         phy_start(phy);
 378 }
 379
 380 static void sfp_sm_link_up(struct sfp *sfp)
 381 {
 382         sfp_link_up(sfp->sfp_bus);
 383         sfp_sm_next(sfp, SFP_S_LINK_UP, 0);
 384 }
 385
 386 static void sfp_sm_link_down(struct sfp *sfp)
 387 {
 388         sfp_link_down(sfp->sfp_bus);
 389 }
 390
 391 static void sfp_sm_link_check_los(struct sfp *sfp)
 392 {
 393         unsigned int los = sfp->state & SFP_F_LOS;
 394
 395         /* If neither SFP_OPTIONS_LOS_INVERTED nor SFP_OPTIONS_LOS_NORMAL
 396          * are set, we assume that no LOS signal is available.
 397          */
 398         if (sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_INVERTED))
 399                 los ^= SFP_F_LOS;
 400         else if (!(sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_NORMAL)))
 401                 los = 0;
 402
 403         if (los)
 404                 sfp_sm_next(sfp, SFP_S_WAIT_LOS, 0);
 405         else
 406                 sfp_sm_link_up(sfp);
 407 }
 408
 409 static bool sfp_los_event_active(struct sfp *sfp, unsigned int event)
 410 {
 411         return (sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_INVERTED) &&
 412                 event == SFP_E_LOS_LOW) ||
 413                (sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_NORMAL) &&
 414                 event == SFP_E_LOS_HIGH);
 415 }
 416
 417 static bool sfp_los_event_inactive(struct sfp *sfp, unsigned int event)
 418 {
 419         return (sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_INVERTED) &&
 420                 event == SFP_E_LOS_HIGH) ||
 421                (sfp->id.ext.options & cpu_to_be16(SFP_OPTIONS_LOS_NORMAL) &&
 422                 event == SFP_E_LOS_LOW);
 423 }
 424
 425 static void sfp_sm_fault(struct sfp *sfp, bool warn)
 426 {
 427         if (sfp->sm_retries && !--sfp->sm_retries) {
 428                 dev_err(sfp->dev,
 429                         "module persistently indicates fault, disabling\n");
 430                 sfp_sm_next(sfp, SFP_S_TX_DISABLE, 0);
 431         } else {
 432                 if (warn)
 433                         dev_err(sfp->dev, "module transmit fault indicated\n");
 434
 435                 sfp_sm_next(sfp, SFP_S_TX_FAULT, T_FAULT_RECOVER);
 436         }
 437 }
 438
 439 static void sfp_sm_mod_init(struct sfp *sfp)
 440 {
 441         sfp_module_tx_enable(sfp);
 442
 443         /* Wait t_init before indicating that the link is up, provided the
 444          * current state indicates no TX_FAULT.  If TX_FAULT clears before
 445          * this time, that's fine too.
 446          */
 447         sfp_sm_next(sfp, SFP_S_INIT, T_INIT_JIFFIES);
 448         sfp->sm_retries = 5;
 449
 450         /* Setting the serdes link mode is guesswork: there's no
 451          * field in the EEPROM which indicates what mode should
 452          * be used.
 453          *
 454          * If it's a gigabit-only fiber module, it probably does
 455          * not have a PHY, so switch to 802.3z negotiation mode.
 456          * Otherwise, switch to SGMII mode (which is required to
 457          * support non-gigabit speeds) and probe for a PHY.
 458          */
 459         if (sfp->id.base.e1000_base_t ||
 460             sfp->id.base.e100_base_lx ||
 461             sfp->id.base.e100_base_fx)
 462                 sfp_sm_probe_phy(sfp);
 463 }
 464
 465 static int sfp_sm_mod_probe(struct sfp *sfp)
 466 {
 467         /* SFP module inserted - read I2C data */
 468         struct sfp_eeprom_id id;
 469         u8 check;
 470         int err;
 471
 472         err = sfp_read(sfp, false, 0, &id, sizeof(id));
 473         if (err < 0) {
 474                 dev_err(sfp->dev, "failed to read EEPROM: %d\n", err);
 475                 return -EAGAIN;
 476         }
 477
 478         if (err != sizeof(id)) {
 479                 dev_err(sfp->dev, "EEPROM short read: %d\n", err);
 480                 return -EAGAIN;
 481         }
 482
 483         /* Validate the checksum over the base structure */
 484         check = sfp_check(&id.base, sizeof(id.base) - 1);
 485         if (check != id.base.cc_base) {
 486                 dev_err(sfp->dev,
 487                         "EEPROM base structure checksum failure: 0x%02x\n",
 488                         check);
 489                 print_hex_dump(KERN_ERR, "sfp EE: ", DUMP_PREFIX_OFFSET,
 490                                16, 1, &id, sizeof(id.base) - 1, true);
 491                 return -EINVAL;
 492         }
 493
 494         check = sfp_check(&id.ext, sizeof(id.ext) - 1);
 495         if (check != id.ext.cc_ext) {
 496                 dev_err(sfp->dev,
 497                         "EEPROM extended structure checksum failure: 0x%02x\n",
 498                         check);
 499                 memset(&id.ext, 0, sizeof(id.ext));
 500         }
 501
 502         sfp->id = id;
 503
 504         dev_info(sfp->dev, "module %.*s %.*s rev %.*s sn %.*s dc %.*s\n",
 505                  (int)sizeof(id.base.vendor_name), id.base.vendor_name,
 506                  (int)sizeof(id.base.vendor_pn), id.base.vendor_pn,
 507                  (int)sizeof(id.base.vendor_rev), id.base.vendor_rev,
 508                  (int)sizeof(id.ext.vendor_sn), id.ext.vendor_sn,
 509                  (int)sizeof(id.ext.datecode), id.ext.datecode);
 510
 511         /* Check whether we support this module */
 512         if (!sfp->type->module_supported(&sfp->id)) {
 513                 dev_err(sfp->dev,
 514                         "module is not supported - phys id 0x%02x 0x%02x\n",
 515                         sfp->id.base.phys_id, sfp->id.base.phys_ext_id);
 516                 return -EINVAL;
 517         }
 518
 519         /* If the module requires address swap mode, warn about it */
 520         if (sfp->id.ext.diagmon & SFP_DIAGMON_ADDRMODE)
 521                 dev_warn(sfp->dev,
 522                          "module address swap to access page 0xA2 is not supported.\n");
 523
 524         return sfp_module_insert(sfp->sfp_bus, &sfp->id);
 525 }
 526
 527 static void sfp_sm_mod_remove(struct sfp *sfp)
 528 {
 529         sfp_module_remove(sfp->sfp_bus);
 530
 531         if (sfp->mod_phy)
 532                 sfp_sm_phy_detach(sfp);
 533
 534         sfp_module_tx_disable(sfp);
 535
 536         memset(&sfp->id, 0, sizeof(sfp->id));
 537
 538         dev_info(sfp->dev, "module removed\n");
 539 }
 540
 541 static void sfp_sm_event(struct sfp *sfp, unsigned int event)
 542 {
 543         mutex_lock(&sfp->sm_mutex);
 544
 545         dev_dbg(sfp->dev, "SM: enter %u:%u:%u event %u\n",
 546                 sfp->sm_mod_state, sfp->sm_dev_state, sfp->sm_state, event);
 547
 548         /* This state machine tracks the insert/remove state of
 549          * the module, and handles probing the on-board EEPROM.
 550          */
 551         switch (sfp->sm_mod_state) {
 552         default:
 553                 if (event == SFP_E_INSERT) {
 554                         sfp_module_tx_disable(sfp);
 555                         sfp_sm_ins_next(sfp, SFP_MOD_PROBE, T_PROBE_INIT);
 556                 }
 557                 break;
 558
 559         case SFP_MOD_PROBE:
 560                 if (event == SFP_E_REMOVE) {
 561                         sfp_sm_ins_next(sfp, SFP_MOD_EMPTY, 0);
 562                 } else if (event == SFP_E_TIMEOUT) {
 563                         int err = sfp_sm_mod_probe(sfp);
 564
 565                         if (err == 0)
 566                                 sfp_sm_ins_next(sfp, SFP_MOD_PRESENT, 0);
 567                         else if (err == -EAGAIN)
 568                                 sfp_sm_set_timer(sfp, T_PROBE_RETRY);
 569                         else
 570                                 sfp_sm_ins_next(sfp, SFP_MOD_ERROR, 0);
 571                 }
 572                 break;
 573
 574         case SFP_MOD_PRESENT:
 575         case SFP_MOD_ERROR:
 576                 if (event == SFP_E_REMOVE) {
 577                         sfp_sm_mod_remove(sfp);
 578                         sfp_sm_ins_next(sfp, SFP_MOD_EMPTY, 0);
 579                 }
 580                 break;
 581         }
 582
 583         /* This state machine tracks the netdev up/down state */
 584         switch (sfp->sm_dev_state) {
 585         default:
 586                 if (event == SFP_E_DEV_UP)
 587                         sfp->sm_dev_state = SFP_DEV_UP;
 588                 break;
 589
 590         case SFP_DEV_UP:
 591                 if (event == SFP_E_DEV_DOWN) {
 592                         /* If the module has a PHY, avoid raising TX disable
 593                          * as this resets the PHY. Otherwise, raise it to
 594                          * turn the laser off.
 595                          */
 596                         if (!sfp->mod_phy)
 597                                 sfp_module_tx_disable(sfp);
 598                         sfp->sm_dev_state = SFP_DEV_DOWN;
 599                 }
 600                 break;
 601         }
 602
 603         /* Some events are global */
 604         if (sfp->sm_state != SFP_S_DOWN &&
 605             (sfp->sm_mod_state != SFP_MOD_PRESENT ||
 606              sfp->sm_dev_state != SFP_DEV_UP)) {
 607                 if (sfp->sm_state == SFP_S_LINK_UP &&
 608                     sfp->sm_dev_state == SFP_DEV_UP)
 609                         sfp_sm_link_down(sfp);
 610                 if (sfp->mod_phy)
 611                         sfp_sm_phy_detach(sfp);
 612                 sfp_sm_next(sfp, SFP_S_DOWN, 0);
 613                 mutex_unlock(&sfp->sm_mutex);
 614                 return;
 615         }
 616
 617         /* The main state machine */
 618         switch (sfp->sm_state) {
 619         case SFP_S_DOWN:
 620                 if (sfp->sm_mod_state == SFP_MOD_PRESENT &&
 621                     sfp->sm_dev_state == SFP_DEV_UP)
 622                         sfp_sm_mod_init(sfp);
 623                 break;
 624
 625         case SFP_S_INIT:
 626                 if (event == SFP_E_TIMEOUT && sfp->state & SFP_F_TX_FAULT)
 627                         sfp_sm_fault(sfp, true);
 628                 else if (event == SFP_E_TIMEOUT || event == SFP_E_TX_CLEAR)
 629                         sfp_sm_link_check_los(sfp);
 630                 break;
 631
 632         case SFP_S_WAIT_LOS:
 633                 if (event == SFP_E_TX_FAULT)
 634                         sfp_sm_fault(sfp, true);
 635                 else if (sfp_los_event_inactive(sfp, event))
 636                         sfp_sm_link_up(sfp);
 637                 break;
 638
 639         case SFP_S_LINK_UP:
 640                 if (event == SFP_E_TX_FAULT) {
 641                         sfp_sm_link_down(sfp);
 642                         sfp_sm_fault(sfp, true);
 643                 } else if (sfp_los_event_active(sfp, event)) {
 644                         sfp_sm_link_down(sfp);
 645                         sfp_sm_next(sfp, SFP_S_WAIT_LOS, 0);
 646                 }
 647                 break;
 648
 649         case SFP_S_TX_FAULT:
 650                 if (event == SFP_E_TIMEOUT) {
 651                         sfp_module_tx_fault_reset(sfp);
 652                         sfp_sm_next(sfp, SFP_S_REINIT, T_INIT_JIFFIES);
 653                 }
 654                 break;
 655
 656         case SFP_S_REINIT:
 657                 if (event == SFP_E_TIMEOUT && sfp->state & SFP_F_TX_FAULT) {
 658                         sfp_sm_fault(sfp, false);
 659                 } else if (event == SFP_E_TIMEOUT || event == SFP_E_TX_CLEAR) {
 660                         dev_info(sfp->dev, "module transmit fault recovered\n");
 661                         sfp_sm_link_check_los(sfp);
 662                 }
 663                 break;
 664
 665         case SFP_S_TX_DISABLE:
 666                 break;
 667         }
 668
 669         dev_dbg(sfp->dev, "SM: exit %u:%u:%u\n",
 670                 sfp->sm_mod_state, sfp->sm_dev_state, sfp->sm_state);
 671
 672         mutex_unlock(&sfp->sm_mutex);
 673 }
 674
 675 static void sfp_start(struct sfp *sfp)
 676 {
 677         sfp_sm_event(sfp, SFP_E_DEV_UP);
 678 }
 679
 680 static void sfp_stop(struct sfp *sfp)
 681 {
 682         sfp_sm_event(sfp, SFP_E_DEV_DOWN);
 683 }
 684
 685 static int sfp_module_info(struct sfp *sfp, struct ethtool_modinfo *modinfo)
 686 {
 687         /* locking... and check module is present */
 688
 689         if (sfp->id.ext.sff8472_compliance &&
 690             !(sfp->id.ext.diagmon & SFP_DIAGMON_ADDRMODE)) {
 691                 modinfo->type = ETH_MODULE_SFF_8472;
 692                 modinfo->eeprom_len = ETH_MODULE_SFF_8472_LEN;
 693         } else {
 694                 modinfo->type = ETH_MODULE_SFF_8079;
 695                 modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
 696         }
 697         return 0;
 698 }
 699
 700 static int sfp_module_eeprom(struct sfp *sfp, struct ethtool_eeprom *ee,
 701                              u8 *data)
 702 {
 703         unsigned int first, last, len;
 704         int ret;
 705
 706         if (ee->len == 0)
 707                 return -EINVAL;
 708
 709         first = ee->offset;
 710         last = ee->offset + ee->len;
 711         if (first < ETH_MODULE_SFF_8079_LEN) {
 712                 len = min_t(unsigned int, last, ETH_MODULE_SFF_8079_LEN);
 713                 len -= first;
 714
 715                 ret = sfp_read(sfp, false, first, data, len);
 716                 if (ret < 0)
 717                         return ret;
 718
 719                 first += len;
 720                 data += len;
 721         }
 722         if (first < ETH_MODULE_SFF_8472_LEN && last > ETH_MODULE_SFF_8079_LEN) {
 723                 len = min_t(unsigned int, last, ETH_MODULE_SFF_8472_LEN);
 724                 len -= first;
 725                 first -= ETH_MODULE_SFF_8079_LEN;
 726
 727                 ret = sfp_read(sfp, true, first, data, len);
 728                 if (ret < 0)
 729                         return ret;
 730         }
 731         return 0;
 732 }
 733
 734 static const struct sfp_socket_ops sfp_module_ops = {
 735         .start = sfp_start,
 736         .stop = sfp_stop,
 737         .module_info = sfp_module_info,
 738         .module_eeprom = sfp_module_eeprom,
 739 };
 740
 741 static void sfp_timeout(struct work_struct *work)
 742 {
 743         struct sfp *sfp = container_of(work, struct sfp, timeout.work);
 744
 745         rtnl_lock();
 746         sfp_sm_event(sfp, SFP_E_TIMEOUT);
 747         rtnl_unlock();
 748 }
 749
 750 static void sfp_check_state(struct sfp *sfp)
 751 {
 752         unsigned int state, i, changed;
 753
 754         state = sfp_get_state(sfp);
 755         changed = state ^ sfp->state;
 756         changed &= SFP_F_PRESENT | SFP_F_LOS | SFP_F_TX_FAULT;
 757
 758         for (i = 0; i < GPIO_MAX; i++)
 759                 if (changed & BIT(i))
 760                         dev_dbg(sfp->dev, "%s %u -> %u\n", gpio_of_names[i],
 761                                 !!(sfp->state & BIT(i)), !!(state & BIT(i)));
 762
 763         state |= sfp->state & (SFP_F_TX_DISABLE | SFP_F_RATE_SELECT);
 764         sfp->state = state;
 765
 766         rtnl_lock();
 767         if (changed & SFP_F_PRESENT)
 768                 sfp_sm_event(sfp, state & SFP_F_PRESENT ?
 769                                 SFP_E_INSERT : SFP_E_REMOVE);
 770
 771         if (changed & SFP_F_TX_FAULT)
 772                 sfp_sm_event(sfp, state & SFP_F_TX_FAULT ?
 773                                 SFP_E_TX_FAULT : SFP_E_TX_CLEAR);
 774
 775         if (changed & SFP_F_LOS)
 776                 sfp_sm_event(sfp, state & SFP_F_LOS ?
 777                                 SFP_E_LOS_HIGH : SFP_E_LOS_LOW);
 778         rtnl_unlock();
 779 }
 780
 781 static irqreturn_t sfp_irq(int irq, void *data)
 782 {
 783         struct sfp *sfp = data;
 784
 785         sfp_check_state(sfp);
 786
 787         return IRQ_HANDLED;
 788 }
 789
 790 static void sfp_poll(struct work_struct *work)
 791 {
 792         struct sfp *sfp = container_of(work, struct sfp, poll.work);
 793
 794         sfp_check_state(sfp);
 795         mod_delayed_work(system_wq, &sfp->poll, poll_jiffies);
 796 }
 797
 798 static struct sfp *sfp_alloc(struct device *dev)
 799 {
 800         struct sfp *sfp;
 801
 802         sfp = kzalloc(sizeof(*sfp), GFP_KERNEL);
 803         if (!sfp)
 804                 return ERR_PTR(-ENOMEM);
 805
 806         sfp->dev = dev;
 807
 808         mutex_init(&sfp->sm_mutex);
 809         INIT_DELAYED_WORK(&sfp->poll, sfp_poll);
 810         INIT_DELAYED_WORK(&sfp->timeout, sfp_timeout);
 811
 812         return sfp;
 813 }
 814
 815 static void sfp_cleanup(void *data)
 816 {
 817         struct sfp *sfp = data;
 818
 819         cancel_delayed_work_sync(&sfp->poll);
 820         cancel_delayed_work_sync(&sfp->timeout);
 821         if (sfp->i2c_mii) {
 822                 mdiobus_unregister(sfp->i2c_mii);
 823                 mdiobus_free(sfp->i2c_mii);
 824         }
 825         if (sfp->i2c)
 826                 i2c_put_adapter(sfp->i2c);
 827         kfree(sfp);
 828 }
 829
 830 static int sfp_probe(struct platform_device *pdev)
 831 {
 832         const struct sff_data *sff;
 833         struct sfp *sfp;
 834         bool poll = false;
 835         int irq, err, i;
 836
 837         sfp = sfp_alloc(&pdev->dev);
 838         if (IS_ERR(sfp))
 839                 return PTR_ERR(sfp);
 840
 841         platform_set_drvdata(pdev, sfp);
 842
 843         err = devm_add_action(sfp->dev, sfp_cleanup, sfp);
 844         if (err < 0)
 845                 return err;
 846
 847         sff = sfp->type = &sfp_data;
 848
 849         if (pdev->dev.of_node) {
 850                 struct device_node *node = pdev->dev.of_node;
 851                 const struct of_device_id *id;
 852                 struct device_node *np;
 853
 854                 id = of_match_node(sfp_of_match, node);
 855                 if (WARN_ON(!id))
 856                         return -EINVAL;
 857
 858                 sff = sfp->type = id->data;
 859
 860                 np = of_parse_phandle(node, "i2c-bus", 0);
 861                 if (np) {
 862                         struct i2c_adapter *i2c;
 863
 864                         i2c = of_find_i2c_adapter_by_node(np);
 865                         of_node_put(np);
 866                         if (!i2c)
 867                                 return -EPROBE_DEFER;
 868
 869                         err = sfp_i2c_configure(sfp, i2c);
 870                         if (err < 0) {
 871                                 i2c_put_adapter(i2c);
 872                                 return err;
 873                         }
 874                 }
 875         }
 876
 877         for (i = 0; i < GPIO_MAX; i++)
 878                 if (sff->gpios & BIT(i)) {
 879                         sfp->gpio[i] = devm_gpiod_get_optional(sfp->dev,
 880                                            gpio_of_names[i], gpio_flags[i]);
 881                         if (IS_ERR(sfp->gpio[i]))
 882                                 return PTR_ERR(sfp->gpio[i]);
 883                 }
 884
 885         sfp->get_state = sfp_gpio_get_state;
 886         sfp->set_state = sfp_gpio_set_state;
 887
 888         /* Modules that have no detect signal are always present */
 889         if (!(sfp->gpio[GPIO_MODDEF0]))
 890                 sfp->get_state = sff_gpio_get_state;
 891
 892         sfp->sfp_bus = sfp_register_socket(sfp->dev, sfp, &sfp_module_ops);
 893         if (!sfp->sfp_bus)
 894                 return -ENOMEM;
 895
 896         /* Get the initial state, and always signal TX disable,
 897          * since the network interface will not be up.
 898          */
 899         sfp->state = sfp_get_state(sfp) | SFP_F_TX_DISABLE;
 900
 901         if (sfp->gpio[GPIO_RATE_SELECT] &&
 902             gpiod_get_value_cansleep(sfp->gpio[GPIO_RATE_SELECT]))
 903                 sfp->state |= SFP_F_RATE_SELECT;
 904         sfp_set_state(sfp, sfp->state);
 905         sfp_module_tx_disable(sfp);
 906         rtnl_lock();
 907         if (sfp->state & SFP_F_PRESENT)
 908                 sfp_sm_event(sfp, SFP_E_INSERT);
 909         rtnl_unlock();
 910
 911         for (i = 0; i < GPIO_MAX; i++) {
 912                 if (gpio_flags[i] != GPIOD_IN || !sfp->gpio[i])
 913                         continue;
 914
 915                 irq = gpiod_to_irq(sfp->gpio[i]);
 916                 if (!irq) {
 917                         poll = true;
 918                         continue;
 919                 }
 920
 921                 err = devm_request_threaded_irq(sfp->dev, irq, NULL, sfp_irq,
 922                                                 IRQF_ONESHOT |
 923                                                 IRQF_TRIGGER_RISING |
 924                                                 IRQF_TRIGGER_FALLING,
 925                                                 dev_name(sfp->dev), sfp);
 926                 if (err)
 927                         poll = true;
 928         }
 929
 930         if (poll)
 931                 mod_delayed_work(system_wq, &sfp->poll, poll_jiffies);
 932
 933         return 0;
 934 }
 935
 936 static int sfp_remove(struct platform_device *pdev)
 937 {
 938         struct sfp *sfp = platform_get_drvdata(pdev);
 939
 940         sfp_unregister_socket(sfp->sfp_bus);
 941
 942         return 0;
 943 }
 944
 945 static struct platform_driver sfp_driver = {
 946         .probe = sfp_probe,
 947         .remove = sfp_remove,
 948         .driver = {
 949                 .name = "sfp",
 950                 .of_match_table = sfp_of_match,
 951         },
 952 };
 953
 954 static int sfp_init(void)
 955 {
 956         poll_jiffies = msecs_to_jiffies(100);
 957
 958         return platform_driver_register(&sfp_driver);
 959 }
 960 module_init(sfp_init);
 961
 962 static void sfp_exit(void)
 963 {
 964         platform_driver_unregister(&sfp_driver);
 965 }
 966 module_exit(sfp_exit);
 967
 968 MODULE_ALIAS("platform:sfp");
 969 MODULE_AUTHOR("Russell King");
 970 MODULE_LICENSE("GPL v2");