target/linux/layerscape/patches-5.4/701-net-0328-net-phy-Inphi-IN112525_s03-retimer-support.patch

   1 From 630ee8f358d961a7c8295d60a112e27cbfe4478d Mon Sep 17 00:00:00 2001
   2 From: Florin Chiculita <florinlaurentiu.chiculita@nxp.com>
   3 Date: Fri, 9 Nov 2018 06:20:36 +0200
   4 Subject: [PATCH] net/phy: Inphi IN112525_s03 retimer support
   5
   6 Software controller for IN112525_s03 retimer
   7
   8 Signed-off-by: Florin Chiculita <florinlaurentiu.chiculita@nxp.com>
   9 ---
  10  drivers/net/phy/Kconfig  |   5 +
  11  drivers/net/phy/Makefile |   1 +
  12  drivers/net/phy/inphi.c  | 578 +++++++++++++++++++++++++++++++++++++++++++++++
  13  3 files changed, 584 insertions(+)
  14  create mode 100644 drivers/net/phy/inphi.c
  15
  16 --- a/drivers/net/phy/Kconfig
  17 +++ b/drivers/net/phy/Kconfig
  18 @@ -479,6 +479,11 @@ config ICPLUS_PHY
  19         ---help---
  20           Currently supports the IP175C and IP1001 PHYs.
  21
  22 +config INPHI_PHY
  23 +       tristate "Inphi CDR 10G/25G Ethernet PHY"
  24 +       ---help---
  25 +         Currently supports the IN112525_S03 part @ 25G
  26 +
  27  config INTEL_XWAY_PHY
  28         tristate "Intel XWAY PHYs"
  29         ---help---
  30 --- a/drivers/net/phy/Makefile
  31 +++ b/drivers/net/phy/Makefile
  32 @@ -88,6 +88,7 @@ obj-$(CONFIG_DP83848_PHY)     += dp83848.o
  33  obj-$(CONFIG_DP83867_PHY)      += dp83867.o
  34  obj-$(CONFIG_FIXED_PHY)                += fixed_phy.o
  35  obj-$(CONFIG_ICPLUS_PHY)       += icplus.o
  36 +obj-$(CONFIG_INPHI_PHY)        += inphi.o
  37  obj-$(CONFIG_INTEL_XWAY_PHY)   += intel-xway.o
  38  obj-$(CONFIG_LSI_ET1011C_PHY)  += et1011c.o
  39  obj-$(CONFIG_LXT_PHY)          += lxt.o
  40 --- /dev/null
  41 +++ b/drivers/net/phy/inphi.c
  42 @@ -0,0 +1,578 @@
  43 +/*
  44 + * Copyright 2018 NXP
  45 + * Copyright 2018 INPHI
  46 + *
  47 + * Redistribution and use in source and binary forms, with or without
  48 + * modification, are permitted provided that the following conditions are met:
  49 + *
  50 + * 1. Redistributions of source code must retain the above copyright notice,
  51 + * this list of conditions and the following disclaimer.
  52 + * 2. Redistributions in binary form must reproduce the above copyright notice,
  53 + * this list of conditions and the following disclaimer in the documentation
  54 + * and/or other materials provided with the distribution.
  55 + *
  56 + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  57 + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  58 + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  59 + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  60 + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  61 + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  62 + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  63 + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  64 + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  65 + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  66 + * POSSIBILITY OF SUCH DAMAGE.
  67 + *
  68 + * Inphi is a registered trademark of Inphi Corporation
  69 + *
  70 + */
  71 +
  72 +#include <linux/module.h>
  73 +#include <linux/phy.h>
  74 +#include <linux/mdio.h>
  75 +#include <linux/interrupt.h>
  76 +#include <linux/platform_device.h>
  77 +#include <linux/of_irq.h>
  78 +#include <linux/workqueue.h>
  79 +#include <linux/i2c.h>
  80 +#include <linux/timer.h>
  81 +#include <linux/delay.h>
  82 +#include <linux/kernel.h>
  83 +#include <linux/init.h>
  84 +#include <linux/fs.h>
  85 +#include <linux/cdev.h>
  86 +#include <linux/device.h>
  87 +#include <linux/slab.h>
  88 +#include <linux/uaccess.h>
  89 +
  90 +#define PHY_ID_IN112525  0x02107440
  91 +
  92 +#define INPHI_S03_DEVICE_ID_MSB 0x2
  93 +#define INPHI_S03_DEVICE_ID_LSB 0x3
  94 +
  95 +#define ALL_LANES              4
  96 +#define INPHI_POLL_DELAY       2500
  97 +
  98 +#define PHYCTRL_REG1   0x0012
  99 +#define PHYCTRL_REG2   0x0014
 100 +#define PHYCTRL_REG3   0x0120
 101 +#define PHYCTRL_REG4   0x0121
 102 +#define PHYCTRL_REG5   0x0180
 103 +#define PHYCTRL_REG6   0x0580
 104 +#define PHYCTRL_REG7   0x05C4
 105 +#define PHYCTRL_REG8   0x01C8
 106 +#define PHYCTRL_REG9   0x0521
 107 +
 108 +#define PHYSTAT_REG1   0x0021
 109 +#define PHYSTAT_REG2   0x0022
 110 +#define PHYSTAT_REG3   0x0123
 111 +
 112 +#define PHYMISC_REG1   0x0025
 113 +#define PHYMISC_REG2   0x002c
 114 +#define PHYMISC_REG3   0x00b3
 115 +#define PHYMISC_REG4   0x0181
 116 +#define PHYMISC_REG5   0x019D
 117 +#define PHYMISC_REG6   0x0198
 118 +#define PHYMISC_REG7   0x0199
 119 +#define PHYMISC_REG8   0x0581
 120 +#define PHYMISC_REG9   0x0598
 121 +#define PHYMISC_REG10  0x059c
 122 +#define PHYMISC_REG20  0x01B0
 123 +#define PHYMISC_REG21  0x01BC
 124 +#define PHYMISC_REG22  0x01C0
 125 +
 126 +#define RX_VCO_CODE_OFFSET     5
 127 +
 128 +#define mdio_wr(a, b)  phy_write_mmd(inphi_phydev, MDIO_MMD_VEND1, (a), (b))
 129 +#define mdio_rd(a)     phy_read_mmd(inphi_phydev, MDIO_MMD_VEND1, (a))
 130 +
 131 +#define VCO_CODE  390
 132 +
 133 +int vco_codes[ALL_LANES] = {
 134 +       VCO_CODE,
 135 +       VCO_CODE,
 136 +       VCO_CODE,
 137 +       VCO_CODE
 138 +};
 139 +
 140 +static void mykmod_work_handler(struct work_struct *w);
 141 +
 142 +static struct workqueue_struct *wq;
 143 +static DECLARE_DELAYED_WORK(mykmod_work, mykmod_work_handler);
 144 +static unsigned long onesec;
 145 +struct phy_device *inphi_phydev;
 146 +
 147 +int bit_test(int value, int bit_field)
 148 +{
 149 +       int result;
 150 +       int bit_mask = (1 << bit_field);
 151 +
 152 +       result = ((value & bit_mask) == bit_mask);
 153 +       return result;
 154 +}
 155 +
 156 +int tx_pll_lock_test(int lane)
 157 +{
 158 +       int i, val, locked = 1;
 159 +
 160 +       if (lane == ALL_LANES) {
 161 +               for (i = 0; i < ALL_LANES; i++) {
 162 +                       val = mdio_rd(i * 0x100 + PHYSTAT_REG3);
 163 +                       locked = locked & bit_test(val, 15);
 164 +               }
 165 +       } else {
 166 +               val = mdio_rd(lane * 0x100 + PHYSTAT_REG3);
 167 +               locked = locked & bit_test(val, 15);
 168 +       }
 169 +
 170 +       return locked;
 171 +}
 172 +
 173 +void rx_reset_assert(int lane)
 174 +{
 175 +       int mask, val;
 176 +
 177 +       if (lane == ALL_LANES) {
 178 +               val = mdio_rd(PHYMISC_REG2);
 179 +               mask = (1 << 15);
 180 +               mdio_wr(PHYMISC_REG2, val + mask);
 181 +       } else {
 182 +               val = mdio_rd(lane * 0x100 + PHYCTRL_REG8);
 183 +               mask = (1 << 6);
 184 +               mdio_wr(lane * 0x100 + PHYCTRL_REG8, val + mask);
 185 +       }
 186 +}
 187 +
 188 +void rx_reset_de_assert(int lane)
 189 +{
 190 +       int mask, val;
 191 +
 192 +       if (lane == ALL_LANES) {
 193 +               val = mdio_rd(PHYMISC_REG2);
 194 +               mask = 0xffff - (1 << 15);
 195 +               mdio_wr(PHYMISC_REG2, val & mask);
 196 +       } else {
 197 +               val = mdio_rd(lane * 0x100 + PHYCTRL_REG8);
 198 +               mask = 0xffff - (1 << 6);
 199 +               mdio_wr(lane * 0x100 + PHYCTRL_REG8, val & mask);
 200 +       }
 201 +}
 202 +
 203 +void rx_powerdown_assert(int lane)
 204 +{
 205 +       int mask, val;
 206 +
 207 +       val = mdio_rd(lane * 0x100 + PHYCTRL_REG8);
 208 +       mask = (1 << 5);
 209 +       mdio_wr(lane * 0x100 + PHYCTRL_REG8, val + mask);
 210 +}
 211 +
 212 +void rx_powerdown_de_assert(int lane)
 213 +{
 214 +       int mask, val;
 215 +
 216 +       val = mdio_rd(lane * 0x100 + PHYCTRL_REG8);
 217 +       mask = 0xffff - (1 << 5);
 218 +       mdio_wr(lane * 0x100 + PHYCTRL_REG8, val & mask);
 219 +}
 220 +
 221 +void tx_pll_assert(int lane)
 222 +{
 223 +       int val, recal;
 224 +
 225 +       if (lane == ALL_LANES) {
 226 +               val = mdio_rd(PHYMISC_REG2);
 227 +               recal = (1 << 12);
 228 +               mdio_wr(PHYMISC_REG2, val | recal);
 229 +       } else {
 230 +               val = mdio_rd(lane * 0x100 + PHYCTRL_REG4);
 231 +               recal = (1 << 15);
 232 +               mdio_wr(lane * 0x100 + PHYCTRL_REG4, val | recal);
 233 +       }
 234 +}
 235 +
 236 +void tx_pll_de_assert(int lane)
 237 +{
 238 +       int recal, val;
 239 +
 240 +       if (lane == ALL_LANES) {
 241 +               val = mdio_rd(PHYMISC_REG2);
 242 +               recal = 0xefff;
 243 +               mdio_wr(PHYMISC_REG2, val & recal);
 244 +       } else {
 245 +               val = mdio_rd(lane * 0x100 + PHYCTRL_REG4);
 246 +               recal = 0x7fff;
 247 +               mdio_wr(lane * 0x100 + PHYCTRL_REG4, val & recal);
 248 +       }
 249 +}
 250 +
 251 +void tx_core_assert(int lane)
 252 +{
 253 +       int recal, val, val2, core_reset;
 254 +
 255 +       if (lane == 4) {
 256 +               val = mdio_rd(PHYMISC_REG2);
 257 +               recal = 1 << 10;
 258 +               mdio_wr(PHYMISC_REG2, val | recal);
 259 +       } else {
 260 +               val2 = mdio_rd(PHYMISC_REG3);
 261 +               core_reset = (1 << (lane + 8));
 262 +               mdio_wr(PHYMISC_REG3, val2 | core_reset);
 263 +       }
 264 +}
 265 +
 266 +void lol_disable(int lane)
 267 +{
 268 +       int val, mask;
 269 +
 270 +       val = mdio_rd(PHYMISC_REG3);
 271 +       mask = 1 << (lane + 4);
 272 +       mdio_wr(PHYMISC_REG3, val | mask);
 273 +}
 274 +
 275 +void tx_core_de_assert(int lane)
 276 +{
 277 +       int val, recal, val2, core_reset;
 278 +
 279 +       if (lane == ALL_LANES) {
 280 +               val = mdio_rd(PHYMISC_REG2);
 281 +               recal = 0xffff - (1 << 10);
 282 +               mdio_wr(PHYMISC_REG2, val & recal);
 283 +       } else {
 284 +               val2 = mdio_rd(PHYMISC_REG3);
 285 +               core_reset = 0xffff - (1 << (lane + 8));
 286 +               mdio_wr(PHYMISC_REG3, val2 & core_reset);
 287 +       }
 288 +}
 289 +
 290 +void tx_restart(int lane)
 291 +{
 292 +       tx_core_assert(lane);
 293 +       tx_pll_assert(lane);
 294 +       tx_pll_de_assert(lane);
 295 +       usleep_range(1500, 1600);
 296 +       tx_core_de_assert(lane);
 297 +}
 298 +
 299 +void disable_lane(int lane)
 300 +{
 301 +       rx_reset_assert(lane);
 302 +       rx_powerdown_assert(lane);
 303 +       tx_core_assert(lane);
 304 +       lol_disable(lane);
 305 +}
 306 +
 307 +void toggle_reset(int lane)
 308 +{
 309 +       int reg, val, orig;
 310 +
 311 +       if (lane == ALL_LANES) {
 312 +               mdio_wr(PHYMISC_REG2, 0x8000);
 313 +               udelay(100);
 314 +               mdio_wr(PHYMISC_REG2, 0x0000);
 315 +       } else {
 316 +               reg = lane * 0x100 + PHYCTRL_REG8;
 317 +               val = (1 << 6);
 318 +               orig = mdio_rd(reg);
 319 +               mdio_wr(reg, orig + val);
 320 +               udelay(100);
 321 +               mdio_wr(reg, orig);
 322 +       }
 323 +}
 324 +
 325 +int az_complete_test(int lane)
 326 +{
 327 +       int success = 1, value;
 328 +
 329 +       if (lane == 0 || lane == ALL_LANES) {
 330 +               value = mdio_rd(PHYCTRL_REG5);
 331 +               success = success & bit_test(value, 2);
 332 +       }
 333 +       if (lane == 1 || lane == ALL_LANES) {
 334 +               value = mdio_rd(PHYCTRL_REG5 + 0x100);
 335 +               success = success & bit_test(value, 2);
 336 +       }
 337 +       if (lane == 2 || lane == ALL_LANES) {
 338 +               value = mdio_rd(PHYCTRL_REG5 + 0x200);
 339 +               success = success & bit_test(value, 2);
 340 +       }
 341 +       if (lane == 3 || lane == ALL_LANES) {
 342 +               value = mdio_rd(PHYCTRL_REG5 + 0x300);
 343 +               success = success & bit_test(value, 2);
 344 +       }
 345 +
 346 +       return success;
 347 +}
 348 +
 349 +void save_az_offsets(int lane)
 350 +{
 351 +       int i;
 352 +
 353 +#define AZ_OFFSET_LANE_UPDATE(reg, lane) \
 354 +       mdio_wr((reg) + (lane) * 0x100,  \
 355 +               (mdio_rd((reg) + (lane) * 0x100) >> 8))
 356 +
 357 +       if (lane == ALL_LANES) {
 358 +               for (i = 0; i < ALL_LANES; i++) {
 359 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20, i);
 360 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 1, i);
 361 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 2, i);
 362 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 3, i);
 363 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21, i);
 364 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 1, i);
 365 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 2, i);
 366 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 3, i);
 367 +                       AZ_OFFSET_LANE_UPDATE(PHYMISC_REG22, i);
 368 +               }
 369 +       } else {
 370 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20, lane);
 371 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 1, lane);
 372 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 2, lane);
 373 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG20 + 3, lane);
 374 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21, lane);
 375 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 1, lane);
 376 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 2, lane);
 377 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG21 + 3, lane);
 378 +               AZ_OFFSET_LANE_UPDATE(PHYMISC_REG22, lane);
 379 +       }
 380 +
 381 +       mdio_wr(PHYCTRL_REG7, 0x0001);
 382 +}
 383 +
 384 +void save_vco_codes(int lane)
 385 +{
 386 +       int i;
 387 +
 388 +       if (lane == ALL_LANES) {
 389 +               for (i = 0; i < ALL_LANES; i++) {
 390 +                       vco_codes[i] = mdio_rd(PHYMISC_REG5 + i * 0x100);
 391 +                       mdio_wr(PHYMISC_REG5 + i * 0x100,
 392 +                               vco_codes[i] + RX_VCO_CODE_OFFSET);
 393 +               }
 394 +       } else {
 395 +               vco_codes[lane] = mdio_rd(PHYMISC_REG5 + lane * 0x100);
 396 +               mdio_wr(PHYMISC_REG5 + lane * 0x100,
 397 +                       vco_codes[lane] + RX_VCO_CODE_OFFSET);
 398 +       }
 399 +}
 400 +
 401 +int inphi_lane_recovery(int lane)
 402 +{
 403 +       int i, value, az_pass;
 404 +
 405 +       switch (lane) {
 406 +       case 0:
 407 +       case 1:
 408 +       case 2:
 409 +       case 3:
 410 +               rx_reset_assert(lane);
 411 +               mdelay(20);
 412 +               break;
 413 +       case ALL_LANES:
 414 +               mdio_wr(PHYMISC_REG2, 0x9C00);
 415 +               mdelay(20);
 416 +               do {
 417 +                       value = mdio_rd(PHYMISC_REG2);
 418 +                       udelay(10);
 419 +               } while (!bit_test(value, 4));
 420 +               break;
 421 +       default:
 422 +               dev_err(&inphi_phydev->mdio.dev,
 423 +                       "Incorrect usage of APIs in %s driver\n",
 424 +                       inphi_phydev->drv->name);
 425 +               break;
 426 +       }
 427 +
 428 +       if (lane == ALL_LANES) {
 429 +               for (i = 0; i < ALL_LANES; i++)
 430 +                       mdio_wr(PHYMISC_REG7 + i * 0x100, VCO_CODE);
 431 +       } else {
 432 +               mdio_wr(PHYMISC_REG7 + lane * 0x100, VCO_CODE);
 433 +       }
 434 +
 435 +       if (lane == ALL_LANES)
 436 +               for (i = 0; i < ALL_LANES; i++)
 437 +                       mdio_wr(PHYCTRL_REG5 + i * 0x100, 0x0418);
 438 +       else
 439 +               mdio_wr(PHYCTRL_REG5 + lane * 0x100, 0x0418);
 440 +
 441 +       mdio_wr(PHYCTRL_REG7,   0x0000);
 442 +
 443 +       rx_reset_de_assert(lane);
 444 +
 445 +       if (lane == ALL_LANES) {
 446 +               for (i = 0; i < ALL_LANES; i++) {
 447 +                       mdio_wr(PHYCTRL_REG5 + i * 0x100, 0x0410);
 448 +                       mdio_wr(PHYCTRL_REG5 + i * 0x100, 0x0412);
 449 +               }
 450 +       } else {
 451 +               mdio_wr(PHYCTRL_REG5 + lane * 0x100, 0x0410);
 452 +               mdio_wr(PHYCTRL_REG5 + lane * 0x100, 0x0412);
 453 +       }
 454 +
 455 +       for (i = 0; i < 64; i++) {
 456 +               mdelay(100);
 457 +               az_pass = az_complete_test(lane);
 458 +               if (az_pass) {
 459 +                       save_az_offsets(lane);
 460 +                       break;
 461 +               }
 462 +       }
 463 +
 464 +       if (!az_pass) {
 465 +               pr_info("in112525: AZ calibration fail @ lane=%d\n", lane);
 466 +               return -1;
 467 +       }
 468 +
 469 +       if (lane == ALL_LANES) {
 470 +               mdio_wr(PHYMISC_REG8, 0x0002);
 471 +               mdio_wr(PHYMISC_REG9, 0x2028);
 472 +               mdio_wr(PHYCTRL_REG6, 0x0010);
 473 +               usleep_range(1000, 1200);
 474 +               mdio_wr(PHYCTRL_REG6, 0x0110);
 475 +               mdelay(30);
 476 +               mdio_wr(PHYMISC_REG9, 0x3020);
 477 +       } else {
 478 +               mdio_wr(PHYMISC_REG4 + lane * 0x100, 0x0002);
 479 +               mdio_wr(PHYMISC_REG6 + lane * 0x100, 0x2028);
 480 +               mdio_wr(PHYCTRL_REG5 + lane * 0x100, 0x0010);
 481 +               usleep_range(1000, 1200);
 482 +               mdio_wr(PHYCTRL_REG5 + lane * 0x100, 0x0110);
 483 +               mdelay(30);
 484 +               mdio_wr(PHYMISC_REG6 + lane * 0x100, 0x3020);
 485 +       }
 486 +
 487 +       if (lane == ALL_LANES) {
 488 +               mdio_wr(PHYMISC_REG2, 0x1C00);
 489 +               mdio_wr(PHYMISC_REG2, 0x0C00);
 490 +       } else {
 491 +               tx_restart(lane);
 492 +               mdelay(11);
 493 +       }
 494 +
 495 +       if (lane == ALL_LANES) {
 496 +               if (bit_test(mdio_rd(PHYMISC_REG2), 6) == 0)
 497 +                       return -1;
 498 +       } else {
 499 +               if (tx_pll_lock_test(lane) == 0)
 500 +                       return -1;
 501 +       }
 502 +
 503 +       save_vco_codes(lane);
 504 +
 505 +       if (lane == ALL_LANES) {
 506 +               mdio_wr(PHYMISC_REG2, 0x0400);
 507 +               mdio_wr(PHYMISC_REG2, 0x0000);
 508 +               value = mdio_rd(PHYCTRL_REG1);
 509 +               value = value & 0xffbf;
 510 +               mdio_wr(PHYCTRL_REG2, value);
 511 +       } else {
 512 +               tx_core_de_assert(lane);
 513 +       }
 514 +
 515 +       if (lane == ALL_LANES) {
 516 +               mdio_wr(PHYMISC_REG1, 0x8000);
 517 +               mdio_wr(PHYMISC_REG1, 0x0000);
 518 +       }
 519 +       mdio_rd(PHYMISC_REG1);
 520 +       mdio_rd(PHYMISC_REG1);
 521 +       usleep_range(1000, 1200);
 522 +       mdio_rd(PHYSTAT_REG1);
 523 +       mdio_rd(PHYSTAT_REG2);
 524 +
 525 +       return 0;
 526 +}
 527 +
 528 +static void mykmod_work_handler(struct work_struct *w)
 529 +{
 530 +       int all_lanes_lock, lane0_lock, lane1_lock, lane2_lock, lane3_lock;
 531 +
 532 +       lane0_lock = bit_test(mdio_rd(0x123), 15);
 533 +       lane1_lock = bit_test(mdio_rd(0x223), 15);
 534 +       lane2_lock = bit_test(mdio_rd(0x323), 15);
 535 +       lane3_lock = bit_test(mdio_rd(0x423), 15);
 536 +
 537 +       /* check if the chip had any successful lane lock from the previous
 538 +        * stage (e.g. u-boot)
 539 +        */
 540 +       all_lanes_lock = lane0_lock | lane1_lock | lane2_lock | lane3_lock;
 541 +
 542 +       if (!all_lanes_lock) {
 543 +               /* start fresh */
 544 +               inphi_lane_recovery(ALL_LANES);
 545 +       } else {
 546 +               if (!lane0_lock)
 547 +                       inphi_lane_recovery(0);
 548 +               if (!lane1_lock)
 549 +                       inphi_lane_recovery(1);
 550 +               if (!lane2_lock)
 551 +                       inphi_lane_recovery(2);
 552 +               if (!lane3_lock)
 553 +                       inphi_lane_recovery(3);
 554 +       }
 555 +
 556 +       queue_delayed_work(wq, &mykmod_work, onesec);
 557 +}
 558 +
 559 +int inphi_probe(struct phy_device *phydev)
 560 +{
 561 +       int phy_id = 0, id_lsb = 0, id_msb = 0;
 562 +
 563 +       /* Read device id from phy registers */
 564 +       id_lsb = phy_read_mmd(phydev, MDIO_MMD_VEND1, INPHI_S03_DEVICE_ID_MSB);
 565 +       if (id_lsb < 0)
 566 +               return -ENXIO;
 567 +
 568 +       phy_id = id_lsb << 16;
 569 +
 570 +       id_msb = phy_read_mmd(phydev, MDIO_MMD_VEND1, INPHI_S03_DEVICE_ID_LSB);
 571 +       if (id_msb < 0)
 572 +               return -ENXIO;
 573 +
 574 +       phy_id |= id_msb;
 575 +
 576 +       /* Make sure the device tree binding matched the driver with the
 577 +        * right device.
 578 +        */
 579 +       if (phy_id != phydev->drv->phy_id) {
 580 +               dev_err(&phydev->mdio.dev,
 581 +                       "Error matching phy with %s driver\n",
 582 +                       phydev->drv->name);
 583 +               return -ENODEV;
 584 +       }
 585 +
 586 +       /* update the local phydev pointer, used inside all APIs */
 587 +       inphi_phydev = phydev;
 588 +       onesec = msecs_to_jiffies(INPHI_POLL_DELAY);
 589 +
 590 +       wq = create_singlethread_workqueue("inphi_kmod");
 591 +       if (wq) {
 592 +               queue_delayed_work(wq, &mykmod_work, onesec);
 593 +       } else {
 594 +               dev_err(&phydev->mdio.dev,
 595 +                       "Error creating kernel workqueue for %s driver\n",
 596 +                       phydev->drv->name);
 597 +               return -ENOMEM;
 598 +       }
 599 +
 600 +       return 0;
 601 +}
 602 +
 603 +static struct phy_driver inphi_driver[] = {
 604 +{
 605 +       .phy_id         = PHY_ID_IN112525,
 606 +       .phy_id_mask    = 0x0ff0fff0,
 607 +       .name           = "Inphi 112525_S03",
 608 +       .features       = PHY_GBIT_FEATURES,
 609 +       .probe          = &inphi_probe,
 610 +},
 611 +};
 612 +
 613 +module_phy_driver(inphi_driver);
 614 +
 615 +static struct mdio_device_id __maybe_unused inphi_tbl[] = {
 616 +       { PHY_ID_IN112525, 0x0ff0fff0},
 617 +       {},
 618 +};
 619 +
 620 +MODULE_DEVICE_TABLE(mdio, inphi_tbl);