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