drivers/net/phy/micrel.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * drivers/net/phy/micrel.c
   4  *
   5  * Driver for Micrel PHYs
   6  *
   7  * Author: David J. Choi
   8  *
   9  * Copyright (c) 2010-2013 Micrel, Inc.
  10  * Copyright (c) 2014 Johan Hovold <johan@kernel.org>
  11  *
  12  * Support : Micrel Phys:
  13  *              Giga phys: ksz9021, ksz9031, ksz9131
  14  *              100/10 Phys : ksz8001, ksz8721, ksz8737, ksz8041
  15  *                         ksz8021, ksz8031, ksz8051,
  16  *                         ksz8081, ksz8091,
  17  *                         ksz8061,
  18  *              Switch : ksz8873, ksz886x
  19  *                       ksz9477
  20  */
  21
  22 #include <linux/bitfield.h>
  23 #include <linux/ethtool_netlink.h>
  24 #include <linux/kernel.h>
  25 #include <linux/module.h>
  26 #include <linux/phy.h>
  27 #include <linux/micrel_phy.h>
  28 #include <linux/of.h>
  29 #include <linux/clk.h>
  30 #include <linux/delay.h>
  31 #include <linux/ptp_clock_kernel.h>
  32 #include <linux/ptp_clock.h>
  33 #include <linux/ptp_classify.h>
  34 #include <linux/net_tstamp.h>
  35 #include <linux/gpio/consumer.h>
  36
  37 /* Operation Mode Strap Override */
  38 #define MII_KSZPHY_OMSO                         0x16
  39 #define KSZPHY_OMSO_FACTORY_TEST                BIT(15)
  40 #define KSZPHY_OMSO_B_CAST_OFF                  BIT(9)
  41 #define KSZPHY_OMSO_NAND_TREE_ON                BIT(5)
  42 #define KSZPHY_OMSO_RMII_OVERRIDE               BIT(1)
  43 #define KSZPHY_OMSO_MII_OVERRIDE                BIT(0)
  44
  45 /* general Interrupt control/status reg in vendor specific block. */
  46 #define MII_KSZPHY_INTCS                        0x1B
  47 #define KSZPHY_INTCS_JABBER                     BIT(15)
  48 #define KSZPHY_INTCS_RECEIVE_ERR                BIT(14)
  49 #define KSZPHY_INTCS_PAGE_RECEIVE               BIT(13)
  50 #define KSZPHY_INTCS_PARELLEL                   BIT(12)
  51 #define KSZPHY_INTCS_LINK_PARTNER_ACK           BIT(11)
  52 #define KSZPHY_INTCS_LINK_DOWN                  BIT(10)
  53 #define KSZPHY_INTCS_REMOTE_FAULT               BIT(9)
  54 #define KSZPHY_INTCS_LINK_UP                    BIT(8)
  55 #define KSZPHY_INTCS_ALL                        (KSZPHY_INTCS_LINK_UP |\
  56                                                 KSZPHY_INTCS_LINK_DOWN)
  57 #define KSZPHY_INTCS_LINK_DOWN_STATUS           BIT(2)
  58 #define KSZPHY_INTCS_LINK_UP_STATUS             BIT(0)
  59 #define KSZPHY_INTCS_STATUS                     (KSZPHY_INTCS_LINK_DOWN_STATUS |\
  60                                                  KSZPHY_INTCS_LINK_UP_STATUS)
  61
  62 /* LinkMD Control/Status */
  63 #define KSZ8081_LMD                             0x1d
  64 #define KSZ8081_LMD_ENABLE_TEST                 BIT(15)
  65 #define KSZ8081_LMD_STAT_NORMAL                 0
  66 #define KSZ8081_LMD_STAT_OPEN                   1
  67 #define KSZ8081_LMD_STAT_SHORT                  2
  68 #define KSZ8081_LMD_STAT_FAIL                   3
  69 #define KSZ8081_LMD_STAT_MASK                   GENMASK(14, 13)
  70 /* Short cable (<10 meter) has been detected by LinkMD */
  71 #define KSZ8081_LMD_SHORT_INDICATOR             BIT(12)
  72 #define KSZ8081_LMD_DELTA_TIME_MASK             GENMASK(8, 0)
  73
  74 #define KSZ9x31_LMD                             0x12
  75 #define KSZ9x31_LMD_VCT_EN                      BIT(15)
  76 #define KSZ9x31_LMD_VCT_DIS_TX                  BIT(14)
  77 #define KSZ9x31_LMD_VCT_PAIR(n)                 (((n) & 0x3) << 12)
  78 #define KSZ9x31_LMD_VCT_SEL_RESULT              0
  79 #define KSZ9x31_LMD_VCT_SEL_THRES_HI            BIT(10)
  80 #define KSZ9x31_LMD_VCT_SEL_THRES_LO            BIT(11)
  81 #define KSZ9x31_LMD_VCT_SEL_MASK                GENMASK(11, 10)
  82 #define KSZ9x31_LMD_VCT_ST_NORMAL               0
  83 #define KSZ9x31_LMD_VCT_ST_OPEN                 1
  84 #define KSZ9x31_LMD_VCT_ST_SHORT                2
  85 #define KSZ9x31_LMD_VCT_ST_FAIL                 3
  86 #define KSZ9x31_LMD_VCT_ST_MASK                 GENMASK(9, 8)
  87 #define KSZ9x31_LMD_VCT_DATA_REFLECTED_INVALID  BIT(7)
  88 #define KSZ9x31_LMD_VCT_DATA_SIG_WAIT_TOO_LONG  BIT(6)
  89 #define KSZ9x31_LMD_VCT_DATA_MASK100            BIT(5)
  90 #define KSZ9x31_LMD_VCT_DATA_NLP_FLP            BIT(4)
  91 #define KSZ9x31_LMD_VCT_DATA_LO_PULSE_MASK      GENMASK(3, 2)
  92 #define KSZ9x31_LMD_VCT_DATA_HI_PULSE_MASK      GENMASK(1, 0)
  93 #define KSZ9x31_LMD_VCT_DATA_MASK               GENMASK(7, 0)
  94
  95 #define KSZPHY_WIRE_PAIR_MASK                   0x3
  96
  97 #define LAN8814_CABLE_DIAG                      0x12
  98 #define LAN8814_CABLE_DIAG_STAT_MASK            GENMASK(9, 8)
  99 #define LAN8814_CABLE_DIAG_VCT_DATA_MASK        GENMASK(7, 0)
 100 #define LAN8814_PAIR_BIT_SHIFT                  12
 101
 102 #define LAN8814_WIRE_PAIR_MASK                  0xF
 103
 104 /* Lan8814 general Interrupt control/status reg in GPHY specific block. */
 105 #define LAN8814_INTC                            0x18
 106 #define LAN8814_INTS                            0x1B
 107
 108 #define LAN8814_INT_LINK_DOWN                   BIT(2)
 109 #define LAN8814_INT_LINK_UP                     BIT(0)
 110 #define LAN8814_INT_LINK                        (LAN8814_INT_LINK_UP |\
 111                                                  LAN8814_INT_LINK_DOWN)
 112
 113 #define LAN8814_INTR_CTRL_REG                   0x34
 114 #define LAN8814_INTR_CTRL_REG_POLARITY          BIT(1)
 115 #define LAN8814_INTR_CTRL_REG_INTR_ENABLE       BIT(0)
 116
 117 /* Represents 1ppm adjustment in 2^32 format with
 118  * each nsec contains 4 clock cycles.
 119  * The value is calculated as following: (1/1000000)/((2^-32)/4)
 120  */
 121 #define LAN8814_1PPM_FORMAT                     17179
 122
 123 #define PTP_RX_MOD                              0x024F
 124 #define PTP_RX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ BIT(3)
 125 #define PTP_RX_TIMESTAMP_EN                     0x024D
 126 #define PTP_TX_TIMESTAMP_EN                     0x028D
 127
 128 #define PTP_TIMESTAMP_EN_SYNC_                  BIT(0)
 129 #define PTP_TIMESTAMP_EN_DREQ_                  BIT(1)
 130 #define PTP_TIMESTAMP_EN_PDREQ_                 BIT(2)
 131 #define PTP_TIMESTAMP_EN_PDRES_                 BIT(3)
 132
 133 #define PTP_TX_PARSE_L2_ADDR_EN                 0x0284
 134 #define PTP_RX_PARSE_L2_ADDR_EN                 0x0244
 135
 136 #define PTP_TX_PARSE_IP_ADDR_EN                 0x0285
 137 #define PTP_RX_PARSE_IP_ADDR_EN                 0x0245
 138 #define LTC_HARD_RESET                          0x023F
 139 #define LTC_HARD_RESET_                         BIT(0)
 140
 141 #define TSU_HARD_RESET                          0x02C1
 142 #define TSU_HARD_RESET_                         BIT(0)
 143
 144 #define PTP_CMD_CTL                             0x0200
 145 #define PTP_CMD_CTL_PTP_DISABLE_                BIT(0)
 146 #define PTP_CMD_CTL_PTP_ENABLE_                 BIT(1)
 147 #define PTP_CMD_CTL_PTP_CLOCK_READ_             BIT(3)
 148 #define PTP_CMD_CTL_PTP_CLOCK_LOAD_             BIT(4)
 149 #define PTP_CMD_CTL_PTP_LTC_STEP_SEC_           BIT(5)
 150 #define PTP_CMD_CTL_PTP_LTC_STEP_NSEC_          BIT(6)
 151
 152 #define PTP_CLOCK_SET_SEC_MID                   0x0206
 153 #define PTP_CLOCK_SET_SEC_LO                    0x0207
 154 #define PTP_CLOCK_SET_NS_HI                     0x0208
 155 #define PTP_CLOCK_SET_NS_LO                     0x0209
 156
 157 #define PTP_CLOCK_READ_SEC_MID                  0x022A
 158 #define PTP_CLOCK_READ_SEC_LO                   0x022B
 159 #define PTP_CLOCK_READ_NS_HI                    0x022C
 160 #define PTP_CLOCK_READ_NS_LO                    0x022D
 161
 162 #define PTP_OPERATING_MODE                      0x0241
 163 #define PTP_OPERATING_MODE_STANDALONE_          BIT(0)
 164
 165 #define PTP_TX_MOD                              0x028F
 166 #define PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_       BIT(12)
 167 #define PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ BIT(3)
 168
 169 #define PTP_RX_PARSE_CONFIG                     0x0242
 170 #define PTP_RX_PARSE_CONFIG_LAYER2_EN_          BIT(0)
 171 #define PTP_RX_PARSE_CONFIG_IPV4_EN_            BIT(1)
 172 #define PTP_RX_PARSE_CONFIG_IPV6_EN_            BIT(2)
 173
 174 #define PTP_TX_PARSE_CONFIG                     0x0282
 175 #define PTP_TX_PARSE_CONFIG_LAYER2_EN_          BIT(0)
 176 #define PTP_TX_PARSE_CONFIG_IPV4_EN_            BIT(1)
 177 #define PTP_TX_PARSE_CONFIG_IPV6_EN_            BIT(2)
 178
 179 #define PTP_CLOCK_RATE_ADJ_HI                   0x020C
 180 #define PTP_CLOCK_RATE_ADJ_LO                   0x020D
 181 #define PTP_CLOCK_RATE_ADJ_DIR_                 BIT(15)
 182
 183 #define PTP_LTC_STEP_ADJ_HI                     0x0212
 184 #define PTP_LTC_STEP_ADJ_LO                     0x0213
 185 #define PTP_LTC_STEP_ADJ_DIR_                   BIT(15)
 186
 187 #define LAN8814_INTR_STS_REG                    0x0033
 188 #define LAN8814_INTR_STS_REG_1588_TSU0_         BIT(0)
 189 #define LAN8814_INTR_STS_REG_1588_TSU1_         BIT(1)
 190 #define LAN8814_INTR_STS_REG_1588_TSU2_         BIT(2)
 191 #define LAN8814_INTR_STS_REG_1588_TSU3_         BIT(3)
 192
 193 #define PTP_CAP_INFO                            0x022A
 194 #define PTP_CAP_INFO_TX_TS_CNT_GET_(reg_val)    (((reg_val) & 0x0f00) >> 8)
 195 #define PTP_CAP_INFO_RX_TS_CNT_GET_(reg_val)    ((reg_val) & 0x000f)
 196
 197 #define PTP_TX_EGRESS_SEC_HI                    0x0296
 198 #define PTP_TX_EGRESS_SEC_LO                    0x0297
 199 #define PTP_TX_EGRESS_NS_HI                     0x0294
 200 #define PTP_TX_EGRESS_NS_LO                     0x0295
 201 #define PTP_TX_MSG_HEADER2                      0x0299
 202
 203 #define PTP_RX_INGRESS_SEC_HI                   0x0256
 204 #define PTP_RX_INGRESS_SEC_LO                   0x0257
 205 #define PTP_RX_INGRESS_NS_HI                    0x0254
 206 #define PTP_RX_INGRESS_NS_LO                    0x0255
 207 #define PTP_RX_MSG_HEADER2                      0x0259
 208
 209 #define PTP_TSU_INT_EN                          0x0200
 210 #define PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN_      BIT(3)
 211 #define PTP_TSU_INT_EN_PTP_TX_TS_EN_            BIT(2)
 212 #define PTP_TSU_INT_EN_PTP_RX_TS_OVRFL_EN_      BIT(1)
 213 #define PTP_TSU_INT_EN_PTP_RX_TS_EN_            BIT(0)
 214
 215 #define PTP_TSU_INT_STS                         0x0201
 216 #define PTP_TSU_INT_STS_PTP_TX_TS_OVRFL_INT_    BIT(3)
 217 #define PTP_TSU_INT_STS_PTP_TX_TS_EN_           BIT(2)
 218 #define PTP_TSU_INT_STS_PTP_RX_TS_OVRFL_INT_    BIT(1)
 219 #define PTP_TSU_INT_STS_PTP_RX_TS_EN_           BIT(0)
 220
 221 #define LAN8814_LED_CTRL_1                      0x0
 222 #define LAN8814_LED_CTRL_1_KSZ9031_LED_MODE_    BIT(6)
 223
 224 /* PHY Control 1 */
 225 #define MII_KSZPHY_CTRL_1                       0x1e
 226 #define KSZ8081_CTRL1_MDIX_STAT                 BIT(4)
 227
 228 /* PHY Control 2 / PHY Control (if no PHY Control 1) */
 229 #define MII_KSZPHY_CTRL_2                       0x1f
 230 #define MII_KSZPHY_CTRL                         MII_KSZPHY_CTRL_2
 231 /* bitmap of PHY register to set interrupt mode */
 232 #define KSZ8081_CTRL2_HP_MDIX                   BIT(15)
 233 #define KSZ8081_CTRL2_MDI_MDI_X_SELECT          BIT(14)
 234 #define KSZ8081_CTRL2_DISABLE_AUTO_MDIX         BIT(13)
 235 #define KSZ8081_CTRL2_FORCE_LINK                BIT(11)
 236 #define KSZ8081_CTRL2_POWER_SAVING              BIT(10)
 237 #define KSZPHY_CTRL_INT_ACTIVE_HIGH             BIT(9)
 238 #define KSZPHY_RMII_REF_CLK_SEL                 BIT(7)
 239
 240 /* Write/read to/from extended registers */
 241 #define MII_KSZPHY_EXTREG                       0x0b
 242 #define KSZPHY_EXTREG_WRITE                     0x8000
 243
 244 #define MII_KSZPHY_EXTREG_WRITE                 0x0c
 245 #define MII_KSZPHY_EXTREG_READ                  0x0d
 246
 247 /* Extended registers */
 248 #define MII_KSZPHY_CLK_CONTROL_PAD_SKEW         0x104
 249 #define MII_KSZPHY_RX_DATA_PAD_SKEW             0x105
 250 #define MII_KSZPHY_TX_DATA_PAD_SKEW             0x106
 251
 252 #define PS_TO_REG                               200
 253 #define FIFO_SIZE                               8
 254
 255 struct kszphy_hw_stat {
 256         const char *string;
 257         u8 reg;
 258         u8 bits;
 259 };
 260
 261 static struct kszphy_hw_stat kszphy_hw_stats[] = {
 262         { "phy_receive_errors", 21, 16},
 263         { "phy_idle_errors", 10, 8 },
 264 };
 265
 266 struct kszphy_type {
 267         u32 led_mode_reg;
 268         u16 interrupt_level_mask;
 269         u16 cable_diag_reg;
 270         unsigned long pair_mask;
 271         u16 disable_dll_tx_bit;
 272         u16 disable_dll_rx_bit;
 273         u16 disable_dll_mask;
 274         bool has_broadcast_disable;
 275         bool has_nand_tree_disable;
 276         bool has_rmii_ref_clk_sel;
 277 };
 278
 279 /* Shared structure between the PHYs of the same package. */
 280 struct lan8814_shared_priv {
 281         struct phy_device *phydev;
 282         struct ptp_clock *ptp_clock;
 283         struct ptp_clock_info ptp_clock_info;
 284
 285         /* Reference counter to how many ports in the package are enabling the
 286          * timestamping
 287          */
 288         u8 ref;
 289
 290         /* Lock for ptp_clock and ref */
 291         struct mutex shared_lock;
 292 };
 293
 294 struct lan8814_ptp_rx_ts {
 295         struct list_head list;
 296         u32 seconds;
 297         u32 nsec;
 298         u16 seq_id;
 299 };
 300
 301 struct kszphy_ptp_priv {
 302         struct mii_timestamper mii_ts;
 303         struct phy_device *phydev;
 304
 305         struct sk_buff_head tx_queue;
 306         struct sk_buff_head rx_queue;
 307
 308         struct list_head rx_ts_list;
 309         /* Lock for Rx ts fifo */
 310         spinlock_t rx_ts_lock;
 311
 312         int hwts_tx_type;
 313         enum hwtstamp_rx_filters rx_filter;
 314         int layer;
 315         int version;
 316
 317         struct ptp_clock *ptp_clock;
 318         struct ptp_clock_info ptp_clock_info;
 319         /* Lock for ptp_clock */
 320         struct mutex ptp_lock;
 321 };
 322
 323 struct kszphy_priv {
 324         struct kszphy_ptp_priv ptp_priv;
 325         const struct kszphy_type *type;
 326         int led_mode;
 327         u16 vct_ctrl1000;
 328         bool rmii_ref_clk_sel;
 329         bool rmii_ref_clk_sel_val;
 330         u64 stats[ARRAY_SIZE(kszphy_hw_stats)];
 331 };
 332
 333 static const struct kszphy_type lan8814_type = {
 334         .led_mode_reg           = ~LAN8814_LED_CTRL_1,
 335         .cable_diag_reg         = LAN8814_CABLE_DIAG,
 336         .pair_mask              = LAN8814_WIRE_PAIR_MASK,
 337 };
 338
 339 static const struct kszphy_type ksz886x_type = {
 340         .cable_diag_reg         = KSZ8081_LMD,
 341         .pair_mask              = KSZPHY_WIRE_PAIR_MASK,
 342 };
 343
 344 static const struct kszphy_type ksz8021_type = {
 345         .led_mode_reg           = MII_KSZPHY_CTRL_2,
 346         .has_broadcast_disable  = true,
 347         .has_nand_tree_disable  = true,
 348         .has_rmii_ref_clk_sel   = true,
 349 };
 350
 351 static const struct kszphy_type ksz8041_type = {
 352         .led_mode_reg           = MII_KSZPHY_CTRL_1,
 353 };
 354
 355 static const struct kszphy_type ksz8051_type = {
 356         .led_mode_reg           = MII_KSZPHY_CTRL_2,
 357         .has_nand_tree_disable  = true,
 358 };
 359
 360 static const struct kszphy_type ksz8081_type = {
 361         .led_mode_reg           = MII_KSZPHY_CTRL_2,
 362         .has_broadcast_disable  = true,
 363         .has_nand_tree_disable  = true,
 364         .has_rmii_ref_clk_sel   = true,
 365 };
 366
 367 static const struct kszphy_type ks8737_type = {
 368         .interrupt_level_mask   = BIT(14),
 369 };
 370
 371 static const struct kszphy_type ksz9021_type = {
 372         .interrupt_level_mask   = BIT(14),
 373 };
 374
 375 static const struct kszphy_type ksz9131_type = {
 376         .interrupt_level_mask   = BIT(14),
 377         .disable_dll_tx_bit     = BIT(12),
 378         .disable_dll_rx_bit     = BIT(12),
 379         .disable_dll_mask       = BIT_MASK(12),
 380 };
 381
 382 static const struct kszphy_type lan8841_type = {
 383         .disable_dll_tx_bit     = BIT(14),
 384         .disable_dll_rx_bit     = BIT(14),
 385         .disable_dll_mask       = BIT_MASK(14),
 386         .cable_diag_reg         = LAN8814_CABLE_DIAG,
 387         .pair_mask              = LAN8814_WIRE_PAIR_MASK,
 388 };
 389
 390 static int kszphy_extended_write(struct phy_device *phydev,
 391                                 u32 regnum, u16 val)
 392 {
 393         phy_write(phydev, MII_KSZPHY_EXTREG, KSZPHY_EXTREG_WRITE | regnum);
 394         return phy_write(phydev, MII_KSZPHY_EXTREG_WRITE, val);
 395 }
 396
 397 static int kszphy_extended_read(struct phy_device *phydev,
 398                                 u32 regnum)
 399 {
 400         phy_write(phydev, MII_KSZPHY_EXTREG, regnum);
 401         return phy_read(phydev, MII_KSZPHY_EXTREG_READ);
 402 }
 403
 404 static int kszphy_ack_interrupt(struct phy_device *phydev)
 405 {
 406         /* bit[7..0] int status, which is a read and clear register. */
 407         int rc;
 408
 409         rc = phy_read(phydev, MII_KSZPHY_INTCS);
 410
 411         return (rc < 0) ? rc : 0;
 412 }
 413
 414 static int kszphy_config_intr(struct phy_device *phydev)
 415 {
 416         const struct kszphy_type *type = phydev->drv->driver_data;
 417         int temp, err;
 418         u16 mask;
 419
 420         if (type && type->interrupt_level_mask)
 421                 mask = type->interrupt_level_mask;
 422         else
 423                 mask = KSZPHY_CTRL_INT_ACTIVE_HIGH;
 424
 425         /* set the interrupt pin active low */
 426         temp = phy_read(phydev, MII_KSZPHY_CTRL);
 427         if (temp < 0)
 428                 return temp;
 429         temp &= ~mask;
 430         phy_write(phydev, MII_KSZPHY_CTRL, temp);
 431
 432         /* enable / disable interrupts */
 433         if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
 434                 err = kszphy_ack_interrupt(phydev);
 435                 if (err)
 436                         return err;
 437
 438                 temp = KSZPHY_INTCS_ALL;
 439                 err = phy_write(phydev, MII_KSZPHY_INTCS, temp);
 440         } else {
 441                 temp = 0;
 442                 err = phy_write(phydev, MII_KSZPHY_INTCS, temp);
 443                 if (err)
 444                         return err;
 445
 446                 err = kszphy_ack_interrupt(phydev);
 447         }
 448
 449         return err;
 450 }
 451
 452 static irqreturn_t kszphy_handle_interrupt(struct phy_device *phydev)
 453 {
 454         int irq_status;
 455
 456         irq_status = phy_read(phydev, MII_KSZPHY_INTCS);
 457         if (irq_status < 0) {
 458                 phy_error(phydev);
 459                 return IRQ_NONE;
 460         }
 461
 462         if (!(irq_status & KSZPHY_INTCS_STATUS))
 463                 return IRQ_NONE;
 464
 465         phy_trigger_machine(phydev);
 466
 467         return IRQ_HANDLED;
 468 }
 469
 470 static int kszphy_rmii_clk_sel(struct phy_device *phydev, bool val)
 471 {
 472         int ctrl;
 473
 474         ctrl = phy_read(phydev, MII_KSZPHY_CTRL);
 475         if (ctrl < 0)
 476                 return ctrl;
 477
 478         if (val)
 479                 ctrl |= KSZPHY_RMII_REF_CLK_SEL;
 480         else
 481                 ctrl &= ~KSZPHY_RMII_REF_CLK_SEL;
 482
 483         return phy_write(phydev, MII_KSZPHY_CTRL, ctrl);
 484 }
 485
 486 static int kszphy_setup_led(struct phy_device *phydev, u32 reg, int val)
 487 {
 488         int rc, temp, shift;
 489
 490         switch (reg) {
 491         case MII_KSZPHY_CTRL_1:
 492                 shift = 14;
 493                 break;
 494         case MII_KSZPHY_CTRL_2:
 495                 shift = 4;
 496                 break;
 497         default:
 498                 return -EINVAL;
 499         }
 500
 501         temp = phy_read(phydev, reg);
 502         if (temp < 0) {
 503                 rc = temp;
 504                 goto out;
 505         }
 506
 507         temp &= ~(3 << shift);
 508         temp |= val << shift;
 509         rc = phy_write(phydev, reg, temp);
 510 out:
 511         if (rc < 0)
 512                 phydev_err(phydev, "failed to set led mode\n");
 513
 514         return rc;
 515 }
 516
 517 /* Disable PHY address 0 as the broadcast address, so that it can be used as a
 518  * unique (non-broadcast) address on a shared bus.
 519  */
 520 static int kszphy_broadcast_disable(struct phy_device *phydev)
 521 {
 522         int ret;
 523
 524         ret = phy_read(phydev, MII_KSZPHY_OMSO);
 525         if (ret < 0)
 526                 goto out;
 527
 528         ret = phy_write(phydev, MII_KSZPHY_OMSO, ret | KSZPHY_OMSO_B_CAST_OFF);
 529 out:
 530         if (ret)
 531                 phydev_err(phydev, "failed to disable broadcast address\n");
 532
 533         return ret;
 534 }
 535
 536 static int kszphy_nand_tree_disable(struct phy_device *phydev)
 537 {
 538         int ret;
 539
 540         ret = phy_read(phydev, MII_KSZPHY_OMSO);
 541         if (ret < 0)
 542                 goto out;
 543
 544         if (!(ret & KSZPHY_OMSO_NAND_TREE_ON))
 545                 return 0;
 546
 547         ret = phy_write(phydev, MII_KSZPHY_OMSO,
 548                         ret & ~KSZPHY_OMSO_NAND_TREE_ON);
 549 out:
 550         if (ret)
 551                 phydev_err(phydev, "failed to disable NAND tree mode\n");
 552
 553         return ret;
 554 }
 555
 556 /* Some config bits need to be set again on resume, handle them here. */
 557 static int kszphy_config_reset(struct phy_device *phydev)
 558 {
 559         struct kszphy_priv *priv = phydev->priv;
 560         int ret;
 561
 562         if (priv->rmii_ref_clk_sel) {
 563                 ret = kszphy_rmii_clk_sel(phydev, priv->rmii_ref_clk_sel_val);
 564                 if (ret) {
 565                         phydev_err(phydev,
 566                                    "failed to set rmii reference clock\n");
 567                         return ret;
 568                 }
 569         }
 570
 571         if (priv->type && priv->led_mode >= 0)
 572                 kszphy_setup_led(phydev, priv->type->led_mode_reg, priv->led_mode);
 573
 574         return 0;
 575 }
 576
 577 static int kszphy_config_init(struct phy_device *phydev)
 578 {
 579         struct kszphy_priv *priv = phydev->priv;
 580         const struct kszphy_type *type;
 581
 582         if (!priv)
 583                 return 0;
 584
 585         type = priv->type;
 586
 587         if (type && type->has_broadcast_disable)
 588                 kszphy_broadcast_disable(phydev);
 589
 590         if (type && type->has_nand_tree_disable)
 591                 kszphy_nand_tree_disable(phydev);
 592
 593         return kszphy_config_reset(phydev);
 594 }
 595
 596 static int ksz8041_fiber_mode(struct phy_device *phydev)
 597 {
 598         struct device_node *of_node = phydev->mdio.dev.of_node;
 599
 600         return of_property_read_bool(of_node, "micrel,fiber-mode");
 601 }
 602
 603 static int ksz8041_config_init(struct phy_device *phydev)
 604 {
 605         __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
 606
 607         /* Limit supported and advertised modes in fiber mode */
 608         if (ksz8041_fiber_mode(phydev)) {
 609                 phydev->dev_flags |= MICREL_PHY_FXEN;
 610                 linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT, mask);
 611                 linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT, mask);
 612
 613                 linkmode_and(phydev->supported, phydev->supported, mask);
 614                 linkmode_set_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
 615                                  phydev->supported);
 616                 linkmode_and(phydev->advertising, phydev->advertising, mask);
 617                 linkmode_set_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
 618                                  phydev->advertising);
 619                 phydev->autoneg = AUTONEG_DISABLE;
 620         }
 621
 622         return kszphy_config_init(phydev);
 623 }
 624
 625 static int ksz8041_config_aneg(struct phy_device *phydev)
 626 {
 627         /* Skip auto-negotiation in fiber mode */
 628         if (phydev->dev_flags & MICREL_PHY_FXEN) {
 629                 phydev->speed = SPEED_100;
 630                 return 0;
 631         }
 632
 633         return genphy_config_aneg(phydev);
 634 }
 635
 636 static int ksz8051_ksz8795_match_phy_device(struct phy_device *phydev,
 637                                             const bool ksz_8051)
 638 {
 639         int ret;
 640
 641         if ((phydev->phy_id & MICREL_PHY_ID_MASK) != PHY_ID_KSZ8051)
 642                 return 0;
 643
 644         ret = phy_read(phydev, MII_BMSR);
 645         if (ret < 0)
 646                 return ret;
 647
 648         /* KSZ8051 PHY and KSZ8794/KSZ8795/KSZ8765 switch share the same
 649          * exact PHY ID. However, they can be told apart by the extended
 650          * capability registers presence. The KSZ8051 PHY has them while
 651          * the switch does not.
 652          */
 653         ret &= BMSR_ERCAP;
 654         if (ksz_8051)
 655                 return ret;
 656         else
 657                 return !ret;
 658 }
 659
 660 static int ksz8051_match_phy_device(struct phy_device *phydev)
 661 {
 662         return ksz8051_ksz8795_match_phy_device(phydev, true);
 663 }
 664
 665 static int ksz8081_config_init(struct phy_device *phydev)
 666 {
 667         /* KSZPHY_OMSO_FACTORY_TEST is set at de-assertion of the reset line
 668          * based on the RXER (KSZ8081RNA/RND) or TXC (KSZ8081MNX/RNB) pin. If a
 669          * pull-down is missing, the factory test mode should be cleared by
 670          * manually writing a 0.
 671          */
 672         phy_clear_bits(phydev, MII_KSZPHY_OMSO, KSZPHY_OMSO_FACTORY_TEST);
 673
 674         return kszphy_config_init(phydev);
 675 }
 676
 677 static int ksz8081_config_mdix(struct phy_device *phydev, u8 ctrl)
 678 {
 679         u16 val;
 680
 681         switch (ctrl) {
 682         case ETH_TP_MDI:
 683                 val = KSZ8081_CTRL2_DISABLE_AUTO_MDIX;
 684                 break;
 685         case ETH_TP_MDI_X:
 686                 val = KSZ8081_CTRL2_DISABLE_AUTO_MDIX |
 687                         KSZ8081_CTRL2_MDI_MDI_X_SELECT;
 688                 break;
 689         case ETH_TP_MDI_AUTO:
 690                 val = 0;
 691                 break;
 692         default:
 693                 return 0;
 694         }
 695
 696         return phy_modify(phydev, MII_KSZPHY_CTRL_2,
 697                           KSZ8081_CTRL2_HP_MDIX |
 698                           KSZ8081_CTRL2_MDI_MDI_X_SELECT |
 699                           KSZ8081_CTRL2_DISABLE_AUTO_MDIX,
 700                           KSZ8081_CTRL2_HP_MDIX | val);
 701 }
 702
 703 static int ksz8081_config_aneg(struct phy_device *phydev)
 704 {
 705         int ret;
 706
 707         ret = genphy_config_aneg(phydev);
 708         if (ret)
 709                 return ret;
 710
 711         /* The MDI-X configuration is automatically changed by the PHY after
 712          * switching from autoneg off to on. So, take MDI-X configuration under
 713          * own control and set it after autoneg configuration was done.
 714          */
 715         return ksz8081_config_mdix(phydev, phydev->mdix_ctrl);
 716 }
 717
 718 static int ksz8081_mdix_update(struct phy_device *phydev)
 719 {
 720         int ret;
 721
 722         ret = phy_read(phydev, MII_KSZPHY_CTRL_2);
 723         if (ret < 0)
 724                 return ret;
 725
 726         if (ret & KSZ8081_CTRL2_DISABLE_AUTO_MDIX) {
 727                 if (ret & KSZ8081_CTRL2_MDI_MDI_X_SELECT)
 728                         phydev->mdix_ctrl = ETH_TP_MDI_X;
 729                 else
 730                         phydev->mdix_ctrl = ETH_TP_MDI;
 731         } else {
 732                 phydev->mdix_ctrl = ETH_TP_MDI_AUTO;
 733         }
 734
 735         ret = phy_read(phydev, MII_KSZPHY_CTRL_1);
 736         if (ret < 0)
 737                 return ret;
 738
 739         if (ret & KSZ8081_CTRL1_MDIX_STAT)
 740                 phydev->mdix = ETH_TP_MDI;
 741         else
 742                 phydev->mdix = ETH_TP_MDI_X;
 743
 744         return 0;
 745 }
 746
 747 static int ksz8081_read_status(struct phy_device *phydev)
 748 {
 749         int ret;
 750
 751         ret = ksz8081_mdix_update(phydev);
 752         if (ret < 0)
 753                 return ret;
 754
 755         return genphy_read_status(phydev);
 756 }
 757
 758 static int ksz8061_config_init(struct phy_device *phydev)
 759 {
 760         int ret;
 761
 762         ret = phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_DEVID1, 0xB61A);
 763         if (ret)
 764                 return ret;
 765
 766         return kszphy_config_init(phydev);
 767 }
 768
 769 static int ksz8795_match_phy_device(struct phy_device *phydev)
 770 {
 771         return ksz8051_ksz8795_match_phy_device(phydev, false);
 772 }
 773
 774 static int ksz9021_load_values_from_of(struct phy_device *phydev,
 775                                        const struct device_node *of_node,
 776                                        u16 reg,
 777                                        const char *field1, const char *field2,
 778                                        const char *field3, const char *field4)
 779 {
 780         int val1 = -1;
 781         int val2 = -2;
 782         int val3 = -3;
 783         int val4 = -4;
 784         int newval;
 785         int matches = 0;
 786
 787         if (!of_property_read_u32(of_node, field1, &val1))
 788                 matches++;
 789
 790         if (!of_property_read_u32(of_node, field2, &val2))
 791                 matches++;
 792
 793         if (!of_property_read_u32(of_node, field3, &val3))
 794                 matches++;
 795
 796         if (!of_property_read_u32(of_node, field4, &val4))
 797                 matches++;
 798
 799         if (!matches)
 800                 return 0;
 801
 802         if (matches < 4)
 803                 newval = kszphy_extended_read(phydev, reg);
 804         else
 805                 newval = 0;
 806
 807         if (val1 != -1)
 808                 newval = ((newval & 0xfff0) | ((val1 / PS_TO_REG) & 0xf) << 0);
 809
 810         if (val2 != -2)
 811                 newval = ((newval & 0xff0f) | ((val2 / PS_TO_REG) & 0xf) << 4);
 812
 813         if (val3 != -3)
 814                 newval = ((newval & 0xf0ff) | ((val3 / PS_TO_REG) & 0xf) << 8);
 815
 816         if (val4 != -4)
 817                 newval = ((newval & 0x0fff) | ((val4 / PS_TO_REG) & 0xf) << 12);
 818
 819         return kszphy_extended_write(phydev, reg, newval);
 820 }
 821
 822 static int ksz9021_config_init(struct phy_device *phydev)
 823 {
 824         const struct device_node *of_node;
 825         const struct device *dev_walker;
 826
 827         /* The Micrel driver has a deprecated option to place phy OF
 828          * properties in the MAC node. Walk up the tree of devices to
 829          * find a device with an OF node.
 830          */
 831         dev_walker = &phydev->mdio.dev;
 832         do {
 833                 of_node = dev_walker->of_node;
 834                 dev_walker = dev_walker->parent;
 835
 836         } while (!of_node && dev_walker);
 837
 838         if (of_node) {
 839                 ksz9021_load_values_from_of(phydev, of_node,
 840                                     MII_KSZPHY_CLK_CONTROL_PAD_SKEW,
 841                                     "txen-skew-ps", "txc-skew-ps",
 842                                     "rxdv-skew-ps", "rxc-skew-ps");
 843                 ksz9021_load_values_from_of(phydev, of_node,
 844                                     MII_KSZPHY_RX_DATA_PAD_SKEW,
 845                                     "rxd0-skew-ps", "rxd1-skew-ps",
 846                                     "rxd2-skew-ps", "rxd3-skew-ps");
 847                 ksz9021_load_values_from_of(phydev, of_node,
 848                                     MII_KSZPHY_TX_DATA_PAD_SKEW,
 849                                     "txd0-skew-ps", "txd1-skew-ps",
 850                                     "txd2-skew-ps", "txd3-skew-ps");
 851         }
 852         return 0;
 853 }
 854
 855 #define KSZ9031_PS_TO_REG               60
 856
 857 /* Extended registers */
 858 /* MMD Address 0x0 */
 859 #define MII_KSZ9031RN_FLP_BURST_TX_LO   3
 860 #define MII_KSZ9031RN_FLP_BURST_TX_HI   4
 861
 862 /* MMD Address 0x2 */
 863 #define MII_KSZ9031RN_CONTROL_PAD_SKEW  4
 864 #define MII_KSZ9031RN_RX_CTL_M          GENMASK(7, 4)
 865 #define MII_KSZ9031RN_TX_CTL_M          GENMASK(3, 0)
 866
 867 #define MII_KSZ9031RN_RX_DATA_PAD_SKEW  5
 868 #define MII_KSZ9031RN_RXD3              GENMASK(15, 12)
 869 #define MII_KSZ9031RN_RXD2              GENMASK(11, 8)
 870 #define MII_KSZ9031RN_RXD1              GENMASK(7, 4)
 871 #define MII_KSZ9031RN_RXD0              GENMASK(3, 0)
 872
 873 #define MII_KSZ9031RN_TX_DATA_PAD_SKEW  6
 874 #define MII_KSZ9031RN_TXD3              GENMASK(15, 12)
 875 #define MII_KSZ9031RN_TXD2              GENMASK(11, 8)
 876 #define MII_KSZ9031RN_TXD1              GENMASK(7, 4)
 877 #define MII_KSZ9031RN_TXD0              GENMASK(3, 0)
 878
 879 #define MII_KSZ9031RN_CLK_PAD_SKEW      8
 880 #define MII_KSZ9031RN_GTX_CLK           GENMASK(9, 5)
 881 #define MII_KSZ9031RN_RX_CLK            GENMASK(4, 0)
 882
 883 /* KSZ9031 has internal RGMII_IDRX = 1.2ns and RGMII_IDTX = 0ns. To
 884  * provide different RGMII options we need to configure delay offset
 885  * for each pad relative to build in delay.
 886  */
 887 /* keep rx as "No delay adjustment" and set rx_clk to +0.60ns to get delays of
 888  * 1.80ns
 889  */
 890 #define RX_ID                           0x7
 891 #define RX_CLK_ID                       0x19
 892
 893 /* set rx to +0.30ns and rx_clk to -0.90ns to compensate the
 894  * internal 1.2ns delay.
 895  */
 896 #define RX_ND                           0xc
 897 #define RX_CLK_ND                       0x0
 898
 899 /* set tx to -0.42ns and tx_clk to +0.96ns to get 1.38ns delay */
 900 #define TX_ID                           0x0
 901 #define TX_CLK_ID                       0x1f
 902
 903 /* set tx and tx_clk to "No delay adjustment" to keep 0ns
 904  * dealy
 905  */
 906 #define TX_ND                           0x7
 907 #define TX_CLK_ND                       0xf
 908
 909 /* MMD Address 0x1C */
 910 #define MII_KSZ9031RN_EDPD              0x23
 911 #define MII_KSZ9031RN_EDPD_ENABLE       BIT(0)
 912
 913 static int ksz9031_of_load_skew_values(struct phy_device *phydev,
 914                                        const struct device_node *of_node,
 915                                        u16 reg, size_t field_sz,
 916                                        const char *field[], u8 numfields,
 917                                        bool *update)
 918 {
 919         int val[4] = {-1, -2, -3, -4};
 920         int matches = 0;
 921         u16 mask;
 922         u16 maxval;
 923         u16 newval;
 924         int i;
 925
 926         for (i = 0; i < numfields; i++)
 927                 if (!of_property_read_u32(of_node, field[i], val + i))
 928                         matches++;
 929
 930         if (!matches)
 931                 return 0;
 932
 933         *update |= true;
 934
 935         if (matches < numfields)
 936                 newval = phy_read_mmd(phydev, 2, reg);
 937         else
 938                 newval = 0;
 939
 940         maxval = (field_sz == 4) ? 0xf : 0x1f;
 941         for (i = 0; i < numfields; i++)
 942                 if (val[i] != -(i + 1)) {
 943                         mask = 0xffff;
 944                         mask ^= maxval << (field_sz * i);
 945                         newval = (newval & mask) |
 946                                 (((val[i] / KSZ9031_PS_TO_REG) & maxval)
 947                                         << (field_sz * i));
 948                 }
 949
 950         return phy_write_mmd(phydev, 2, reg, newval);
 951 }
 952
 953 /* Center KSZ9031RNX FLP timing at 16ms. */
 954 static int ksz9031_center_flp_timing(struct phy_device *phydev)
 955 {
 956         int result;
 957
 958         result = phy_write_mmd(phydev, 0, MII_KSZ9031RN_FLP_BURST_TX_HI,
 959                                0x0006);
 960         if (result)
 961                 return result;
 962
 963         result = phy_write_mmd(phydev, 0, MII_KSZ9031RN_FLP_BURST_TX_LO,
 964                                0x1A80);
 965         if (result)
 966                 return result;
 967
 968         return genphy_restart_aneg(phydev);
 969 }
 970
 971 /* Enable energy-detect power-down mode */
 972 static int ksz9031_enable_edpd(struct phy_device *phydev)
 973 {
 974         int reg;
 975
 976         reg = phy_read_mmd(phydev, 0x1C, MII_KSZ9031RN_EDPD);
 977         if (reg < 0)
 978                 return reg;
 979         return phy_write_mmd(phydev, 0x1C, MII_KSZ9031RN_EDPD,
 980                              reg | MII_KSZ9031RN_EDPD_ENABLE);
 981 }
 982
 983 static int ksz9031_config_rgmii_delay(struct phy_device *phydev)
 984 {
 985         u16 rx, tx, rx_clk, tx_clk;
 986         int ret;
 987
 988         switch (phydev->interface) {
 989         case PHY_INTERFACE_MODE_RGMII:
 990                 tx = TX_ND;
 991                 tx_clk = TX_CLK_ND;
 992                 rx = RX_ND;
 993                 rx_clk = RX_CLK_ND;
 994                 break;
 995         case PHY_INTERFACE_MODE_RGMII_ID:
 996                 tx = TX_ID;
 997                 tx_clk = TX_CLK_ID;
 998                 rx = RX_ID;
 999                 rx_clk = RX_CLK_ID;
1000                 break;
1001         case PHY_INTERFACE_MODE_RGMII_RXID:
1002                 tx = TX_ND;
1003                 tx_clk = TX_CLK_ND;
1004                 rx = RX_ID;
1005                 rx_clk = RX_CLK_ID;
1006                 break;
1007         case PHY_INTERFACE_MODE_RGMII_TXID:
1008                 tx = TX_ID;
1009                 tx_clk = TX_CLK_ID;
1010                 rx = RX_ND;
1011                 rx_clk = RX_CLK_ND;
1012                 break;
1013         default:
1014                 return 0;
1015         }
1016
1017         ret = phy_write_mmd(phydev, 2, MII_KSZ9031RN_CONTROL_PAD_SKEW,
1018                             FIELD_PREP(MII_KSZ9031RN_RX_CTL_M, rx) |
1019                             FIELD_PREP(MII_KSZ9031RN_TX_CTL_M, tx));
1020         if (ret < 0)
1021                 return ret;
1022
1023         ret = phy_write_mmd(phydev, 2, MII_KSZ9031RN_RX_DATA_PAD_SKEW,
1024                             FIELD_PREP(MII_KSZ9031RN_RXD3, rx) |
1025                             FIELD_PREP(MII_KSZ9031RN_RXD2, rx) |
1026                             FIELD_PREP(MII_KSZ9031RN_RXD1, rx) |
1027                             FIELD_PREP(MII_KSZ9031RN_RXD0, rx));
1028         if (ret < 0)
1029                 return ret;
1030
1031         ret = phy_write_mmd(phydev, 2, MII_KSZ9031RN_TX_DATA_PAD_SKEW,
1032                             FIELD_PREP(MII_KSZ9031RN_TXD3, tx) |
1033                             FIELD_PREP(MII_KSZ9031RN_TXD2, tx) |
1034                             FIELD_PREP(MII_KSZ9031RN_TXD1, tx) |
1035                             FIELD_PREP(MII_KSZ9031RN_TXD0, tx));
1036         if (ret < 0)
1037                 return ret;
1038
1039         return phy_write_mmd(phydev, 2, MII_KSZ9031RN_CLK_PAD_SKEW,
1040                              FIELD_PREP(MII_KSZ9031RN_GTX_CLK, tx_clk) |
1041                              FIELD_PREP(MII_KSZ9031RN_RX_CLK, rx_clk));
1042 }
1043
1044 static int ksz9031_config_init(struct phy_device *phydev)
1045 {
1046         const struct device_node *of_node;
1047         static const char *clk_skews[2] = {"rxc-skew-ps", "txc-skew-ps"};
1048         static const char *rx_data_skews[4] = {
1049                 "rxd0-skew-ps", "rxd1-skew-ps",
1050                 "rxd2-skew-ps", "rxd3-skew-ps"
1051         };
1052         static const char *tx_data_skews[4] = {
1053                 "txd0-skew-ps", "txd1-skew-ps",
1054                 "txd2-skew-ps", "txd3-skew-ps"
1055         };
1056         static const char *control_skews[2] = {"txen-skew-ps", "rxdv-skew-ps"};
1057         const struct device *dev_walker;
1058         int result;
1059
1060         result = ksz9031_enable_edpd(phydev);
1061         if (result < 0)
1062                 return result;
1063
1064         /* The Micrel driver has a deprecated option to place phy OF
1065          * properties in the MAC node. Walk up the tree of devices to
1066          * find a device with an OF node.
1067          */
1068         dev_walker = &phydev->mdio.dev;
1069         do {
1070                 of_node = dev_walker->of_node;
1071                 dev_walker = dev_walker->parent;
1072         } while (!of_node && dev_walker);
1073
1074         if (of_node) {
1075                 bool update = false;
1076
1077                 if (phy_interface_is_rgmii(phydev)) {
1078                         result = ksz9031_config_rgmii_delay(phydev);
1079                         if (result < 0)
1080                                 return result;
1081                 }
1082
1083                 ksz9031_of_load_skew_values(phydev, of_node,
1084                                 MII_KSZ9031RN_CLK_PAD_SKEW, 5,
1085                                 clk_skews, 2, &update);
1086
1087                 ksz9031_of_load_skew_values(phydev, of_node,
1088                                 MII_KSZ9031RN_CONTROL_PAD_SKEW, 4,
1089                                 control_skews, 2, &update);
1090
1091                 ksz9031_of_load_skew_values(phydev, of_node,
1092                                 MII_KSZ9031RN_RX_DATA_PAD_SKEW, 4,
1093                                 rx_data_skews, 4, &update);
1094
1095                 ksz9031_of_load_skew_values(phydev, of_node,
1096                                 MII_KSZ9031RN_TX_DATA_PAD_SKEW, 4,
1097                                 tx_data_skews, 4, &update);
1098
1099                 if (update && !phy_interface_is_rgmii(phydev))
1100                         phydev_warn(phydev,
1101                                     "*-skew-ps values should be used only with RGMII PHY modes\n");
1102
1103                 /* Silicon Errata Sheet (DS80000691D or DS80000692D):
1104                  * When the device links in the 1000BASE-T slave mode only,
1105                  * the optional 125MHz reference output clock (CLK125_NDO)
1106                  * has wide duty cycle variation.
1107                  *
1108                  * The optional CLK125_NDO clock does not meet the RGMII
1109                  * 45/55 percent (min/max) duty cycle requirement and therefore
1110                  * cannot be used directly by the MAC side for clocking
1111                  * applications that have setup/hold time requirements on
1112                  * rising and falling clock edges.
1113                  *
1114                  * Workaround:
1115                  * Force the phy to be the master to receive a stable clock
1116                  * which meets the duty cycle requirement.
1117                  */
1118                 if (of_property_read_bool(of_node, "micrel,force-master")) {
1119                         result = phy_read(phydev, MII_CTRL1000);
1120                         if (result < 0)
1121                                 goto err_force_master;
1122
1123                         /* enable master mode, config & prefer master */
1124                         result |= CTL1000_ENABLE_MASTER | CTL1000_AS_MASTER;
1125                         result = phy_write(phydev, MII_CTRL1000, result);
1126                         if (result < 0)
1127                                 goto err_force_master;
1128                 }
1129         }
1130
1131         return ksz9031_center_flp_timing(phydev);
1132
1133 err_force_master:
1134         phydev_err(phydev, "failed to force the phy to master mode\n");
1135         return result;
1136 }
1137
1138 #define KSZ9131_SKEW_5BIT_MAX   2400
1139 #define KSZ9131_SKEW_4BIT_MAX   800
1140 #define KSZ9131_OFFSET          700
1141 #define KSZ9131_STEP            100
1142
1143 static int ksz9131_of_load_skew_values(struct phy_device *phydev,
1144                                        struct device_node *of_node,
1145                                        u16 reg, size_t field_sz,
1146                                        char *field[], u8 numfields)
1147 {
1148         int val[4] = {-(1 + KSZ9131_OFFSET), -(2 + KSZ9131_OFFSET),
1149                       -(3 + KSZ9131_OFFSET), -(4 + KSZ9131_OFFSET)};
1150         int skewval, skewmax = 0;
1151         int matches = 0;
1152         u16 maxval;
1153         u16 newval;
1154         u16 mask;
1155         int i;
1156
1157         /* psec properties in dts should mean x pico seconds */
1158         if (field_sz == 5)
1159                 skewmax = KSZ9131_SKEW_5BIT_MAX;
1160         else
1161                 skewmax = KSZ9131_SKEW_4BIT_MAX;
1162
1163         for (i = 0; i < numfields; i++)
1164                 if (!of_property_read_s32(of_node, field[i], &skewval)) {
1165                         if (skewval < -KSZ9131_OFFSET)
1166                                 skewval = -KSZ9131_OFFSET;
1167                         else if (skewval > skewmax)
1168                                 skewval = skewmax;
1169
1170                         val[i] = skewval + KSZ9131_OFFSET;
1171                         matches++;
1172                 }
1173
1174         if (!matches)
1175                 return 0;
1176
1177         if (matches < numfields)
1178                 newval = phy_read_mmd(phydev, 2, reg);
1179         else
1180                 newval = 0;
1181
1182         maxval = (field_sz == 4) ? 0xf : 0x1f;
1183         for (i = 0; i < numfields; i++)
1184                 if (val[i] != -(i + 1 + KSZ9131_OFFSET)) {
1185                         mask = 0xffff;
1186                         mask ^= maxval << (field_sz * i);
1187                         newval = (newval & mask) |
1188                                 (((val[i] / KSZ9131_STEP) & maxval)
1189                                         << (field_sz * i));
1190                 }
1191
1192         return phy_write_mmd(phydev, 2, reg, newval);
1193 }
1194
1195 #define KSZ9131RN_MMD_COMMON_CTRL_REG   2
1196 #define KSZ9131RN_RXC_DLL_CTRL          76
1197 #define KSZ9131RN_TXC_DLL_CTRL          77
1198 #define KSZ9131RN_DLL_ENABLE_DELAY      0
1199
1200 static int ksz9131_config_rgmii_delay(struct phy_device *phydev)
1201 {
1202         const struct kszphy_type *type = phydev->drv->driver_data;
1203         u16 rxcdll_val, txcdll_val;
1204         int ret;
1205
1206         switch (phydev->interface) {
1207         case PHY_INTERFACE_MODE_RGMII:
1208                 rxcdll_val = type->disable_dll_rx_bit;
1209                 txcdll_val = type->disable_dll_tx_bit;
1210                 break;
1211         case PHY_INTERFACE_MODE_RGMII_ID:
1212                 rxcdll_val = KSZ9131RN_DLL_ENABLE_DELAY;
1213                 txcdll_val = KSZ9131RN_DLL_ENABLE_DELAY;
1214                 break;
1215         case PHY_INTERFACE_MODE_RGMII_RXID:
1216                 rxcdll_val = KSZ9131RN_DLL_ENABLE_DELAY;
1217                 txcdll_val = type->disable_dll_tx_bit;
1218                 break;
1219         case PHY_INTERFACE_MODE_RGMII_TXID:
1220                 rxcdll_val = type->disable_dll_rx_bit;
1221                 txcdll_val = KSZ9131RN_DLL_ENABLE_DELAY;
1222                 break;
1223         default:
1224                 return 0;
1225         }
1226
1227         ret = phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
1228                              KSZ9131RN_RXC_DLL_CTRL, type->disable_dll_mask,
1229                              rxcdll_val);
1230         if (ret < 0)
1231                 return ret;
1232
1233         return phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
1234                               KSZ9131RN_TXC_DLL_CTRL, type->disable_dll_mask,
1235                               txcdll_val);
1236 }
1237
1238 /* Silicon Errata DS80000693B
1239  *
1240  * When LEDs are configured in Individual Mode, LED1 is ON in a no-link
1241  * condition. Workaround is to set register 0x1e, bit 9, this way LED1 behaves
1242  * according to the datasheet (off if there is no link).
1243  */
1244 static int ksz9131_led_errata(struct phy_device *phydev)
1245 {
1246         int reg;
1247
1248         reg = phy_read_mmd(phydev, 2, 0);
1249         if (reg < 0)
1250                 return reg;
1251
1252         if (!(reg & BIT(4)))
1253                 return 0;
1254
1255         return phy_set_bits(phydev, 0x1e, BIT(9));
1256 }
1257
1258 static int ksz9131_config_init(struct phy_device *phydev)
1259 {
1260         struct device_node *of_node;
1261         char *clk_skews[2] = {"rxc-skew-psec", "txc-skew-psec"};
1262         char *rx_data_skews[4] = {
1263                 "rxd0-skew-psec", "rxd1-skew-psec",
1264                 "rxd2-skew-psec", "rxd3-skew-psec"
1265         };
1266         char *tx_data_skews[4] = {
1267                 "txd0-skew-psec", "txd1-skew-psec",
1268                 "txd2-skew-psec", "txd3-skew-psec"
1269         };
1270         char *control_skews[2] = {"txen-skew-psec", "rxdv-skew-psec"};
1271         const struct device *dev_walker;
1272         int ret;
1273
1274         dev_walker = &phydev->mdio.dev;
1275         do {
1276                 of_node = dev_walker->of_node;
1277                 dev_walker = dev_walker->parent;
1278         } while (!of_node && dev_walker);
1279
1280         if (!of_node)
1281                 return 0;
1282
1283         if (phy_interface_is_rgmii(phydev)) {
1284                 ret = ksz9131_config_rgmii_delay(phydev);
1285                 if (ret < 0)
1286                         return ret;
1287         }
1288
1289         ret = ksz9131_of_load_skew_values(phydev, of_node,
1290                                           MII_KSZ9031RN_CLK_PAD_SKEW, 5,
1291                                           clk_skews, 2);
1292         if (ret < 0)
1293                 return ret;
1294
1295         ret = ksz9131_of_load_skew_values(phydev, of_node,
1296                                           MII_KSZ9031RN_CONTROL_PAD_SKEW, 4,
1297                                           control_skews, 2);
1298         if (ret < 0)
1299                 return ret;
1300
1301         ret = ksz9131_of_load_skew_values(phydev, of_node,
1302                                           MII_KSZ9031RN_RX_DATA_PAD_SKEW, 4,
1303                                           rx_data_skews, 4);
1304         if (ret < 0)
1305                 return ret;
1306
1307         ret = ksz9131_of_load_skew_values(phydev, of_node,
1308                                           MII_KSZ9031RN_TX_DATA_PAD_SKEW, 4,
1309                                           tx_data_skews, 4);
1310         if (ret < 0)
1311                 return ret;
1312
1313         ret = ksz9131_led_errata(phydev);
1314         if (ret < 0)
1315                 return ret;
1316
1317         return 0;
1318 }
1319
1320 #define MII_KSZ9131_AUTO_MDIX           0x1C
1321 #define MII_KSZ9131_AUTO_MDI_SET        BIT(7)
1322 #define MII_KSZ9131_AUTO_MDIX_SWAP_OFF  BIT(6)
1323
1324 static int ksz9131_mdix_update(struct phy_device *phydev)
1325 {
1326         int ret;
1327
1328         ret = phy_read(phydev, MII_KSZ9131_AUTO_MDIX);
1329         if (ret < 0)
1330                 return ret;
1331
1332         if (ret & MII_KSZ9131_AUTO_MDIX_SWAP_OFF) {
1333                 if (ret & MII_KSZ9131_AUTO_MDI_SET)
1334                         phydev->mdix_ctrl = ETH_TP_MDI;
1335                 else
1336                         phydev->mdix_ctrl = ETH_TP_MDI_X;
1337         } else {
1338                 phydev->mdix_ctrl = ETH_TP_MDI_AUTO;
1339         }
1340
1341         if (ret & MII_KSZ9131_AUTO_MDI_SET)
1342                 phydev->mdix = ETH_TP_MDI;
1343         else
1344                 phydev->mdix = ETH_TP_MDI_X;
1345
1346         return 0;
1347 }
1348
1349 static int ksz9131_config_mdix(struct phy_device *phydev, u8 ctrl)
1350 {
1351         u16 val;
1352
1353         switch (ctrl) {
1354         case ETH_TP_MDI:
1355                 val = MII_KSZ9131_AUTO_MDIX_SWAP_OFF |
1356                       MII_KSZ9131_AUTO_MDI_SET;
1357                 break;
1358         case ETH_TP_MDI_X:
1359                 val = MII_KSZ9131_AUTO_MDIX_SWAP_OFF;
1360                 break;
1361         case ETH_TP_MDI_AUTO:
1362                 val = 0;
1363                 break;
1364         default:
1365                 return 0;
1366         }
1367
1368         return phy_modify(phydev, MII_KSZ9131_AUTO_MDIX,
1369                           MII_KSZ9131_AUTO_MDIX_SWAP_OFF |
1370                           MII_KSZ9131_AUTO_MDI_SET, val);
1371 }
1372
1373 static int ksz9131_read_status(struct phy_device *phydev)
1374 {
1375         int ret;
1376
1377         ret = ksz9131_mdix_update(phydev);
1378         if (ret < 0)
1379                 return ret;
1380
1381         return genphy_read_status(phydev);
1382 }
1383
1384 static int ksz9131_config_aneg(struct phy_device *phydev)
1385 {
1386         int ret;
1387
1388         ret = ksz9131_config_mdix(phydev, phydev->mdix_ctrl);
1389         if (ret)
1390                 return ret;
1391
1392         return genphy_config_aneg(phydev);
1393 }
1394
1395 static int ksz9477_get_features(struct phy_device *phydev)
1396 {
1397         int ret;
1398
1399         ret = genphy_read_abilities(phydev);
1400         if (ret)
1401                 return ret;
1402
1403         /* The "EEE control and capability 1" (Register 3.20) seems to be
1404          * influenced by the "EEE advertisement 1" (Register 7.60). Changes
1405          * on the 7.60 will affect 3.20. So, we need to construct our own list
1406          * of caps.
1407          * KSZ8563R should have 100BaseTX/Full only.
1408          */
1409         linkmode_and(phydev->supported_eee, phydev->supported,
1410                      PHY_EEE_CAP1_FEATURES);
1411
1412         return 0;
1413 }
1414
1415 #define KSZ8873MLL_GLOBAL_CONTROL_4     0x06
1416 #define KSZ8873MLL_GLOBAL_CONTROL_4_DUPLEX      BIT(6)
1417 #define KSZ8873MLL_GLOBAL_CONTROL_4_SPEED       BIT(4)
1418 static int ksz8873mll_read_status(struct phy_device *phydev)
1419 {
1420         int regval;
1421
1422         /* dummy read */
1423         regval = phy_read(phydev, KSZ8873MLL_GLOBAL_CONTROL_4);
1424
1425         regval = phy_read(phydev, KSZ8873MLL_GLOBAL_CONTROL_4);
1426
1427         if (regval & KSZ8873MLL_GLOBAL_CONTROL_4_DUPLEX)
1428                 phydev->duplex = DUPLEX_HALF;
1429         else
1430                 phydev->duplex = DUPLEX_FULL;
1431
1432         if (regval & KSZ8873MLL_GLOBAL_CONTROL_4_SPEED)
1433                 phydev->speed = SPEED_10;
1434         else
1435                 phydev->speed = SPEED_100;
1436
1437         phydev->link = 1;
1438         phydev->pause = phydev->asym_pause = 0;
1439
1440         return 0;
1441 }
1442
1443 static int ksz9031_get_features(struct phy_device *phydev)
1444 {
1445         int ret;
1446
1447         ret = genphy_read_abilities(phydev);
1448         if (ret < 0)
1449                 return ret;
1450
1451         /* Silicon Errata Sheet (DS80000691D or DS80000692D):
1452          * Whenever the device's Asymmetric Pause capability is set to 1,
1453          * link-up may fail after a link-up to link-down transition.
1454          *
1455          * The Errata Sheet is for ksz9031, but ksz9021 has the same issue
1456          *
1457          * Workaround:
1458          * Do not enable the Asymmetric Pause capability bit.
1459          */
1460         linkmode_clear_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, phydev->supported);
1461
1462         /* We force setting the Pause capability as the core will force the
1463          * Asymmetric Pause capability to 1 otherwise.
1464          */
1465         linkmode_set_bit(ETHTOOL_LINK_MODE_Pause_BIT, phydev->supported);
1466
1467         return 0;
1468 }
1469
1470 static int ksz9031_read_status(struct phy_device *phydev)
1471 {
1472         int err;
1473         int regval;
1474
1475         err = genphy_read_status(phydev);
1476         if (err)
1477                 return err;
1478
1479         /* Make sure the PHY is not broken. Read idle error count,
1480          * and reset the PHY if it is maxed out.
1481          */
1482         regval = phy_read(phydev, MII_STAT1000);
1483         if ((regval & 0xFF) == 0xFF) {
1484                 phy_init_hw(phydev);
1485                 phydev->link = 0;
1486                 if (phydev->drv->config_intr && phy_interrupt_is_valid(phydev))
1487                         phydev->drv->config_intr(phydev);
1488                 return genphy_config_aneg(phydev);
1489         }
1490
1491         return 0;
1492 }
1493
1494 static int ksz9x31_cable_test_start(struct phy_device *phydev)
1495 {
1496         struct kszphy_priv *priv = phydev->priv;
1497         int ret;
1498
1499         /* KSZ9131RNX, DS00002841B-page 38, 4.14 LinkMD (R) Cable Diagnostic
1500          * Prior to running the cable diagnostics, Auto-negotiation should
1501          * be disabled, full duplex set and the link speed set to 1000Mbps
1502          * via the Basic Control Register.
1503          */
1504         ret = phy_modify(phydev, MII_BMCR,
1505                          BMCR_SPEED1000 | BMCR_FULLDPLX |
1506                          BMCR_ANENABLE | BMCR_SPEED100,
1507                          BMCR_SPEED1000 | BMCR_FULLDPLX);
1508         if (ret)
1509                 return ret;
1510
1511         /* KSZ9131RNX, DS00002841B-page 38, 4.14 LinkMD (R) Cable Diagnostic
1512          * The Master-Slave configuration should be set to Slave by writing
1513          * a value of 0x1000 to the Auto-Negotiation Master Slave Control
1514          * Register.
1515          */
1516         ret = phy_read(phydev, MII_CTRL1000);
1517         if (ret < 0)
1518                 return ret;
1519
1520         /* Cache these bits, they need to be restored once LinkMD finishes. */
1521         priv->vct_ctrl1000 = ret & (CTL1000_ENABLE_MASTER | CTL1000_AS_MASTER);
1522         ret &= ~(CTL1000_ENABLE_MASTER | CTL1000_AS_MASTER);
1523         ret |= CTL1000_ENABLE_MASTER;
1524
1525         return phy_write(phydev, MII_CTRL1000, ret);
1526 }
1527
1528 static int ksz9x31_cable_test_result_trans(u16 status)
1529 {
1530         switch (FIELD_GET(KSZ9x31_LMD_VCT_ST_MASK, status)) {
1531         case KSZ9x31_LMD_VCT_ST_NORMAL:
1532                 return ETHTOOL_A_CABLE_RESULT_CODE_OK;
1533         case KSZ9x31_LMD_VCT_ST_OPEN:
1534                 return ETHTOOL_A_CABLE_RESULT_CODE_OPEN;
1535         case KSZ9x31_LMD_VCT_ST_SHORT:
1536                 return ETHTOOL_A_CABLE_RESULT_CODE_SAME_SHORT;
1537         case KSZ9x31_LMD_VCT_ST_FAIL:
1538                 fallthrough;
1539         default:
1540                 return ETHTOOL_A_CABLE_RESULT_CODE_UNSPEC;
1541         }
1542 }
1543
1544 static bool ksz9x31_cable_test_failed(u16 status)
1545 {
1546         int stat = FIELD_GET(KSZ9x31_LMD_VCT_ST_MASK, status);
1547
1548         return stat == KSZ9x31_LMD_VCT_ST_FAIL;
1549 }
1550
1551 static bool ksz9x31_cable_test_fault_length_valid(u16 status)
1552 {
1553         switch (FIELD_GET(KSZ9x31_LMD_VCT_ST_MASK, status)) {
1554         case KSZ9x31_LMD_VCT_ST_OPEN:
1555                 fallthrough;
1556         case KSZ9x31_LMD_VCT_ST_SHORT:
1557                 return true;
1558         }
1559         return false;
1560 }
1561
1562 static int ksz9x31_cable_test_fault_length(struct phy_device *phydev, u16 stat)
1563 {
1564         int dt = FIELD_GET(KSZ9x31_LMD_VCT_DATA_MASK, stat);
1565
1566         /* KSZ9131RNX, DS00002841B-page 38, 4.14 LinkMD (R) Cable Diagnostic
1567          *
1568          * distance to fault = (VCT_DATA - 22) * 4 / cable propagation velocity
1569          */
1570         if ((phydev->phy_id & MICREL_PHY_ID_MASK) == PHY_ID_KSZ9131)
1571                 dt = clamp(dt - 22, 0, 255);
1572
1573         return (dt * 400) / 10;
1574 }
1575
1576 static int ksz9x31_cable_test_wait_for_completion(struct phy_device *phydev)
1577 {
1578         int val, ret;
1579
1580         ret = phy_read_poll_timeout(phydev, KSZ9x31_LMD, val,
1581                                     !(val & KSZ9x31_LMD_VCT_EN),
1582                                     30000, 100000, true);
1583
1584         return ret < 0 ? ret : 0;
1585 }
1586
1587 static int ksz9x31_cable_test_get_pair(int pair)
1588 {
1589         static const int ethtool_pair[] = {
1590                 ETHTOOL_A_CABLE_PAIR_A,
1591                 ETHTOOL_A_CABLE_PAIR_B,
1592                 ETHTOOL_A_CABLE_PAIR_C,
1593                 ETHTOOL_A_CABLE_PAIR_D,
1594         };
1595
1596         return ethtool_pair[pair];
1597 }
1598
1599 static int ksz9x31_cable_test_one_pair(struct phy_device *phydev, int pair)
1600 {
1601         int ret, val;
1602
1603         /* KSZ9131RNX, DS00002841B-page 38, 4.14 LinkMD (R) Cable Diagnostic
1604          * To test each individual cable pair, set the cable pair in the Cable
1605          * Diagnostics Test Pair (VCT_PAIR[1:0]) field of the LinkMD Cable
1606          * Diagnostic Register, along with setting the Cable Diagnostics Test
1607          * Enable (VCT_EN) bit. The Cable Diagnostics Test Enable (VCT_EN) bit
1608          * will self clear when the test is concluded.
1609          */
1610         ret = phy_write(phydev, KSZ9x31_LMD,
1611                         KSZ9x31_LMD_VCT_EN | KSZ9x31_LMD_VCT_PAIR(pair));
1612         if (ret)
1613                 return ret;
1614
1615         ret = ksz9x31_cable_test_wait_for_completion(phydev);
1616         if (ret)
1617                 return ret;
1618
1619         val = phy_read(phydev, KSZ9x31_LMD);
1620         if (val < 0)
1621                 return val;
1622
1623         if (ksz9x31_cable_test_failed(val))
1624                 return -EAGAIN;
1625
1626         ret = ethnl_cable_test_result(phydev,
1627                                       ksz9x31_cable_test_get_pair(pair),
1628                                       ksz9x31_cable_test_result_trans(val));
1629         if (ret)
1630                 return ret;
1631
1632         if (!ksz9x31_cable_test_fault_length_valid(val))
1633                 return 0;
1634
1635         return ethnl_cable_test_fault_length(phydev,
1636                                              ksz9x31_cable_test_get_pair(pair),
1637                                              ksz9x31_cable_test_fault_length(phydev, val));
1638 }
1639
1640 static int ksz9x31_cable_test_get_status(struct phy_device *phydev,
1641                                          bool *finished)
1642 {
1643         struct kszphy_priv *priv = phydev->priv;
1644         unsigned long pair_mask = 0xf;
1645         int retries = 20;
1646         int pair, ret, rv;
1647
1648         *finished = false;
1649
1650         /* Try harder if link partner is active */
1651         while (pair_mask && retries--) {
1652                 for_each_set_bit(pair, &pair_mask, 4) {
1653                         ret = ksz9x31_cable_test_one_pair(phydev, pair);
1654                         if (ret == -EAGAIN)
1655                                 continue;
1656                         if (ret < 0)
1657                                 return ret;
1658                         clear_bit(pair, &pair_mask);
1659                 }
1660                 /* If link partner is in autonegotiation mode it will send 2ms
1661                  * of FLPs with at least 6ms of silence.
1662                  * Add 2ms sleep to have better chances to hit this silence.
1663                  */
1664                 if (pair_mask)
1665                         usleep_range(2000, 3000);
1666         }
1667
1668         /* Report remaining unfinished pair result as unknown. */
1669         for_each_set_bit(pair, &pair_mask, 4) {
1670                 ret = ethnl_cable_test_result(phydev,
1671                                               ksz9x31_cable_test_get_pair(pair),
1672                                               ETHTOOL_A_CABLE_RESULT_CODE_UNSPEC);
1673         }
1674
1675         *finished = true;
1676
1677         /* Restore cached bits from before LinkMD got started. */
1678         rv = phy_modify(phydev, MII_CTRL1000,
1679                         CTL1000_ENABLE_MASTER | CTL1000_AS_MASTER,
1680                         priv->vct_ctrl1000);
1681         if (rv)
1682                 return rv;
1683
1684         return ret;
1685 }
1686
1687 static int ksz8873mll_config_aneg(struct phy_device *phydev)
1688 {
1689         return 0;
1690 }
1691
1692 static int ksz886x_config_mdix(struct phy_device *phydev, u8 ctrl)
1693 {
1694         u16 val;
1695
1696         switch (ctrl) {
1697         case ETH_TP_MDI:
1698                 val = KSZ886X_BMCR_DISABLE_AUTO_MDIX;
1699                 break;
1700         case ETH_TP_MDI_X:
1701                 /* Note: The naming of the bit KSZ886X_BMCR_FORCE_MDI is bit
1702                  * counter intuitive, the "-X" in "1 = Force MDI" in the data
1703                  * sheet seems to be missing:
1704                  * 1 = Force MDI (sic!) (transmit on RX+/RX- pins)
1705                  * 0 = Normal operation (transmit on TX+/TX- pins)
1706                  */
1707                 val = KSZ886X_BMCR_DISABLE_AUTO_MDIX | KSZ886X_BMCR_FORCE_MDI;
1708                 break;
1709         case ETH_TP_MDI_AUTO:
1710                 val = 0;
1711                 break;
1712         default:
1713                 return 0;
1714         }
1715
1716         return phy_modify(phydev, MII_BMCR,
1717                           KSZ886X_BMCR_HP_MDIX | KSZ886X_BMCR_FORCE_MDI |
1718                           KSZ886X_BMCR_DISABLE_AUTO_MDIX,
1719                           KSZ886X_BMCR_HP_MDIX | val);
1720 }
1721
1722 static int ksz886x_config_aneg(struct phy_device *phydev)
1723 {
1724         int ret;
1725
1726         ret = genphy_config_aneg(phydev);
1727         if (ret)
1728                 return ret;
1729
1730         /* The MDI-X configuration is automatically changed by the PHY after
1731          * switching from autoneg off to on. So, take MDI-X configuration under
1732          * own control and set it after autoneg configuration was done.
1733          */
1734         return ksz886x_config_mdix(phydev, phydev->mdix_ctrl);
1735 }
1736
1737 static int ksz886x_mdix_update(struct phy_device *phydev)
1738 {
1739         int ret;
1740
1741         ret = phy_read(phydev, MII_BMCR);
1742         if (ret < 0)
1743                 return ret;
1744
1745         if (ret & KSZ886X_BMCR_DISABLE_AUTO_MDIX) {
1746                 if (ret & KSZ886X_BMCR_FORCE_MDI)
1747                         phydev->mdix_ctrl = ETH_TP_MDI_X;
1748                 else
1749                         phydev->mdix_ctrl = ETH_TP_MDI;
1750         } else {
1751                 phydev->mdix_ctrl = ETH_TP_MDI_AUTO;
1752         }
1753
1754         ret = phy_read(phydev, MII_KSZPHY_CTRL);
1755         if (ret < 0)
1756                 return ret;
1757
1758         /* Same reverse logic as KSZ886X_BMCR_FORCE_MDI */
1759         if (ret & KSZ886X_CTRL_MDIX_STAT)
1760                 phydev->mdix = ETH_TP_MDI_X;
1761         else
1762                 phydev->mdix = ETH_TP_MDI;
1763
1764         return 0;
1765 }
1766
1767 static int ksz886x_read_status(struct phy_device *phydev)
1768 {
1769         int ret;
1770
1771         ret = ksz886x_mdix_update(phydev);
1772         if (ret < 0)
1773                 return ret;
1774
1775         return genphy_read_status(phydev);
1776 }
1777
1778 static int kszphy_get_sset_count(struct phy_device *phydev)
1779 {
1780         return ARRAY_SIZE(kszphy_hw_stats);
1781 }
1782
1783 static void kszphy_get_strings(struct phy_device *phydev, u8 *data)
1784 {
1785         int i;
1786
1787         for (i = 0; i < ARRAY_SIZE(kszphy_hw_stats); i++) {
1788                 strscpy(data + i * ETH_GSTRING_LEN,
1789                         kszphy_hw_stats[i].string, ETH_GSTRING_LEN);
1790         }
1791 }
1792
1793 static u64 kszphy_get_stat(struct phy_device *phydev, int i)
1794 {
1795         struct kszphy_hw_stat stat = kszphy_hw_stats[i];
1796         struct kszphy_priv *priv = phydev->priv;
1797         int val;
1798         u64 ret;
1799
1800         val = phy_read(phydev, stat.reg);
1801         if (val < 0) {
1802                 ret = U64_MAX;
1803         } else {
1804                 val = val & ((1 << stat.bits) - 1);
1805                 priv->stats[i] += val;
1806                 ret = priv->stats[i];
1807         }
1808
1809         return ret;
1810 }
1811
1812 static void kszphy_get_stats(struct phy_device *phydev,
1813                              struct ethtool_stats *stats, u64 *data)
1814 {
1815         int i;
1816
1817         for (i = 0; i < ARRAY_SIZE(kszphy_hw_stats); i++)
1818                 data[i] = kszphy_get_stat(phydev, i);
1819 }
1820
1821 static int kszphy_suspend(struct phy_device *phydev)
1822 {
1823         /* Disable PHY Interrupts */
1824         if (phy_interrupt_is_valid(phydev)) {
1825                 phydev->interrupts = PHY_INTERRUPT_DISABLED;
1826                 if (phydev->drv->config_intr)
1827                         phydev->drv->config_intr(phydev);
1828         }
1829
1830         return genphy_suspend(phydev);
1831 }
1832
1833 static void kszphy_parse_led_mode(struct phy_device *phydev)
1834 {
1835         const struct kszphy_type *type = phydev->drv->driver_data;
1836         const struct device_node *np = phydev->mdio.dev.of_node;
1837         struct kszphy_priv *priv = phydev->priv;
1838         int ret;
1839
1840         if (type && type->led_mode_reg) {
1841                 ret = of_property_read_u32(np, "micrel,led-mode",
1842                                            &priv->led_mode);
1843
1844                 if (ret)
1845                         priv->led_mode = -1;
1846
1847                 if (priv->led_mode > 3) {
1848                         phydev_err(phydev, "invalid led mode: 0x%02x\n",
1849                                    priv->led_mode);
1850                         priv->led_mode = -1;
1851                 }
1852         } else {
1853                 priv->led_mode = -1;
1854         }
1855 }
1856
1857 static int kszphy_resume(struct phy_device *phydev)
1858 {
1859         int ret;
1860
1861         genphy_resume(phydev);
1862
1863         /* After switching from power-down to normal mode, an internal global
1864          * reset is automatically generated. Wait a minimum of 1 ms before
1865          * read/write access to the PHY registers.
1866          */
1867         usleep_range(1000, 2000);
1868
1869         ret = kszphy_config_reset(phydev);
1870         if (ret)
1871                 return ret;
1872
1873         /* Enable PHY Interrupts */
1874         if (phy_interrupt_is_valid(phydev)) {
1875                 phydev->interrupts = PHY_INTERRUPT_ENABLED;
1876                 if (phydev->drv->config_intr)
1877                         phydev->drv->config_intr(phydev);
1878         }
1879
1880         return 0;
1881 }
1882
1883 static int kszphy_probe(struct phy_device *phydev)
1884 {
1885         const struct kszphy_type *type = phydev->drv->driver_data;
1886         const struct device_node *np = phydev->mdio.dev.of_node;
1887         struct kszphy_priv *priv;
1888         struct clk *clk;
1889
1890         priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
1891         if (!priv)
1892                 return -ENOMEM;
1893
1894         phydev->priv = priv;
1895
1896         priv->type = type;
1897
1898         kszphy_parse_led_mode(phydev);
1899
1900         clk = devm_clk_get(&phydev->mdio.dev, "rmii-ref");
1901         /* NOTE: clk may be NULL if building without CONFIG_HAVE_CLK */
1902         if (!IS_ERR_OR_NULL(clk)) {
1903                 unsigned long rate = clk_get_rate(clk);
1904                 bool rmii_ref_clk_sel_25_mhz;
1905
1906                 if (type)
1907                         priv->rmii_ref_clk_sel = type->has_rmii_ref_clk_sel;
1908                 rmii_ref_clk_sel_25_mhz = of_property_read_bool(np,
1909                                 "micrel,rmii-reference-clock-select-25-mhz");
1910
1911                 if (rate > 24500000 && rate < 25500000) {
1912                         priv->rmii_ref_clk_sel_val = rmii_ref_clk_sel_25_mhz;
1913                 } else if (rate > 49500000 && rate < 50500000) {
1914                         priv->rmii_ref_clk_sel_val = !rmii_ref_clk_sel_25_mhz;
1915                 } else {
1916                         phydev_err(phydev, "Clock rate out of range: %ld\n",
1917                                    rate);
1918                         return -EINVAL;
1919                 }
1920         }
1921
1922         if (ksz8041_fiber_mode(phydev))
1923                 phydev->port = PORT_FIBRE;
1924
1925         /* Support legacy board-file configuration */
1926         if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK) {
1927                 priv->rmii_ref_clk_sel = true;
1928                 priv->rmii_ref_clk_sel_val = true;
1929         }
1930
1931         return 0;
1932 }
1933
1934 static int lan8814_cable_test_start(struct phy_device *phydev)
1935 {
1936         /* If autoneg is enabled, we won't be able to test cross pair
1937          * short. In this case, the PHY will "detect" a link and
1938          * confuse the internal state machine - disable auto neg here.
1939          * Set the speed to 1000mbit and full duplex.
1940          */
1941         return phy_modify(phydev, MII_BMCR, BMCR_ANENABLE | BMCR_SPEED100,
1942                           BMCR_SPEED1000 | BMCR_FULLDPLX);
1943 }
1944
1945 static int ksz886x_cable_test_start(struct phy_device *phydev)
1946 {
1947         if (phydev->dev_flags & MICREL_KSZ8_P1_ERRATA)
1948                 return -EOPNOTSUPP;
1949
1950         /* If autoneg is enabled, we won't be able to test cross pair
1951          * short. In this case, the PHY will "detect" a link and
1952          * confuse the internal state machine - disable auto neg here.
1953          * If autoneg is disabled, we should set the speed to 10mbit.
1954          */
1955         return phy_clear_bits(phydev, MII_BMCR, BMCR_ANENABLE | BMCR_SPEED100);
1956 }
1957
1958 static __always_inline int ksz886x_cable_test_result_trans(u16 status, u16 mask)
1959 {
1960         switch (FIELD_GET(mask, status)) {
1961         case KSZ8081_LMD_STAT_NORMAL:
1962                 return ETHTOOL_A_CABLE_RESULT_CODE_OK;
1963         case KSZ8081_LMD_STAT_SHORT:
1964                 return ETHTOOL_A_CABLE_RESULT_CODE_SAME_SHORT;
1965         case KSZ8081_LMD_STAT_OPEN:
1966                 return ETHTOOL_A_CABLE_RESULT_CODE_OPEN;
1967         case KSZ8081_LMD_STAT_FAIL:
1968                 fallthrough;
1969         default:
1970                 return ETHTOOL_A_CABLE_RESULT_CODE_UNSPEC;
1971         }
1972 }
1973
1974 static __always_inline bool ksz886x_cable_test_failed(u16 status, u16 mask)
1975 {
1976         return FIELD_GET(mask, status) ==
1977                 KSZ8081_LMD_STAT_FAIL;
1978 }
1979
1980 static __always_inline bool ksz886x_cable_test_fault_length_valid(u16 status, u16 mask)
1981 {
1982         switch (FIELD_GET(mask, status)) {
1983         case KSZ8081_LMD_STAT_OPEN:
1984                 fallthrough;
1985         case KSZ8081_LMD_STAT_SHORT:
1986                 return true;
1987         }
1988         return false;
1989 }
1990
1991 static __always_inline int ksz886x_cable_test_fault_length(struct phy_device *phydev,
1992                                                            u16 status, u16 data_mask)
1993 {
1994         int dt;
1995
1996         /* According to the data sheet the distance to the fault is
1997          * DELTA_TIME * 0.4 meters for ksz phys.
1998          * (DELTA_TIME - 22) * 0.8 for lan8814 phy.
1999          */
2000         dt = FIELD_GET(data_mask, status);
2001
2002         if ((phydev->phy_id & MICREL_PHY_ID_MASK) == PHY_ID_LAN8814)
2003                 return ((dt - 22) * 800) / 10;
2004         else
2005                 return (dt * 400) / 10;
2006 }
2007
2008 static int ksz886x_cable_test_wait_for_completion(struct phy_device *phydev)
2009 {
2010         const struct kszphy_type *type = phydev->drv->driver_data;
2011         int val, ret;
2012
2013         ret = phy_read_poll_timeout(phydev, type->cable_diag_reg, val,
2014                                     !(val & KSZ8081_LMD_ENABLE_TEST),
2015                                     30000, 100000, true);
2016
2017         return ret < 0 ? ret : 0;
2018 }
2019
2020 static int lan8814_cable_test_one_pair(struct phy_device *phydev, int pair)
2021 {
2022         static const int ethtool_pair[] = { ETHTOOL_A_CABLE_PAIR_A,
2023                                             ETHTOOL_A_CABLE_PAIR_B,
2024                                             ETHTOOL_A_CABLE_PAIR_C,
2025                                             ETHTOOL_A_CABLE_PAIR_D,
2026                                           };
2027         u32 fault_length;
2028         int ret;
2029         int val;
2030
2031         val = KSZ8081_LMD_ENABLE_TEST;
2032         val = val | (pair << LAN8814_PAIR_BIT_SHIFT);
2033
2034         ret = phy_write(phydev, LAN8814_CABLE_DIAG, val);
2035         if (ret < 0)
2036                 return ret;
2037
2038         ret = ksz886x_cable_test_wait_for_completion(phydev);
2039         if (ret)
2040                 return ret;
2041
2042         val = phy_read(phydev, LAN8814_CABLE_DIAG);
2043         if (val < 0)
2044                 return val;
2045
2046         if (ksz886x_cable_test_failed(val, LAN8814_CABLE_DIAG_STAT_MASK))
2047                 return -EAGAIN;
2048
2049         ret = ethnl_cable_test_result(phydev, ethtool_pair[pair],
2050                                       ksz886x_cable_test_result_trans(val,
2051                                                                       LAN8814_CABLE_DIAG_STAT_MASK
2052                                                                       ));
2053         if (ret)
2054                 return ret;
2055
2056         if (!ksz886x_cable_test_fault_length_valid(val, LAN8814_CABLE_DIAG_STAT_MASK))
2057                 return 0;
2058
2059         fault_length = ksz886x_cable_test_fault_length(phydev, val,
2060                                                        LAN8814_CABLE_DIAG_VCT_DATA_MASK);
2061
2062         return ethnl_cable_test_fault_length(phydev, ethtool_pair[pair], fault_length);
2063 }
2064
2065 static int ksz886x_cable_test_one_pair(struct phy_device *phydev, int pair)
2066 {
2067         static const int ethtool_pair[] = {
2068                 ETHTOOL_A_CABLE_PAIR_A,
2069                 ETHTOOL_A_CABLE_PAIR_B,
2070         };
2071         int ret, val, mdix;
2072         u32 fault_length;
2073
2074         /* There is no way to choice the pair, like we do one ksz9031.
2075          * We can workaround this limitation by using the MDI-X functionality.
2076          */
2077         if (pair == 0)
2078                 mdix = ETH_TP_MDI;
2079         else
2080                 mdix = ETH_TP_MDI_X;
2081
2082         switch (phydev->phy_id & MICREL_PHY_ID_MASK) {
2083         case PHY_ID_KSZ8081:
2084                 ret = ksz8081_config_mdix(phydev, mdix);
2085                 break;
2086         case PHY_ID_KSZ886X:
2087                 ret = ksz886x_config_mdix(phydev, mdix);
2088                 break;
2089         default:
2090                 ret = -ENODEV;
2091         }
2092
2093         if (ret)
2094                 return ret;
2095
2096         /* Now we are ready to fire. This command will send a 100ns pulse
2097          * to the pair.
2098          */
2099         ret = phy_write(phydev, KSZ8081_LMD, KSZ8081_LMD_ENABLE_TEST);
2100         if (ret)
2101                 return ret;
2102
2103         ret = ksz886x_cable_test_wait_for_completion(phydev);
2104         if (ret)
2105                 return ret;
2106
2107         val = phy_read(phydev, KSZ8081_LMD);
2108         if (val < 0)
2109                 return val;
2110
2111         if (ksz886x_cable_test_failed(val, KSZ8081_LMD_STAT_MASK))
2112                 return -EAGAIN;
2113
2114         ret = ethnl_cable_test_result(phydev, ethtool_pair[pair],
2115                                       ksz886x_cable_test_result_trans(val, KSZ8081_LMD_STAT_MASK));
2116         if (ret)
2117                 return ret;
2118
2119         if (!ksz886x_cable_test_fault_length_valid(val, KSZ8081_LMD_STAT_MASK))
2120                 return 0;
2121
2122         fault_length = ksz886x_cable_test_fault_length(phydev, val, KSZ8081_LMD_DELTA_TIME_MASK);
2123
2124         return ethnl_cable_test_fault_length(phydev, ethtool_pair[pair], fault_length);
2125 }
2126
2127 static int ksz886x_cable_test_get_status(struct phy_device *phydev,
2128                                          bool *finished)
2129 {
2130         const struct kszphy_type *type = phydev->drv->driver_data;
2131         unsigned long pair_mask = type->pair_mask;
2132         int retries = 20;
2133         int ret = 0;
2134         int pair;
2135
2136         *finished = false;
2137
2138         /* Try harder if link partner is active */
2139         while (pair_mask && retries--) {
2140                 for_each_set_bit(pair, &pair_mask, 4) {
2141                         if (type->cable_diag_reg == LAN8814_CABLE_DIAG)
2142                                 ret = lan8814_cable_test_one_pair(phydev, pair);
2143                         else
2144                                 ret = ksz886x_cable_test_one_pair(phydev, pair);
2145                         if (ret == -EAGAIN)
2146                                 continue;
2147                         if (ret < 0)
2148                                 return ret;
2149                         clear_bit(pair, &pair_mask);
2150                 }
2151                 /* If link partner is in autonegotiation mode it will send 2ms
2152                  * of FLPs with at least 6ms of silence.
2153                  * Add 2ms sleep to have better chances to hit this silence.
2154                  */
2155                 if (pair_mask)
2156                         msleep(2);
2157         }
2158
2159         *finished = true;
2160
2161         return ret;
2162 }
2163
2164 #define LAN_EXT_PAGE_ACCESS_CONTROL                     0x16
2165 #define LAN_EXT_PAGE_ACCESS_ADDRESS_DATA                0x17
2166 #define LAN_EXT_PAGE_ACCESS_CTRL_EP_FUNC                0x4000
2167
2168 #define LAN8814_QSGMII_SOFT_RESET                       0x43
2169 #define LAN8814_QSGMII_SOFT_RESET_BIT                   BIT(0)
2170 #define LAN8814_QSGMII_PCS1G_ANEG_CONFIG                0x13
2171 #define LAN8814_QSGMII_PCS1G_ANEG_CONFIG_ANEG_ENA       BIT(3)
2172 #define LAN8814_ALIGN_SWAP                              0x4a
2173 #define LAN8814_ALIGN_TX_A_B_SWAP                       0x1
2174 #define LAN8814_ALIGN_TX_A_B_SWAP_MASK                  GENMASK(2, 0)
2175
2176 #define LAN8804_ALIGN_SWAP                              0x4a
2177 #define LAN8804_ALIGN_TX_A_B_SWAP                       0x1
2178 #define LAN8804_ALIGN_TX_A_B_SWAP_MASK                  GENMASK(2, 0)
2179 #define LAN8814_CLOCK_MANAGEMENT                        0xd
2180 #define LAN8814_LINK_QUALITY                            0x8e
2181
2182 static int lanphy_read_page_reg(struct phy_device *phydev, int page, u32 addr)
2183 {
2184         int data;
2185
2186         phy_lock_mdio_bus(phydev);
2187         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_CONTROL, page);
2188         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_ADDRESS_DATA, addr);
2189         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_CONTROL,
2190                     (page | LAN_EXT_PAGE_ACCESS_CTRL_EP_FUNC));
2191         data = __phy_read(phydev, LAN_EXT_PAGE_ACCESS_ADDRESS_DATA);
2192         phy_unlock_mdio_bus(phydev);
2193
2194         return data;
2195 }
2196
2197 static int lanphy_write_page_reg(struct phy_device *phydev, int page, u16 addr,
2198                                  u16 val)
2199 {
2200         phy_lock_mdio_bus(phydev);
2201         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_CONTROL, page);
2202         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_ADDRESS_DATA, addr);
2203         __phy_write(phydev, LAN_EXT_PAGE_ACCESS_CONTROL,
2204                     page | LAN_EXT_PAGE_ACCESS_CTRL_EP_FUNC);
2205
2206         val = __phy_write(phydev, LAN_EXT_PAGE_ACCESS_ADDRESS_DATA, val);
2207         if (val != 0)
2208                 phydev_err(phydev, "Error: phy_write has returned error %d\n",
2209                            val);
2210         phy_unlock_mdio_bus(phydev);
2211         return val;
2212 }
2213
2214 static int lan8814_config_ts_intr(struct phy_device *phydev, bool enable)
2215 {
2216         u16 val = 0;
2217
2218         if (enable)
2219                 val = PTP_TSU_INT_EN_PTP_TX_TS_EN_ |
2220                       PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN_ |
2221                       PTP_TSU_INT_EN_PTP_RX_TS_EN_ |
2222                       PTP_TSU_INT_EN_PTP_RX_TS_OVRFL_EN_;
2223
2224         return lanphy_write_page_reg(phydev, 5, PTP_TSU_INT_EN, val);
2225 }
2226
2227 static void lan8814_ptp_rx_ts_get(struct phy_device *phydev,
2228                                   u32 *seconds, u32 *nano_seconds, u16 *seq_id)
2229 {
2230         *seconds = lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_SEC_HI);
2231         *seconds = (*seconds << 16) |
2232                    lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_SEC_LO);
2233
2234         *nano_seconds = lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_NS_HI);
2235         *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
2236                         lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_NS_LO);
2237
2238         *seq_id = lanphy_read_page_reg(phydev, 5, PTP_RX_MSG_HEADER2);
2239 }
2240
2241 static void lan8814_ptp_tx_ts_get(struct phy_device *phydev,
2242                                   u32 *seconds, u32 *nano_seconds, u16 *seq_id)
2243 {
2244         *seconds = lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_SEC_HI);
2245         *seconds = *seconds << 16 |
2246                    lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_SEC_LO);
2247
2248         *nano_seconds = lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_NS_HI);
2249         *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
2250                         lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_NS_LO);
2251
2252         *seq_id = lanphy_read_page_reg(phydev, 5, PTP_TX_MSG_HEADER2);
2253 }
2254
2255 static int lan8814_ts_info(struct mii_timestamper *mii_ts, struct ethtool_ts_info *info)
2256 {
2257         struct kszphy_ptp_priv *ptp_priv = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
2258         struct phy_device *phydev = ptp_priv->phydev;
2259         struct lan8814_shared_priv *shared = phydev->shared->priv;
2260
2261         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
2262                                 SOF_TIMESTAMPING_RX_HARDWARE |
2263                                 SOF_TIMESTAMPING_RAW_HARDWARE;
2264
2265         info->phc_index = ptp_clock_index(shared->ptp_clock);
2266
2267         info->tx_types =
2268                 (1 << HWTSTAMP_TX_OFF) |
2269                 (1 << HWTSTAMP_TX_ON) |
2270                 (1 << HWTSTAMP_TX_ONESTEP_SYNC);
2271
2272         info->rx_filters =
2273                 (1 << HWTSTAMP_FILTER_NONE) |
2274                 (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
2275                 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
2276                 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
2277                 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
2278
2279         return 0;
2280 }
2281
2282 static void lan8814_flush_fifo(struct phy_device *phydev, bool egress)
2283 {
2284         int i;
2285
2286         for (i = 0; i < FIFO_SIZE; ++i)
2287                 lanphy_read_page_reg(phydev, 5,
2288                                      egress ? PTP_TX_MSG_HEADER2 : PTP_RX_MSG_HEADER2);
2289
2290         /* Read to clear overflow status bit */
2291         lanphy_read_page_reg(phydev, 5, PTP_TSU_INT_STS);
2292 }
2293
2294 static int lan8814_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
2295 {
2296         struct kszphy_ptp_priv *ptp_priv =
2297                           container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
2298         struct phy_device *phydev = ptp_priv->phydev;
2299         struct lan8814_shared_priv *shared = phydev->shared->priv;
2300         struct lan8814_ptp_rx_ts *rx_ts, *tmp;
2301         struct hwtstamp_config config;
2302         int txcfg = 0, rxcfg = 0;
2303         int pkt_ts_enable;
2304
2305         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
2306                 return -EFAULT;
2307
2308         ptp_priv->hwts_tx_type = config.tx_type;
2309         ptp_priv->rx_filter = config.rx_filter;
2310
2311         switch (config.rx_filter) {
2312         case HWTSTAMP_FILTER_NONE:
2313                 ptp_priv->layer = 0;
2314                 ptp_priv->version = 0;
2315                 break;
2316         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
2317         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
2318         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
2319                 ptp_priv->layer = PTP_CLASS_L4;
2320                 ptp_priv->version = PTP_CLASS_V2;
2321                 break;
2322         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
2323         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
2324         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
2325                 ptp_priv->layer = PTP_CLASS_L2;
2326                 ptp_priv->version = PTP_CLASS_V2;
2327                 break;
2328         case HWTSTAMP_FILTER_PTP_V2_EVENT:
2329         case HWTSTAMP_FILTER_PTP_V2_SYNC:
2330         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
2331                 ptp_priv->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
2332                 ptp_priv->version = PTP_CLASS_V2;
2333                 break;
2334         default:
2335                 return -ERANGE;
2336         }
2337
2338         if (ptp_priv->layer & PTP_CLASS_L2) {
2339                 rxcfg = PTP_RX_PARSE_CONFIG_LAYER2_EN_;
2340                 txcfg = PTP_TX_PARSE_CONFIG_LAYER2_EN_;
2341         } else if (ptp_priv->layer & PTP_CLASS_L4) {
2342                 rxcfg |= PTP_RX_PARSE_CONFIG_IPV4_EN_ | PTP_RX_PARSE_CONFIG_IPV6_EN_;
2343                 txcfg |= PTP_TX_PARSE_CONFIG_IPV4_EN_ | PTP_TX_PARSE_CONFIG_IPV6_EN_;
2344         }
2345         lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_RX_PARSE_CONFIG, rxcfg);
2346         lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_PARSE_CONFIG, txcfg);
2347
2348         pkt_ts_enable = PTP_TIMESTAMP_EN_SYNC_ | PTP_TIMESTAMP_EN_DREQ_ |
2349                         PTP_TIMESTAMP_EN_PDREQ_ | PTP_TIMESTAMP_EN_PDRES_;
2350         lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_RX_TIMESTAMP_EN, pkt_ts_enable);
2351         lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_TIMESTAMP_EN, pkt_ts_enable);
2352
2353         if (ptp_priv->hwts_tx_type == HWTSTAMP_TX_ONESTEP_SYNC)
2354                 lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_MOD,
2355                                       PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_);
2356
2357         if (config.rx_filter != HWTSTAMP_FILTER_NONE)
2358                 lan8814_config_ts_intr(ptp_priv->phydev, true);
2359         else
2360                 lan8814_config_ts_intr(ptp_priv->phydev, false);
2361
2362         mutex_lock(&shared->shared_lock);
2363         if (config.rx_filter != HWTSTAMP_FILTER_NONE)
2364                 shared->ref++;
2365         else
2366                 shared->ref--;
2367
2368         if (shared->ref)
2369                 lanphy_write_page_reg(ptp_priv->phydev, 4, PTP_CMD_CTL,
2370                                       PTP_CMD_CTL_PTP_ENABLE_);
2371         else
2372                 lanphy_write_page_reg(ptp_priv->phydev, 4, PTP_CMD_CTL,
2373                                       PTP_CMD_CTL_PTP_DISABLE_);
2374         mutex_unlock(&shared->shared_lock);
2375
2376         /* In case of multiple starts and stops, these needs to be cleared */
2377         list_for_each_entry_safe(rx_ts, tmp, &ptp_priv->rx_ts_list, list) {
2378                 list_del(&rx_ts->list);
2379                 kfree(rx_ts);
2380         }
2381         skb_queue_purge(&ptp_priv->rx_queue);
2382         skb_queue_purge(&ptp_priv->tx_queue);
2383
2384         lan8814_flush_fifo(ptp_priv->phydev, false);
2385         lan8814_flush_fifo(ptp_priv->phydev, true);
2386
2387         return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? -EFAULT : 0;
2388 }
2389
2390 static void lan8814_txtstamp(struct mii_timestamper *mii_ts,
2391                              struct sk_buff *skb, int type)
2392 {
2393         struct kszphy_ptp_priv *ptp_priv = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
2394
2395         switch (ptp_priv->hwts_tx_type) {
2396         case HWTSTAMP_TX_ONESTEP_SYNC:
2397                 if (ptp_msg_is_sync(skb, type)) {
2398                         kfree_skb(skb);
2399                         return;
2400                 }
2401                 fallthrough;
2402         case HWTSTAMP_TX_ON:
2403                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
2404                 skb_queue_tail(&ptp_priv->tx_queue, skb);
2405                 break;
2406         case HWTSTAMP_TX_OFF:
2407         default:
2408                 kfree_skb(skb);
2409                 break;
2410         }
2411 }
2412
2413 static void lan8814_get_sig_rx(struct sk_buff *skb, u16 *sig)
2414 {
2415         struct ptp_header *ptp_header;
2416         u32 type;
2417
2418         skb_push(skb, ETH_HLEN);
2419         type = ptp_classify_raw(skb);
2420         ptp_header = ptp_parse_header(skb, type);
2421         skb_pull_inline(skb, ETH_HLEN);
2422
2423         *sig = (__force u16)(ntohs(ptp_header->sequence_id));
2424 }
2425
2426 static bool lan8814_match_rx_skb(struct kszphy_ptp_priv *ptp_priv,
2427                                  struct sk_buff *skb)
2428 {
2429         struct skb_shared_hwtstamps *shhwtstamps;
2430         struct lan8814_ptp_rx_ts *rx_ts, *tmp;
2431         unsigned long flags;
2432         bool ret = false;
2433         u16 skb_sig;
2434
2435         lan8814_get_sig_rx(skb, &skb_sig);
2436
2437         /* Iterate over all RX timestamps and match it with the received skbs */
2438         spin_lock_irqsave(&ptp_priv->rx_ts_lock, flags);
2439         list_for_each_entry_safe(rx_ts, tmp, &ptp_priv->rx_ts_list, list) {
2440                 /* Check if we found the signature we were looking for. */
2441                 if (memcmp(&skb_sig, &rx_ts->seq_id, sizeof(rx_ts->seq_id)))
2442                         continue;
2443
2444                 shhwtstamps = skb_hwtstamps(skb);
2445                 memset(shhwtstamps, 0, sizeof(*shhwtstamps));
2446                 shhwtstamps->hwtstamp = ktime_set(rx_ts->seconds,
2447                                                   rx_ts->nsec);
2448                 list_del(&rx_ts->list);
2449                 kfree(rx_ts);
2450
2451                 ret = true;
2452                 break;
2453         }
2454         spin_unlock_irqrestore(&ptp_priv->rx_ts_lock, flags);
2455
2456         if (ret)
2457                 netif_rx(skb);
2458         return ret;
2459 }
2460
2461 static bool lan8814_rxtstamp(struct mii_timestamper *mii_ts, struct sk_buff *skb, int type)
2462 {
2463         struct kszphy_ptp_priv *ptp_priv =
2464                         container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
2465
2466         if (ptp_priv->rx_filter == HWTSTAMP_FILTER_NONE ||
2467             type == PTP_CLASS_NONE)
2468                 return false;
2469
2470         if ((type & ptp_priv->version) == 0 || (type & ptp_priv->layer) == 0)
2471                 return false;
2472
2473         /* If we failed to match then add it to the queue for when the timestamp
2474          * will come
2475          */
2476         if (!lan8814_match_rx_skb(ptp_priv, skb))
2477                 skb_queue_tail(&ptp_priv->rx_queue, skb);
2478
2479         return true;
2480 }
2481
2482 static void lan8814_ptp_clock_set(struct phy_device *phydev,
2483                                   u32 seconds, u32 nano_seconds)
2484 {
2485         u32 sec_low, sec_high, nsec_low, nsec_high;
2486
2487         sec_low = seconds & 0xffff;
2488         sec_high = (seconds >> 16) & 0xffff;
2489         nsec_low = nano_seconds & 0xffff;
2490         nsec_high = (nano_seconds >> 16) & 0x3fff;
2491
2492         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_SEC_LO, sec_low);
2493         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_SEC_MID, sec_high);
2494         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_NS_LO, nsec_low);
2495         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_NS_HI, nsec_high);
2496
2497         lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
2498 }
2499
2500 static void lan8814_ptp_clock_get(struct phy_device *phydev,
2501                                   u32 *seconds, u32 *nano_seconds)
2502 {
2503         lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_READ_);
2504
2505         *seconds = lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_SEC_MID);
2506         *seconds = (*seconds << 16) |
2507                    lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_SEC_LO);
2508
2509         *nano_seconds = lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_NS_HI);
2510         *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
2511                         lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_NS_LO);
2512 }
2513
2514 static int lan8814_ptpci_gettime64(struct ptp_clock_info *ptpci,
2515                                    struct timespec64 *ts)
2516 {
2517         struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
2518                                                           ptp_clock_info);
2519         struct phy_device *phydev = shared->phydev;
2520         u32 nano_seconds;
2521         u32 seconds;
2522
2523         mutex_lock(&shared->shared_lock);
2524         lan8814_ptp_clock_get(phydev, &seconds, &nano_seconds);
2525         mutex_unlock(&shared->shared_lock);
2526         ts->tv_sec = seconds;
2527         ts->tv_nsec = nano_seconds;
2528
2529         return 0;
2530 }
2531
2532 static int lan8814_ptpci_settime64(struct ptp_clock_info *ptpci,
2533                                    const struct timespec64 *ts)
2534 {
2535         struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
2536                                                           ptp_clock_info);
2537         struct phy_device *phydev = shared->phydev;
2538
2539         mutex_lock(&shared->shared_lock);
2540         lan8814_ptp_clock_set(phydev, ts->tv_sec, ts->tv_nsec);
2541         mutex_unlock(&shared->shared_lock);
2542
2543         return 0;
2544 }
2545
2546 static void lan8814_ptp_clock_step(struct phy_device *phydev,
2547                                    s64 time_step_ns)
2548 {
2549         u32 nano_seconds_step;
2550         u64 abs_time_step_ns;
2551         u32 unsigned_seconds;
2552         u32 nano_seconds;
2553         u32 remainder;
2554         s32 seconds;
2555
2556         if (time_step_ns >  15000000000LL) {
2557                 /* convert to clock set */
2558                 lan8814_ptp_clock_get(phydev, &unsigned_seconds, &nano_seconds);
2559                 unsigned_seconds += div_u64_rem(time_step_ns, 1000000000LL,
2560                                                 &remainder);
2561                 nano_seconds += remainder;
2562                 if (nano_seconds >= 1000000000) {
2563                         unsigned_seconds++;
2564                         nano_seconds -= 1000000000;
2565                 }
2566                 lan8814_ptp_clock_set(phydev, unsigned_seconds, nano_seconds);
2567                 return;
2568         } else if (time_step_ns < -15000000000LL) {
2569                 /* convert to clock set */
2570                 time_step_ns = -time_step_ns;
2571
2572                 lan8814_ptp_clock_get(phydev, &unsigned_seconds, &nano_seconds);
2573                 unsigned_seconds -= div_u64_rem(time_step_ns, 1000000000LL,
2574                                                 &remainder);
2575                 nano_seconds_step = remainder;
2576                 if (nano_seconds < nano_seconds_step) {
2577                         unsigned_seconds--;
2578                         nano_seconds += 1000000000;
2579                 }
2580                 nano_seconds -= nano_seconds_step;
2581                 lan8814_ptp_clock_set(phydev, unsigned_seconds,
2582                                       nano_seconds);
2583                 return;
2584         }
2585
2586         /* do clock step */
2587         if (time_step_ns >= 0) {
2588                 abs_time_step_ns = (u64)time_step_ns;
2589                 seconds = (s32)div_u64_rem(abs_time_step_ns, 1000000000,
2590                                            &remainder);
2591                 nano_seconds = remainder;
2592         } else {
2593                 abs_time_step_ns = (u64)(-time_step_ns);
2594                 seconds = -((s32)div_u64_rem(abs_time_step_ns, 1000000000,
2595                             &remainder));
2596                 nano_seconds = remainder;
2597                 if (nano_seconds > 0) {
2598                         /* subtracting nano seconds is not allowed
2599                          * convert to subtracting from seconds,
2600                          * and adding to nanoseconds
2601                          */
2602                         seconds--;
2603                         nano_seconds = (1000000000 - nano_seconds);
2604                 }
2605         }
2606
2607         if (nano_seconds > 0) {
2608                 /* add 8 ns to cover the likely normal increment */
2609                 nano_seconds += 8;
2610         }
2611
2612         if (nano_seconds >= 1000000000) {
2613                 /* carry into seconds */
2614                 seconds++;
2615                 nano_seconds -= 1000000000;
2616         }
2617
2618         while (seconds) {
2619                 if (seconds > 0) {
2620                         u32 adjustment_value = (u32)seconds;
2621                         u16 adjustment_value_lo, adjustment_value_hi;
2622
2623                         if (adjustment_value > 0xF)
2624                                 adjustment_value = 0xF;
2625
2626                         adjustment_value_lo = adjustment_value & 0xffff;
2627                         adjustment_value_hi = (adjustment_value >> 16) & 0x3fff;
2628
2629                         lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
2630                                               adjustment_value_lo);
2631                         lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
2632                                               PTP_LTC_STEP_ADJ_DIR_ |
2633                                               adjustment_value_hi);
2634                         seconds -= ((s32)adjustment_value);
2635                 } else {
2636                         u32 adjustment_value = (u32)(-seconds);
2637                         u16 adjustment_value_lo, adjustment_value_hi;
2638
2639                         if (adjustment_value > 0xF)
2640                                 adjustment_value = 0xF;
2641
2642                         adjustment_value_lo = adjustment_value & 0xffff;
2643                         adjustment_value_hi = (adjustment_value >> 16) & 0x3fff;
2644
2645                         lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
2646                                               adjustment_value_lo);
2647                         lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
2648                                               adjustment_value_hi);
2649                         seconds += ((s32)adjustment_value);
2650                 }
2651                 lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL,
2652                                       PTP_CMD_CTL_PTP_LTC_STEP_SEC_);
2653         }
2654         if (nano_seconds) {
2655                 u16 nano_seconds_lo;
2656                 u16 nano_seconds_hi;
2657
2658                 nano_seconds_lo = nano_seconds & 0xffff;
2659                 nano_seconds_hi = (nano_seconds >> 16) & 0x3fff;
2660
2661                 lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
2662                                       nano_seconds_lo);
2663                 lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
2664                                       PTP_LTC_STEP_ADJ_DIR_ |
2665                                       nano_seconds_hi);
2666                 lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL,
2667                                       PTP_CMD_CTL_PTP_LTC_STEP_NSEC_);
2668         }
2669 }
2670
2671 static int lan8814_ptpci_adjtime(struct ptp_clock_info *ptpci, s64 delta)
2672 {
2673         struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
2674                                                           ptp_clock_info);
2675         struct phy_device *phydev = shared->phydev;
2676
2677         mutex_lock(&shared->shared_lock);
2678         lan8814_ptp_clock_step(phydev, delta);
2679         mutex_unlock(&shared->shared_lock);
2680
2681         return 0;
2682 }
2683
2684 static int lan8814_ptpci_adjfine(struct ptp_clock_info *ptpci, long scaled_ppm)
2685 {
2686         struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
2687                                                           ptp_clock_info);
2688         struct phy_device *phydev = shared->phydev;
2689         u16 kszphy_rate_adj_lo, kszphy_rate_adj_hi;
2690         bool positive = true;
2691         u32 kszphy_rate_adj;
2692
2693         if (scaled_ppm < 0) {
2694                 scaled_ppm = -scaled_ppm;
2695                 positive = false;
2696         }
2697
2698         kszphy_rate_adj = LAN8814_1PPM_FORMAT * (scaled_ppm >> 16);
2699         kszphy_rate_adj += (LAN8814_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
2700
2701         kszphy_rate_adj_lo = kszphy_rate_adj & 0xffff;
2702         kszphy_rate_adj_hi = (kszphy_rate_adj >> 16) & 0x3fff;
2703
2704         if (positive)
2705                 kszphy_rate_adj_hi |= PTP_CLOCK_RATE_ADJ_DIR_;
2706
2707         mutex_lock(&shared->shared_lock);
2708         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_RATE_ADJ_HI, kszphy_rate_adj_hi);
2709         lanphy_write_page_reg(phydev, 4, PTP_CLOCK_RATE_ADJ_LO, kszphy_rate_adj_lo);
2710         mutex_unlock(&shared->shared_lock);
2711
2712         return 0;
2713 }
2714
2715 static void lan8814_get_sig_tx(struct sk_buff *skb, u16 *sig)
2716 {
2717         struct ptp_header *ptp_header;
2718         u32 type;
2719
2720         type = ptp_classify_raw(skb);
2721         ptp_header = ptp_parse_header(skb, type);
2722
2723         *sig = (__force u16)(ntohs(ptp_header->sequence_id));
2724 }
2725
2726 static void lan8814_match_tx_skb(struct kszphy_ptp_priv *ptp_priv,
2727                                  u32 seconds, u32 nsec, u16 seq_id)
2728 {
2729         struct skb_shared_hwtstamps shhwtstamps;
2730         struct sk_buff *skb, *skb_tmp;
2731         unsigned long flags;
2732         bool ret = false;
2733         u16 skb_sig;
2734
2735         spin_lock_irqsave(&ptp_priv->tx_queue.lock, flags);
2736         skb_queue_walk_safe(&ptp_priv->tx_queue, skb, skb_tmp) {
2737                 lan8814_get_sig_tx(skb, &skb_sig);
2738
2739                 if (memcmp(&skb_sig, &seq_id, sizeof(seq_id)))
2740                         continue;
2741
2742                 __skb_unlink(skb, &ptp_priv->tx_queue);
2743                 ret = true;
2744                 break;
2745         }
2746         spin_unlock_irqrestore(&ptp_priv->tx_queue.lock, flags);
2747
2748         if (ret) {
2749                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
2750                 shhwtstamps.hwtstamp = ktime_set(seconds, nsec);
2751                 skb_complete_tx_timestamp(skb, &shhwtstamps);
2752         }
2753 }
2754
2755 static void lan8814_dequeue_tx_skb(struct kszphy_ptp_priv *ptp_priv)
2756 {
2757         struct phy_device *phydev = ptp_priv->phydev;
2758         u32 seconds, nsec;
2759         u16 seq_id;
2760
2761         lan8814_ptp_tx_ts_get(phydev, &seconds, &nsec, &seq_id);
2762         lan8814_match_tx_skb(ptp_priv, seconds, nsec, seq_id);
2763 }
2764
2765 static void lan8814_get_tx_ts(struct kszphy_ptp_priv *ptp_priv)
2766 {
2767         struct phy_device *phydev = ptp_priv->phydev;
2768         u32 reg;
2769
2770         do {
2771                 lan8814_dequeue_tx_skb(ptp_priv);
2772
2773                 /* If other timestamps are available in the FIFO,
2774                  * process them.
2775                  */
2776                 reg = lanphy_read_page_reg(phydev, 5, PTP_CAP_INFO);
2777         } while (PTP_CAP_INFO_TX_TS_CNT_GET_(reg) > 0);
2778 }
2779
2780 static bool lan8814_match_skb(struct kszphy_ptp_priv *ptp_priv,
2781                               struct lan8814_ptp_rx_ts *rx_ts)
2782 {
2783         struct skb_shared_hwtstamps *shhwtstamps;
2784         struct sk_buff *skb, *skb_tmp;
2785         unsigned long flags;
2786         bool ret = false;
2787         u16 skb_sig;
2788
2789         spin_lock_irqsave(&ptp_priv->rx_queue.lock, flags);
2790         skb_queue_walk_safe(&ptp_priv->rx_queue, skb, skb_tmp) {
2791                 lan8814_get_sig_rx(skb, &skb_sig);
2792
2793                 if (memcmp(&skb_sig, &rx_ts->seq_id, sizeof(rx_ts->seq_id)))
2794                         continue;
2795
2796                 __skb_unlink(skb, &ptp_priv->rx_queue);
2797
2798                 ret = true;
2799                 break;
2800         }
2801         spin_unlock_irqrestore(&ptp_priv->rx_queue.lock, flags);
2802
2803         if (ret) {
2804                 shhwtstamps = skb_hwtstamps(skb);
2805                 memset(shhwtstamps, 0, sizeof(*shhwtstamps));
2806                 shhwtstamps->hwtstamp = ktime_set(rx_ts->seconds, rx_ts->nsec);
2807                 netif_rx(skb);
2808         }
2809
2810         return ret;
2811 }
2812
2813 static void lan8814_match_rx_ts(struct kszphy_ptp_priv *ptp_priv,
2814                                 struct lan8814_ptp_rx_ts *rx_ts)
2815 {
2816         unsigned long flags;
2817
2818         /* If we failed to match the skb add it to the queue for when
2819          * the frame will come
2820          */
2821         if (!lan8814_match_skb(ptp_priv, rx_ts)) {
2822                 spin_lock_irqsave(&ptp_priv->rx_ts_lock, flags);
2823                 list_add(&rx_ts->list, &ptp_priv->rx_ts_list);
2824                 spin_unlock_irqrestore(&ptp_priv->rx_ts_lock, flags);
2825         } else {
2826                 kfree(rx_ts);
2827         }
2828 }
2829
2830 static void lan8814_get_rx_ts(struct kszphy_ptp_priv *ptp_priv)
2831 {
2832         struct phy_device *phydev = ptp_priv->phydev;
2833         struct lan8814_ptp_rx_ts *rx_ts;
2834         u32 reg;
2835
2836         do {
2837                 rx_ts = kzalloc(sizeof(*rx_ts), GFP_KERNEL);
2838                 if (!rx_ts)
2839                         return;
2840
2841                 lan8814_ptp_rx_ts_get(phydev, &rx_ts->seconds, &rx_ts->nsec,
2842                                       &rx_ts->seq_id);
2843                 lan8814_match_rx_ts(ptp_priv, rx_ts);
2844
2845                 /* If other timestamps are available in the FIFO,
2846                  * process them.
2847                  */
2848                 reg = lanphy_read_page_reg(phydev, 5, PTP_CAP_INFO);
2849         } while (PTP_CAP_INFO_RX_TS_CNT_GET_(reg) > 0);
2850 }
2851
2852 static void lan8814_handle_ptp_interrupt(struct phy_device *phydev, u16 status)
2853 {
2854         struct kszphy_priv *priv = phydev->priv;
2855         struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
2856
2857         if (status & PTP_TSU_INT_STS_PTP_TX_TS_EN_)
2858                 lan8814_get_tx_ts(ptp_priv);
2859
2860         if (status & PTP_TSU_INT_STS_PTP_RX_TS_EN_)
2861                 lan8814_get_rx_ts(ptp_priv);
2862
2863         if (status & PTP_TSU_INT_STS_PTP_TX_TS_OVRFL_INT_) {
2864                 lan8814_flush_fifo(phydev, true);
2865                 skb_queue_purge(&ptp_priv->tx_queue);
2866         }
2867
2868         if (status & PTP_TSU_INT_STS_PTP_RX_TS_OVRFL_INT_) {
2869                 lan8814_flush_fifo(phydev, false);
2870                 skb_queue_purge(&ptp_priv->rx_queue);
2871         }
2872 }
2873
2874 static int lan8804_config_init(struct phy_device *phydev)
2875 {
2876         int val;
2877
2878         /* MDI-X setting for swap A,B transmit */
2879         val = lanphy_read_page_reg(phydev, 2, LAN8804_ALIGN_SWAP);
2880         val &= ~LAN8804_ALIGN_TX_A_B_SWAP_MASK;
2881         val |= LAN8804_ALIGN_TX_A_B_SWAP;
2882         lanphy_write_page_reg(phydev, 2, LAN8804_ALIGN_SWAP, val);
2883
2884         /* Make sure that the PHY will not stop generating the clock when the
2885          * link partner goes down
2886          */
2887         lanphy_write_page_reg(phydev, 31, LAN8814_CLOCK_MANAGEMENT, 0x27e);
2888         lanphy_read_page_reg(phydev, 1, LAN8814_LINK_QUALITY);
2889
2890         return 0;
2891 }
2892
2893 static irqreturn_t lan8804_handle_interrupt(struct phy_device *phydev)
2894 {
2895         int status;
2896
2897         status = phy_read(phydev, LAN8814_INTS);
2898         if (status < 0) {
2899                 phy_error(phydev);
2900                 return IRQ_NONE;
2901         }
2902
2903         if (status > 0)
2904                 phy_trigger_machine(phydev);
2905
2906         return IRQ_HANDLED;
2907 }
2908
2909 #define LAN8804_OUTPUT_CONTROL                  25
2910 #define LAN8804_OUTPUT_CONTROL_INTR_BUFFER      BIT(14)
2911 #define LAN8804_CONTROL                         31
2912 #define LAN8804_CONTROL_INTR_POLARITY           BIT(14)
2913
2914 static int lan8804_config_intr(struct phy_device *phydev)
2915 {
2916         int err;
2917
2918         /* This is an internal PHY of lan966x and is not possible to change the
2919          * polarity on the GIC found in lan966x, therefore change the polarity
2920          * of the interrupt in the PHY from being active low instead of active
2921          * high.
2922          */
2923         phy_write(phydev, LAN8804_CONTROL, LAN8804_CONTROL_INTR_POLARITY);
2924
2925         /* By default interrupt buffer is open-drain in which case the interrupt
2926          * can be active only low. Therefore change the interrupt buffer to be
2927          * push-pull to be able to change interrupt polarity
2928          */
2929         phy_write(phydev, LAN8804_OUTPUT_CONTROL,
2930                   LAN8804_OUTPUT_CONTROL_INTR_BUFFER);
2931
2932         if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
2933                 err = phy_read(phydev, LAN8814_INTS);
2934                 if (err < 0)
2935                         return err;
2936
2937                 err = phy_write(phydev, LAN8814_INTC, LAN8814_INT_LINK);
2938                 if (err)
2939                         return err;
2940         } else {
2941                 err = phy_write(phydev, LAN8814_INTC, 0);
2942                 if (err)
2943                         return err;
2944
2945                 err = phy_read(phydev, LAN8814_INTS);
2946                 if (err < 0)
2947                         return err;
2948         }
2949
2950         return 0;
2951 }
2952
2953 static irqreturn_t lan8814_handle_interrupt(struct phy_device *phydev)
2954 {
2955         int ret = IRQ_NONE;
2956         int irq_status;
2957
2958         irq_status = phy_read(phydev, LAN8814_INTS);
2959         if (irq_status < 0) {
2960                 phy_error(phydev);
2961                 return IRQ_NONE;
2962         }
2963
2964         if (irq_status & LAN8814_INT_LINK) {
2965                 phy_trigger_machine(phydev);
2966                 ret = IRQ_HANDLED;
2967         }
2968
2969         while (true) {
2970                 irq_status = lanphy_read_page_reg(phydev, 5, PTP_TSU_INT_STS);
2971                 if (!irq_status)
2972                         break;
2973
2974                 lan8814_handle_ptp_interrupt(phydev, irq_status);
2975                 ret = IRQ_HANDLED;
2976         }
2977
2978         return ret;
2979 }
2980
2981 static int lan8814_ack_interrupt(struct phy_device *phydev)
2982 {
2983         /* bit[12..0] int status, which is a read and clear register. */
2984         int rc;
2985
2986         rc = phy_read(phydev, LAN8814_INTS);
2987
2988         return (rc < 0) ? rc : 0;
2989 }
2990
2991 static int lan8814_config_intr(struct phy_device *phydev)
2992 {
2993         int err;
2994
2995         lanphy_write_page_reg(phydev, 4, LAN8814_INTR_CTRL_REG,
2996                               LAN8814_INTR_CTRL_REG_POLARITY |
2997                               LAN8814_INTR_CTRL_REG_INTR_ENABLE);
2998
2999         /* enable / disable interrupts */
3000         if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
3001                 err = lan8814_ack_interrupt(phydev);
3002                 if (err)
3003                         return err;
3004
3005                 err = phy_write(phydev, LAN8814_INTC, LAN8814_INT_LINK);
3006         } else {
3007                 err = phy_write(phydev, LAN8814_INTC, 0);
3008                 if (err)
3009                         return err;
3010
3011                 err = lan8814_ack_interrupt(phydev);
3012         }
3013
3014         return err;
3015 }
3016
3017 static void lan8814_ptp_init(struct phy_device *phydev)
3018 {
3019         struct kszphy_priv *priv = phydev->priv;
3020         struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
3021         u32 temp;
3022
3023         if (!IS_ENABLED(CONFIG_PTP_1588_CLOCK) ||
3024             !IS_ENABLED(CONFIG_NETWORK_PHY_TIMESTAMPING))
3025                 return;
3026
3027         lanphy_write_page_reg(phydev, 5, TSU_HARD_RESET, TSU_HARD_RESET_);
3028
3029         temp = lanphy_read_page_reg(phydev, 5, PTP_TX_MOD);
3030         temp |= PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_;
3031         lanphy_write_page_reg(phydev, 5, PTP_TX_MOD, temp);
3032
3033         temp = lanphy_read_page_reg(phydev, 5, PTP_RX_MOD);
3034         temp |= PTP_RX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_;
3035         lanphy_write_page_reg(phydev, 5, PTP_RX_MOD, temp);
3036
3037         lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_CONFIG, 0);
3038         lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_CONFIG, 0);
3039
3040         /* Removing default registers configs related to L2 and IP */
3041         lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_L2_ADDR_EN, 0);
3042         lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_L2_ADDR_EN, 0);
3043         lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_IP_ADDR_EN, 0);
3044         lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_IP_ADDR_EN, 0);
3045
3046         skb_queue_head_init(&ptp_priv->tx_queue);
3047         skb_queue_head_init(&ptp_priv->rx_queue);
3048         INIT_LIST_HEAD(&ptp_priv->rx_ts_list);
3049         spin_lock_init(&ptp_priv->rx_ts_lock);
3050
3051         ptp_priv->phydev = phydev;
3052
3053         ptp_priv->mii_ts.rxtstamp = lan8814_rxtstamp;
3054         ptp_priv->mii_ts.txtstamp = lan8814_txtstamp;
3055         ptp_priv->mii_ts.hwtstamp = lan8814_hwtstamp;
3056         ptp_priv->mii_ts.ts_info  = lan8814_ts_info;
3057
3058         phydev->mii_ts = &ptp_priv->mii_ts;
3059 }
3060
3061 static int lan8814_ptp_probe_once(struct phy_device *phydev)
3062 {
3063         struct lan8814_shared_priv *shared = phydev->shared->priv;
3064
3065         /* Initialise shared lock for clock*/
3066         mutex_init(&shared->shared_lock);
3067
3068         shared->ptp_clock_info.owner = THIS_MODULE;
3069         snprintf(shared->ptp_clock_info.name, 30, "%s", phydev->drv->name);
3070         shared->ptp_clock_info.max_adj = 31249999;
3071         shared->ptp_clock_info.n_alarm = 0;
3072         shared->ptp_clock_info.n_ext_ts = 0;
3073         shared->ptp_clock_info.n_pins = 0;
3074         shared->ptp_clock_info.pps = 0;
3075         shared->ptp_clock_info.pin_config = NULL;
3076         shared->ptp_clock_info.adjfine = lan8814_ptpci_adjfine;
3077         shared->ptp_clock_info.adjtime = lan8814_ptpci_adjtime;
3078         shared->ptp_clock_info.gettime64 = lan8814_ptpci_gettime64;
3079         shared->ptp_clock_info.settime64 = lan8814_ptpci_settime64;
3080         shared->ptp_clock_info.getcrosststamp = NULL;
3081
3082         shared->ptp_clock = ptp_clock_register(&shared->ptp_clock_info,
3083                                                &phydev->mdio.dev);
3084         if (IS_ERR(shared->ptp_clock)) {
3085                 phydev_err(phydev, "ptp_clock_register failed %lu\n",
3086                            PTR_ERR(shared->ptp_clock));
3087                 return -EINVAL;
3088         }
3089
3090         /* Check if PHC support is missing at the configuration level */
3091         if (!shared->ptp_clock)
3092                 return 0;
3093
3094         phydev_dbg(phydev, "successfully registered ptp clock\n");
3095
3096         shared->phydev = phydev;
3097
3098         /* The EP.4 is shared between all the PHYs in the package and also it
3099          * can be accessed by any of the PHYs
3100          */
3101         lanphy_write_page_reg(phydev, 4, LTC_HARD_RESET, LTC_HARD_RESET_);
3102         lanphy_write_page_reg(phydev, 4, PTP_OPERATING_MODE,
3103                               PTP_OPERATING_MODE_STANDALONE_);
3104
3105         return 0;
3106 }
3107
3108 static void lan8814_setup_led(struct phy_device *phydev, int val)
3109 {
3110         int temp;
3111
3112         temp = lanphy_read_page_reg(phydev, 5, LAN8814_LED_CTRL_1);
3113
3114         if (val)
3115                 temp |= LAN8814_LED_CTRL_1_KSZ9031_LED_MODE_;
3116         else
3117                 temp &= ~LAN8814_LED_CTRL_1_KSZ9031_LED_MODE_;
3118
3119         lanphy_write_page_reg(phydev, 5, LAN8814_LED_CTRL_1, temp);
3120 }
3121
3122 static int lan8814_config_init(struct phy_device *phydev)
3123 {
3124         struct kszphy_priv *lan8814 = phydev->priv;
3125         int val;
3126
3127         /* Reset the PHY */
3128         val = lanphy_read_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET);
3129         val |= LAN8814_QSGMII_SOFT_RESET_BIT;
3130         lanphy_write_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET, val);
3131
3132         /* Disable ANEG with QSGMII PCS Host side */
3133         val = lanphy_read_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG);
3134         val &= ~LAN8814_QSGMII_PCS1G_ANEG_CONFIG_ANEG_ENA;
3135         lanphy_write_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG, val);
3136
3137         /* MDI-X setting for swap A,B transmit */
3138         val = lanphy_read_page_reg(phydev, 2, LAN8814_ALIGN_SWAP);
3139         val &= ~LAN8814_ALIGN_TX_A_B_SWAP_MASK;
3140         val |= LAN8814_ALIGN_TX_A_B_SWAP;
3141         lanphy_write_page_reg(phydev, 2, LAN8814_ALIGN_SWAP, val);
3142
3143         if (lan8814->led_mode >= 0)
3144                 lan8814_setup_led(phydev, lan8814->led_mode);
3145
3146         return 0;
3147 }
3148
3149 /* It is expected that there will not be any 'lan8814_take_coma_mode'
3150  * function called in suspend. Because the GPIO line can be shared, so if one of
3151  * the phys goes back in coma mode, then all the other PHYs will go, which is
3152  * wrong.
3153  */
3154 static int lan8814_release_coma_mode(struct phy_device *phydev)
3155 {
3156         struct gpio_desc *gpiod;
3157
3158         gpiod = devm_gpiod_get_optional(&phydev->mdio.dev, "coma-mode",
3159                                         GPIOD_OUT_HIGH_OPEN_DRAIN |
3160                                         GPIOD_FLAGS_BIT_NONEXCLUSIVE);
3161         if (IS_ERR(gpiod))
3162                 return PTR_ERR(gpiod);
3163
3164         gpiod_set_consumer_name(gpiod, "LAN8814 coma mode");
3165         gpiod_set_value_cansleep(gpiod, 0);
3166
3167         return 0;
3168 }
3169
3170 static int lan8814_probe(struct phy_device *phydev)
3171 {
3172         const struct kszphy_type *type = phydev->drv->driver_data;
3173         struct kszphy_priv *priv;
3174         u16 addr;
3175         int err;
3176
3177         priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
3178         if (!priv)
3179                 return -ENOMEM;
3180
3181         phydev->priv = priv;
3182
3183         priv->type = type;
3184
3185         kszphy_parse_led_mode(phydev);
3186
3187         /* Strap-in value for PHY address, below register read gives starting
3188          * phy address value
3189          */
3190         addr = lanphy_read_page_reg(phydev, 4, 0) & 0x1F;
3191         devm_phy_package_join(&phydev->mdio.dev, phydev,
3192                               addr, sizeof(struct lan8814_shared_priv));
3193
3194         if (phy_package_init_once(phydev)) {
3195                 err = lan8814_release_coma_mode(phydev);
3196                 if (err)
3197                         return err;
3198
3199                 err = lan8814_ptp_probe_once(phydev);
3200                 if (err)
3201                         return err;
3202         }
3203
3204         lan8814_ptp_init(phydev);
3205
3206         return 0;
3207 }
3208
3209 #define LAN8841_MMD_TIMER_REG                   0
3210 #define LAN8841_MMD0_REGISTER_17                17
3211 #define LAN8841_MMD0_REGISTER_17_DROP_OPT(x)    ((x) & 0x3)
3212 #define LAN8841_MMD0_REGISTER_17_XMIT_TOG_TX_DIS        BIT(3)
3213 #define LAN8841_OPERATION_MODE_STRAP_OVERRIDE_LOW_REG   2
3214 #define LAN8841_OPERATION_MODE_STRAP_OVERRIDE_LOW_REG_MAGJACK   BIT(14)
3215 #define LAN8841_MMD_ANALOG_REG                  28
3216 #define LAN8841_ANALOG_CONTROL_1                1
3217 #define LAN8841_ANALOG_CONTROL_1_PLL_TRIM(x)    (((x) & 0x3) << 5)
3218 #define LAN8841_ANALOG_CONTROL_10               13
3219 #define LAN8841_ANALOG_CONTROL_10_PLL_DIV(x)    ((x) & 0x3)
3220 #define LAN8841_ANALOG_CONTROL_11               14
3221 #define LAN8841_ANALOG_CONTROL_11_LDO_REF(x)    (((x) & 0x7) << 12)
3222 #define LAN8841_TX_LOW_I_CH_C_D_POWER_MANAGMENT 69
3223 #define LAN8841_TX_LOW_I_CH_C_D_POWER_MANAGMENT_VAL 0xbffc
3224 #define LAN8841_BTRX_POWER_DOWN                 70
3225 #define LAN8841_BTRX_POWER_DOWN_QBIAS_CH_A      BIT(0)
3226 #define LAN8841_BTRX_POWER_DOWN_BTRX_CH_A       BIT(1)
3227 #define LAN8841_BTRX_POWER_DOWN_QBIAS_CH_B      BIT(2)
3228 #define LAN8841_BTRX_POWER_DOWN_BTRX_CH_B       BIT(3)
3229 #define LAN8841_BTRX_POWER_DOWN_BTRX_CH_C       BIT(5)
3230 #define LAN8841_BTRX_POWER_DOWN_BTRX_CH_D       BIT(7)
3231 #define LAN8841_ADC_CHANNEL_MASK                198
3232 #define LAN8841_PTP_RX_PARSE_L2_ADDR_EN         370
3233 #define LAN8841_PTP_RX_PARSE_IP_ADDR_EN         371
3234 #define LAN8841_PTP_TX_PARSE_L2_ADDR_EN         434
3235 #define LAN8841_PTP_TX_PARSE_IP_ADDR_EN         435
3236 #define LAN8841_PTP_CMD_CTL                     256
3237 #define LAN8841_PTP_CMD_CTL_PTP_ENABLE          BIT(2)
3238 #define LAN8841_PTP_CMD_CTL_PTP_DISABLE         BIT(1)
3239 #define LAN8841_PTP_CMD_CTL_PTP_RESET           BIT(0)
3240 #define LAN8841_PTP_RX_PARSE_CONFIG             368
3241 #define LAN8841_PTP_TX_PARSE_CONFIG             432
3242
3243 static int lan8841_config_init(struct phy_device *phydev)
3244 {
3245         int ret;
3246
3247         ret = ksz9131_config_init(phydev);
3248         if (ret)
3249                 return ret;
3250
3251         /* Initialize the HW by resetting everything */
3252         phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3253                        LAN8841_PTP_CMD_CTL,
3254                        LAN8841_PTP_CMD_CTL_PTP_RESET,
3255                        LAN8841_PTP_CMD_CTL_PTP_RESET);
3256
3257         phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3258                        LAN8841_PTP_CMD_CTL,
3259                        LAN8841_PTP_CMD_CTL_PTP_ENABLE,
3260                        LAN8841_PTP_CMD_CTL_PTP_ENABLE);
3261
3262         /* Don't process any frames */
3263         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3264                       LAN8841_PTP_RX_PARSE_CONFIG, 0);
3265         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3266                       LAN8841_PTP_TX_PARSE_CONFIG, 0);
3267         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3268                       LAN8841_PTP_TX_PARSE_L2_ADDR_EN, 0);
3269         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3270                       LAN8841_PTP_RX_PARSE_L2_ADDR_EN, 0);
3271         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3272                       LAN8841_PTP_TX_PARSE_IP_ADDR_EN, 0);
3273         phy_write_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3274                       LAN8841_PTP_RX_PARSE_IP_ADDR_EN, 0);
3275
3276         /* 100BT Clause 40 improvenent errata */
3277         phy_write_mmd(phydev, LAN8841_MMD_ANALOG_REG,
3278                       LAN8841_ANALOG_CONTROL_1,
3279                       LAN8841_ANALOG_CONTROL_1_PLL_TRIM(0x2));
3280         phy_write_mmd(phydev, LAN8841_MMD_ANALOG_REG,
3281                       LAN8841_ANALOG_CONTROL_10,
3282                       LAN8841_ANALOG_CONTROL_10_PLL_DIV(0x1));
3283
3284         /* 10M/100M Ethernet Signal Tuning Errata for Shorted-Center Tap
3285          * Magnetics
3286          */
3287         ret = phy_read_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3288                            LAN8841_OPERATION_MODE_STRAP_OVERRIDE_LOW_REG);
3289         if (ret & LAN8841_OPERATION_MODE_STRAP_OVERRIDE_LOW_REG_MAGJACK) {
3290                 phy_write_mmd(phydev, LAN8841_MMD_ANALOG_REG,
3291                               LAN8841_TX_LOW_I_CH_C_D_POWER_MANAGMENT,
3292                               LAN8841_TX_LOW_I_CH_C_D_POWER_MANAGMENT_VAL);
3293                 phy_write_mmd(phydev, LAN8841_MMD_ANALOG_REG,
3294                               LAN8841_BTRX_POWER_DOWN,
3295                               LAN8841_BTRX_POWER_DOWN_QBIAS_CH_A |
3296                               LAN8841_BTRX_POWER_DOWN_BTRX_CH_A |
3297                               LAN8841_BTRX_POWER_DOWN_QBIAS_CH_B |
3298                               LAN8841_BTRX_POWER_DOWN_BTRX_CH_B |
3299                               LAN8841_BTRX_POWER_DOWN_BTRX_CH_C |
3300                               LAN8841_BTRX_POWER_DOWN_BTRX_CH_D);
3301         }
3302
3303         /* LDO Adjustment errata */
3304         phy_write_mmd(phydev, LAN8841_MMD_ANALOG_REG,
3305                       LAN8841_ANALOG_CONTROL_11,
3306                       LAN8841_ANALOG_CONTROL_11_LDO_REF(1));
3307
3308         /* 100BT RGMII latency tuning errata */
3309         phy_write_mmd(phydev, MDIO_MMD_PMAPMD,
3310                       LAN8841_ADC_CHANNEL_MASK, 0x0);
3311         phy_write_mmd(phydev, LAN8841_MMD_TIMER_REG,
3312                       LAN8841_MMD0_REGISTER_17,
3313                       LAN8841_MMD0_REGISTER_17_DROP_OPT(2) |
3314                       LAN8841_MMD0_REGISTER_17_XMIT_TOG_TX_DIS);
3315
3316         return 0;
3317 }
3318
3319 #define LAN8841_OUTPUT_CTRL                     25
3320 #define LAN8841_OUTPUT_CTRL_INT_BUFFER          BIT(14)
3321 #define LAN8841_INT_PTP                         BIT(9)
3322
3323 static int lan8841_config_intr(struct phy_device *phydev)
3324 {
3325         int err;
3326
3327         phy_modify(phydev, LAN8841_OUTPUT_CTRL,
3328                    LAN8841_OUTPUT_CTRL_INT_BUFFER, 0);
3329
3330         if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
3331                 err = phy_read(phydev, LAN8814_INTS);
3332                 if (err)
3333                         return err;
3334
3335                 /* Enable / disable interrupts. It is OK to enable PTP interrupt
3336                  * even if it PTP is not enabled. Because the underneath blocks
3337                  * will not enable the PTP so we will never get the PTP
3338                  * interrupt.
3339                  */
3340                 err = phy_write(phydev, LAN8814_INTC,
3341                                 LAN8814_INT_LINK | LAN8841_INT_PTP);
3342         } else {
3343                 err = phy_write(phydev, LAN8814_INTC, 0);
3344                 if (err)
3345                         return err;
3346
3347                 err = phy_read(phydev, LAN8814_INTS);
3348         }
3349
3350         return err;
3351 }
3352
3353 #define LAN8841_PTP_TX_EGRESS_SEC_LO                    453
3354 #define LAN8841_PTP_TX_EGRESS_SEC_HI                    452
3355 #define LAN8841_PTP_TX_EGRESS_NS_LO                     451
3356 #define LAN8841_PTP_TX_EGRESS_NS_HI                     450
3357 #define LAN8841_PTP_TX_EGRESS_NSEC_HI_VALID             BIT(15)
3358 #define LAN8841_PTP_TX_MSG_HEADER2                      455
3359
3360 static bool lan8841_ptp_get_tx_ts(struct kszphy_ptp_priv *ptp_priv,
3361                                   u32 *sec, u32 *nsec, u16 *seq)
3362 {
3363         struct phy_device *phydev = ptp_priv->phydev;
3364
3365         *nsec = phy_read_mmd(phydev, 2, LAN8841_PTP_TX_EGRESS_NS_HI);
3366         if (!(*nsec & LAN8841_PTP_TX_EGRESS_NSEC_HI_VALID))
3367                 return false;
3368
3369         *nsec = ((*nsec & 0x3fff) << 16);
3370         *nsec = *nsec | phy_read_mmd(phydev, 2, LAN8841_PTP_TX_EGRESS_NS_LO);
3371
3372         *sec = phy_read_mmd(phydev, 2, LAN8841_PTP_TX_EGRESS_SEC_HI);
3373         *sec = *sec << 16;
3374         *sec = *sec | phy_read_mmd(phydev, 2, LAN8841_PTP_TX_EGRESS_SEC_LO);
3375
3376         *seq = phy_read_mmd(phydev, 2, LAN8841_PTP_TX_MSG_HEADER2);
3377
3378         return true;
3379 }
3380
3381 static void lan8841_ptp_process_tx_ts(struct kszphy_ptp_priv *ptp_priv)
3382 {
3383         u32 sec, nsec;
3384         u16 seq;
3385
3386         while (lan8841_ptp_get_tx_ts(ptp_priv, &sec, &nsec, &seq))
3387                 lan8814_match_tx_skb(ptp_priv, sec, nsec, seq);
3388 }
3389
3390 #define LAN8841_PTP_RX_INGRESS_SEC_LO                   389
3391 #define LAN8841_PTP_RX_INGRESS_SEC_HI                   388
3392 #define LAN8841_PTP_RX_INGRESS_NS_LO                    387
3393 #define LAN8841_PTP_RX_INGRESS_NS_HI                    386
3394 #define LAN8841_PTP_RX_INGRESS_NSEC_HI_VALID            BIT(15)
3395 #define LAN8841_PTP_RX_MSG_HEADER2                      391
3396
3397 static struct lan8814_ptp_rx_ts *lan8841_ptp_get_rx_ts(struct kszphy_ptp_priv *ptp_priv)
3398 {
3399         struct phy_device *phydev = ptp_priv->phydev;
3400         struct lan8814_ptp_rx_ts *rx_ts;
3401         u32 sec, nsec;
3402         u16 seq;
3403
3404         nsec = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_NS_HI);
3405         if (!(nsec & LAN8841_PTP_RX_INGRESS_NSEC_HI_VALID))
3406                 return NULL;
3407
3408         nsec = ((nsec & 0x3fff) << 16);
3409         nsec = nsec | phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_NS_LO);
3410
3411         sec = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_SEC_HI);
3412         sec = sec << 16;
3413         sec = sec | phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_SEC_LO);
3414
3415         seq = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_MSG_HEADER2);
3416
3417         rx_ts = kzalloc(sizeof(*rx_ts), GFP_KERNEL);
3418         if (!rx_ts)
3419                 return NULL;
3420
3421         rx_ts->seconds = sec;
3422         rx_ts->nsec = nsec;
3423         rx_ts->seq_id = seq;
3424
3425         return rx_ts;
3426 }
3427
3428 static void lan8841_ptp_process_rx_ts(struct kszphy_ptp_priv *ptp_priv)
3429 {
3430         struct lan8814_ptp_rx_ts *rx_ts;
3431
3432         while ((rx_ts = lan8841_ptp_get_rx_ts(ptp_priv)) != NULL)
3433                 lan8814_match_rx_ts(ptp_priv, rx_ts);
3434 }
3435
3436 #define LAN8841_PTP_INT_STS                     259
3437 #define LAN8841_PTP_INT_STS_PTP_TX_TS_OVRFL_INT BIT(13)
3438 #define LAN8841_PTP_INT_STS_PTP_TX_TS_INT       BIT(12)
3439 #define LAN8841_PTP_INT_STS_PTP_RX_TS_OVRFL_INT BIT(9)
3440 #define LAN8841_PTP_INT_STS_PTP_RX_TS_INT       BIT(8)
3441
3442 static void lan8841_ptp_flush_fifo(struct kszphy_ptp_priv *ptp_priv, bool egress)
3443 {
3444         struct phy_device *phydev = ptp_priv->phydev;
3445         int i;
3446
3447         for (i = 0; i < FIFO_SIZE; ++i)
3448                 phy_read_mmd(phydev, 2,
3449                              egress ? LAN8841_PTP_TX_MSG_HEADER2 :
3450                                       LAN8841_PTP_RX_MSG_HEADER2);
3451
3452         phy_read_mmd(phydev, 2, LAN8841_PTP_INT_STS);
3453 }
3454
3455 static void lan8841_handle_ptp_interrupt(struct phy_device *phydev)
3456 {
3457         struct kszphy_priv *priv = phydev->priv;
3458         struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
3459         u16 status;
3460
3461         do {
3462                 status = phy_read_mmd(phydev, 2, LAN8841_PTP_INT_STS);
3463                 if (status & LAN8841_PTP_INT_STS_PTP_TX_TS_INT)
3464                         lan8841_ptp_process_tx_ts(ptp_priv);
3465
3466                 if (status & LAN8841_PTP_INT_STS_PTP_RX_TS_INT)
3467                         lan8841_ptp_process_rx_ts(ptp_priv);
3468
3469                 if (status & LAN8841_PTP_INT_STS_PTP_TX_TS_OVRFL_INT) {
3470                         lan8841_ptp_flush_fifo(ptp_priv, true);
3471                         skb_queue_purge(&ptp_priv->tx_queue);
3472                 }
3473
3474                 if (status & LAN8841_PTP_INT_STS_PTP_RX_TS_OVRFL_INT) {
3475                         lan8841_ptp_flush_fifo(ptp_priv, false);
3476                         skb_queue_purge(&ptp_priv->rx_queue);
3477                 }
3478
3479         } while (status);
3480 }
3481
3482 #define LAN8841_INTS_PTP                BIT(9)
3483
3484 static irqreturn_t lan8841_handle_interrupt(struct phy_device *phydev)
3485 {
3486         irqreturn_t ret = IRQ_NONE;
3487         int irq_status;
3488
3489         irq_status = phy_read(phydev, LAN8814_INTS);
3490         if (irq_status < 0) {
3491                 phy_error(phydev);
3492                 return IRQ_NONE;
3493         }
3494
3495         if (irq_status & LAN8814_INT_LINK) {
3496                 phy_trigger_machine(phydev);
3497                 ret = IRQ_HANDLED;
3498         }
3499
3500         if (irq_status & LAN8841_INTS_PTP) {
3501                 lan8841_handle_ptp_interrupt(phydev);
3502                 ret = IRQ_HANDLED;
3503         }
3504
3505         return ret;
3506 }
3507
3508 static int lan8841_ts_info(struct mii_timestamper *mii_ts,
3509                            struct ethtool_ts_info *info)
3510 {
3511         struct kszphy_ptp_priv *ptp_priv;
3512
3513         ptp_priv = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
3514
3515         info->phc_index = ptp_priv->ptp_clock ?
3516                                 ptp_clock_index(ptp_priv->ptp_clock) : -1;
3517         if (info->phc_index == -1) {
3518                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
3519                                          SOF_TIMESTAMPING_RX_SOFTWARE |
3520                                          SOF_TIMESTAMPING_SOFTWARE;
3521                 return 0;
3522         }
3523
3524         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
3525                                 SOF_TIMESTAMPING_RX_HARDWARE |
3526                                 SOF_TIMESTAMPING_RAW_HARDWARE;
3527
3528         info->tx_types = (1 << HWTSTAMP_TX_OFF) |
3529                          (1 << HWTSTAMP_TX_ON) |
3530                          (1 << HWTSTAMP_TX_ONESTEP_SYNC);
3531
3532         info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
3533                            (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
3534                            (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
3535                            (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
3536
3537         return 0;
3538 }
3539
3540 #define LAN8841_PTP_INT_EN                      260
3541 #define LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN   BIT(13)
3542 #define LAN8841_PTP_INT_EN_PTP_TX_TS_EN         BIT(12)
3543 #define LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN   BIT(9)
3544 #define LAN8841_PTP_INT_EN_PTP_RX_TS_EN         BIT(8)
3545
3546 static void lan8841_ptp_enable_int(struct kszphy_ptp_priv *ptp_priv,
3547                                    bool enable)
3548 {
3549         struct phy_device *phydev = ptp_priv->phydev;
3550
3551         if (enable)
3552                 /* Enable interrupts */
3553                 phy_modify_mmd(phydev, 2, LAN8841_PTP_INT_EN,
3554                                LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
3555                                LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
3556                                LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
3557                                LAN8841_PTP_INT_EN_PTP_RX_TS_EN,
3558                                LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
3559                                LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
3560                                LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
3561                                LAN8841_PTP_INT_EN_PTP_RX_TS_EN);
3562         else
3563                 /* Disable interrupts */
3564                 phy_modify_mmd(phydev, 2, LAN8841_PTP_INT_EN,
3565                                LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
3566                                LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
3567                                LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
3568                                LAN8841_PTP_INT_EN_PTP_RX_TS_EN, 0);
3569 }
3570
3571 #define LAN8841_PTP_RX_TIMESTAMP_EN             379
3572 #define LAN8841_PTP_TX_TIMESTAMP_EN             443
3573 #define LAN8841_PTP_TX_MOD                      445
3574
3575 static int lan8841_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
3576 {
3577         struct kszphy_ptp_priv *ptp_priv = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
3578         struct phy_device *phydev = ptp_priv->phydev;
3579         struct lan8814_ptp_rx_ts *rx_ts, *tmp;
3580         struct hwtstamp_config config;
3581         int txcfg = 0, rxcfg = 0;
3582         int pkt_ts_enable;
3583
3584         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
3585                 return -EFAULT;
3586
3587         ptp_priv->hwts_tx_type = config.tx_type;
3588         ptp_priv->rx_filter = config.rx_filter;
3589
3590         switch (config.rx_filter) {
3591         case HWTSTAMP_FILTER_NONE:
3592                 ptp_priv->layer = 0;
3593                 ptp_priv->version = 0;
3594                 break;
3595         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
3596         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
3597         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
3598                 ptp_priv->layer = PTP_CLASS_L4;
3599                 ptp_priv->version = PTP_CLASS_V2;
3600                 break;
3601         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
3602         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
3603         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
3604                 ptp_priv->layer = PTP_CLASS_L2;
3605                 ptp_priv->version = PTP_CLASS_V2;
3606                 break;
3607         case HWTSTAMP_FILTER_PTP_V2_EVENT:
3608         case HWTSTAMP_FILTER_PTP_V2_SYNC:
3609         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
3610                 ptp_priv->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
3611                 ptp_priv->version = PTP_CLASS_V2;
3612                 break;
3613         default:
3614                 return -ERANGE;
3615         }
3616
3617         /* Setup parsing of the frames and enable the timestamping for ptp
3618          * frames
3619          */
3620         if (ptp_priv->layer & PTP_CLASS_L2) {
3621                 rxcfg |= PTP_RX_PARSE_CONFIG_LAYER2_EN_;
3622                 txcfg |= PTP_TX_PARSE_CONFIG_LAYER2_EN_;
3623         } else if (ptp_priv->layer & PTP_CLASS_L4) {
3624                 rxcfg |= PTP_RX_PARSE_CONFIG_IPV4_EN_ | PTP_RX_PARSE_CONFIG_IPV6_EN_;
3625                 txcfg |= PTP_TX_PARSE_CONFIG_IPV4_EN_ | PTP_TX_PARSE_CONFIG_IPV6_EN_;
3626         }
3627
3628         phy_write_mmd(phydev, 2, LAN8841_PTP_RX_PARSE_CONFIG, rxcfg);
3629         phy_write_mmd(phydev, 2, LAN8841_PTP_TX_PARSE_CONFIG, txcfg);
3630
3631         pkt_ts_enable = PTP_TIMESTAMP_EN_SYNC_ | PTP_TIMESTAMP_EN_DREQ_ |
3632                         PTP_TIMESTAMP_EN_PDREQ_ | PTP_TIMESTAMP_EN_PDRES_;
3633         phy_write_mmd(phydev, 2, LAN8841_PTP_RX_TIMESTAMP_EN, pkt_ts_enable);
3634         phy_write_mmd(phydev, 2, LAN8841_PTP_TX_TIMESTAMP_EN, pkt_ts_enable);
3635
3636         /* Enable / disable of the TX timestamp in the SYNC frames */
3637         phy_modify_mmd(phydev, 2, LAN8841_PTP_TX_MOD,
3638                        PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_,
3639                        ptp_priv->hwts_tx_type == HWTSTAMP_TX_ONESTEP_SYNC ?
3640                                 PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ : 0);
3641
3642         /* Now enable/disable the timestamping */
3643         lan8841_ptp_enable_int(ptp_priv,
3644                                config.rx_filter != HWTSTAMP_FILTER_NONE);
3645
3646         /* In case of multiple starts and stops, these needs to be cleared */
3647         list_for_each_entry_safe(rx_ts, tmp, &ptp_priv->rx_ts_list, list) {
3648                 list_del(&rx_ts->list);
3649                 kfree(rx_ts);
3650         }
3651
3652         skb_queue_purge(&ptp_priv->rx_queue);
3653         skb_queue_purge(&ptp_priv->tx_queue);
3654
3655         lan8841_ptp_flush_fifo(ptp_priv, false);
3656         lan8841_ptp_flush_fifo(ptp_priv, true);
3657
3658         return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? -EFAULT : 0;
3659 }
3660
3661 #define LAN8841_PTP_LTC_SET_SEC_HI      262
3662 #define LAN8841_PTP_LTC_SET_SEC_MID     263
3663 #define LAN8841_PTP_LTC_SET_SEC_LO      264
3664 #define LAN8841_PTP_LTC_SET_NS_HI       265
3665 #define LAN8841_PTP_LTC_SET_NS_LO       266
3666 #define LAN8841_PTP_CMD_CTL_PTP_LTC_LOAD        BIT(4)
3667
3668 static int lan8841_ptp_settime64(struct ptp_clock_info *ptp,
3669                                  const struct timespec64 *ts)
3670 {
3671         struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
3672                                                         ptp_clock_info);
3673         struct phy_device *phydev = ptp_priv->phydev;
3674
3675         /* Set the value to be stored */
3676         mutex_lock(&ptp_priv->ptp_lock);
3677         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_SET_SEC_LO, lower_16_bits(ts->tv_sec));
3678         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_SET_SEC_MID, upper_16_bits(ts->tv_sec));
3679         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_SET_SEC_HI, upper_32_bits(ts->tv_sec) & 0xffff);
3680         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_SET_NS_LO, lower_16_bits(ts->tv_nsec));
3681         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_SET_NS_HI, upper_16_bits(ts->tv_nsec) & 0x3fff);
3682
3683         /* Set the command to load the LTC */
3684         phy_write_mmd(phydev, 2, LAN8841_PTP_CMD_CTL,
3685                       LAN8841_PTP_CMD_CTL_PTP_LTC_LOAD);
3686         mutex_unlock(&ptp_priv->ptp_lock);
3687
3688         return 0;
3689 }
3690
3691 #define LAN8841_PTP_LTC_RD_SEC_HI       358
3692 #define LAN8841_PTP_LTC_RD_SEC_MID      359
3693 #define LAN8841_PTP_LTC_RD_SEC_LO       360
3694 #define LAN8841_PTP_LTC_RD_NS_HI        361
3695 #define LAN8841_PTP_LTC_RD_NS_LO        362
3696 #define LAN8841_PTP_CMD_CTL_PTP_LTC_READ        BIT(3)
3697
3698 static int lan8841_ptp_gettime64(struct ptp_clock_info *ptp,
3699                                  struct timespec64 *ts)
3700 {
3701         struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
3702                                                         ptp_clock_info);
3703         struct phy_device *phydev = ptp_priv->phydev;
3704         time64_t s;
3705         s64 ns;
3706
3707         mutex_lock(&ptp_priv->ptp_lock);
3708         /* Issue the command to read the LTC */
3709         phy_write_mmd(phydev, 2, LAN8841_PTP_CMD_CTL,
3710                       LAN8841_PTP_CMD_CTL_PTP_LTC_READ);
3711
3712         /* Read the LTC */
3713         s = phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_HI);
3714         s <<= 16;
3715         s |= phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_MID);
3716         s <<= 16;
3717         s |= phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_LO);
3718
3719         ns = phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_NS_HI) & 0x3fff;
3720         ns <<= 16;
3721         ns |= phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_NS_LO);
3722         mutex_unlock(&ptp_priv->ptp_lock);
3723
3724         set_normalized_timespec64(ts, s, ns);
3725         return 0;
3726 }
3727
3728 #define LAN8841_PTP_LTC_STEP_ADJ_LO                     276
3729 #define LAN8841_PTP_LTC_STEP_ADJ_HI                     275
3730 #define LAN8841_PTP_LTC_STEP_ADJ_DIR                    BIT(15)
3731 #define LAN8841_PTP_CMD_CTL_PTP_LTC_STEP_SECONDS        BIT(5)
3732 #define LAN8841_PTP_CMD_CTL_PTP_LTC_STEP_NANOSECONDS    BIT(6)
3733
3734 static int lan8841_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
3735 {
3736         struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
3737                                                         ptp_clock_info);
3738         struct phy_device *phydev = ptp_priv->phydev;
3739         struct timespec64 ts;
3740         bool add = true;
3741         u32 nsec;
3742         s32 sec;
3743
3744         /* The HW allows up to 15 sec to adjust the time, but here we limit to
3745          * 10 sec the adjustment. The reason is, in case the adjustment is 14
3746          * sec and 999999999 nsec, then we add 8ns to compansate the actual
3747          * increment so the value can be bigger than 15 sec. Therefore limit the
3748          * possible adjustments so we will not have these corner cases
3749          */
3750         if (delta > 10000000000LL || delta < -10000000000LL) {
3751                 /* The timeadjustment is too big, so fall back using set time */
3752                 u64 now;
3753
3754                 ptp->gettime64(ptp, &ts);
3755
3756                 now = ktime_to_ns(timespec64_to_ktime(ts));
3757                 ts = ns_to_timespec64(now + delta);
3758
3759                 ptp->settime64(ptp, &ts);
3760                 return 0;
3761         }
3762
3763         sec = div_u64_rem(delta < 0 ? -delta : delta, NSEC_PER_SEC, &nsec);
3764         if (delta < 0 && nsec != 0) {
3765                 /* It is not allowed to adjust low the nsec part, therefore
3766                  * subtract more from second part and add to nanosecond such
3767                  * that would roll over, so the second part will increase
3768                  */
3769                 sec--;
3770                 nsec = NSEC_PER_SEC - nsec;
3771         }
3772
3773         /* Calculate the adjustments and the direction */
3774         if (delta < 0)
3775                 add = false;
3776
3777         if (nsec > 0)
3778                 /* add 8 ns to cover the likely normal increment */
3779                 nsec += 8;
3780
3781         if (nsec >= NSEC_PER_SEC) {
3782                 /* carry into seconds */
3783                 sec++;
3784                 nsec -= NSEC_PER_SEC;
3785         }
3786
3787         mutex_lock(&ptp_priv->ptp_lock);
3788         if (sec) {
3789                 phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_STEP_ADJ_LO, sec);
3790                 phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_STEP_ADJ_HI,
3791                               add ? LAN8841_PTP_LTC_STEP_ADJ_DIR : 0);
3792                 phy_write_mmd(phydev, 2, LAN8841_PTP_CMD_CTL,
3793                               LAN8841_PTP_CMD_CTL_PTP_LTC_STEP_SECONDS);
3794         }
3795
3796         if (nsec) {
3797                 phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_STEP_ADJ_LO,
3798                               nsec & 0xffff);
3799                 phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_STEP_ADJ_HI,
3800                               (nsec >> 16) & 0x3fff);
3801                 phy_write_mmd(phydev, 2, LAN8841_PTP_CMD_CTL,
3802                               LAN8841_PTP_CMD_CTL_PTP_LTC_STEP_NANOSECONDS);
3803         }
3804         mutex_unlock(&ptp_priv->ptp_lock);
3805
3806         return 0;
3807 }
3808
3809 #define LAN8841_PTP_LTC_RATE_ADJ_HI             269
3810 #define LAN8841_PTP_LTC_RATE_ADJ_HI_DIR         BIT(15)
3811 #define LAN8841_PTP_LTC_RATE_ADJ_LO             270
3812
3813 static int lan8841_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
3814 {
3815         struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
3816                                                         ptp_clock_info);
3817         struct phy_device *phydev = ptp_priv->phydev;
3818         bool faster = true;
3819         u32 rate;
3820
3821         if (!scaled_ppm)
3822                 return 0;
3823
3824         if (scaled_ppm < 0) {
3825                 scaled_ppm = -scaled_ppm;
3826                 faster = false;
3827         }
3828
3829         rate = LAN8814_1PPM_FORMAT * (upper_16_bits(scaled_ppm));
3830         rate += (LAN8814_1PPM_FORMAT * (lower_16_bits(scaled_ppm))) >> 16;
3831
3832         mutex_lock(&ptp_priv->ptp_lock);
3833         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_RATE_ADJ_HI,
3834                       faster ? LAN8841_PTP_LTC_RATE_ADJ_HI_DIR | (upper_16_bits(rate) & 0x3fff)
3835                              : upper_16_bits(rate) & 0x3fff);
3836         phy_write_mmd(phydev, 2, LAN8841_PTP_LTC_RATE_ADJ_LO, lower_16_bits(rate));
3837         mutex_unlock(&ptp_priv->ptp_lock);
3838
3839         return 0;
3840 }
3841
3842 static struct ptp_clock_info lan8841_ptp_clock_info = {
3843         .owner          = THIS_MODULE,
3844         .name           = "lan8841 ptp",
3845         .max_adj        = 31249999,
3846         .gettime64      = lan8841_ptp_gettime64,
3847         .settime64      = lan8841_ptp_settime64,
3848         .adjtime        = lan8841_ptp_adjtime,
3849         .adjfine        = lan8841_ptp_adjfine,
3850 };
3851
3852 #define LAN8841_OPERATION_MODE_STRAP_LOW_REGISTER 3
3853 #define LAN8841_OPERATION_MODE_STRAP_LOW_REGISTER_STRAP_RGMII_EN BIT(0)
3854
3855 static int lan8841_probe(struct phy_device *phydev)
3856 {
3857         struct kszphy_ptp_priv *ptp_priv;
3858         struct kszphy_priv *priv;
3859         int err;
3860
3861         err = kszphy_probe(phydev);
3862         if (err)
3863                 return err;
3864
3865         if (phy_read_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
3866                          LAN8841_OPERATION_MODE_STRAP_LOW_REGISTER) &
3867             LAN8841_OPERATION_MODE_STRAP_LOW_REGISTER_STRAP_RGMII_EN)
3868                 phydev->interface = PHY_INTERFACE_MODE_RGMII_RXID;
3869
3870         /* Register the clock */
3871         if (!IS_ENABLED(CONFIG_NETWORK_PHY_TIMESTAMPING))
3872                 return 0;
3873
3874         priv = phydev->priv;
3875         ptp_priv = &priv->ptp_priv;
3876
3877         ptp_priv->ptp_clock_info = lan8841_ptp_clock_info;
3878         ptp_priv->ptp_clock = ptp_clock_register(&ptp_priv->ptp_clock_info,
3879                                                  &phydev->mdio.dev);
3880         if (IS_ERR(ptp_priv->ptp_clock)) {
3881                 phydev_err(phydev, "ptp_clock_register failed: %lu\n",
3882                            PTR_ERR(ptp_priv->ptp_clock));
3883                 return -EINVAL;
3884         }
3885
3886         if (!ptp_priv->ptp_clock)
3887                 return 0;
3888
3889         /* Initialize the SW */
3890         skb_queue_head_init(&ptp_priv->tx_queue);
3891         skb_queue_head_init(&ptp_priv->rx_queue);
3892         INIT_LIST_HEAD(&ptp_priv->rx_ts_list);
3893         spin_lock_init(&ptp_priv->rx_ts_lock);
3894         ptp_priv->phydev = phydev;
3895         mutex_init(&ptp_priv->ptp_lock);
3896
3897         ptp_priv->mii_ts.rxtstamp = lan8814_rxtstamp;
3898         ptp_priv->mii_ts.txtstamp = lan8814_txtstamp;
3899         ptp_priv->mii_ts.hwtstamp = lan8841_hwtstamp;
3900         ptp_priv->mii_ts.ts_info = lan8841_ts_info;
3901
3902         phydev->mii_ts = &ptp_priv->mii_ts;
3903
3904         return 0;
3905 }
3906
3907 static struct phy_driver ksphy_driver[] = {
3908 {
3909         .phy_id         = PHY_ID_KS8737,
3910         .phy_id_mask    = MICREL_PHY_ID_MASK,
3911         .name           = "Micrel KS8737",
3912         /* PHY_BASIC_FEATURES */
3913         .driver_data    = &ks8737_type,
3914         .probe          = kszphy_probe,
3915         .config_init    = kszphy_config_init,
3916         .config_intr    = kszphy_config_intr,
3917         .handle_interrupt = kszphy_handle_interrupt,
3918         .suspend        = kszphy_suspend,
3919         .resume         = kszphy_resume,
3920 }, {
3921         .phy_id         = PHY_ID_KSZ8021,
3922         .phy_id_mask    = 0x00ffffff,
3923         .name           = "Micrel KSZ8021 or KSZ8031",
3924         /* PHY_BASIC_FEATURES */
3925         .driver_data    = &ksz8021_type,
3926         .probe          = kszphy_probe,
3927         .config_init    = kszphy_config_init,
3928         .config_intr    = kszphy_config_intr,
3929         .handle_interrupt = kszphy_handle_interrupt,
3930         .get_sset_count = kszphy_get_sset_count,
3931         .get_strings    = kszphy_get_strings,
3932         .get_stats      = kszphy_get_stats,
3933         .suspend        = kszphy_suspend,
3934         .resume         = kszphy_resume,
3935 }, {
3936         .phy_id         = PHY_ID_KSZ8031,
3937         .phy_id_mask    = 0x00ffffff,
3938         .name           = "Micrel KSZ8031",
3939         /* PHY_BASIC_FEATURES */
3940         .driver_data    = &ksz8021_type,
3941         .probe          = kszphy_probe,
3942         .config_init    = kszphy_config_init,
3943         .config_intr    = kszphy_config_intr,
3944         .handle_interrupt = kszphy_handle_interrupt,
3945         .get_sset_count = kszphy_get_sset_count,
3946         .get_strings    = kszphy_get_strings,
3947         .get_stats      = kszphy_get_stats,
3948         .suspend        = kszphy_suspend,
3949         .resume         = kszphy_resume,
3950 }, {
3951         .phy_id         = PHY_ID_KSZ8041,
3952         .phy_id_mask    = MICREL_PHY_ID_MASK,
3953         .name           = "Micrel KSZ8041",
3954         /* PHY_BASIC_FEATURES */
3955         .driver_data    = &ksz8041_type,
3956         .probe          = kszphy_probe,
3957         .config_init    = ksz8041_config_init,
3958         .config_aneg    = ksz8041_config_aneg,
3959         .config_intr    = kszphy_config_intr,
3960         .handle_interrupt = kszphy_handle_interrupt,
3961         .get_sset_count = kszphy_get_sset_count,
3962         .get_strings    = kszphy_get_strings,
3963         .get_stats      = kszphy_get_stats,
3964         /* No suspend/resume callbacks because of errata DS80000700A,
3965          * receiver error following software power down.
3966          */
3967 }, {
3968         .phy_id         = PHY_ID_KSZ8041RNLI,
3969         .phy_id_mask    = MICREL_PHY_ID_MASK,
3970         .name           = "Micrel KSZ8041RNLI",
3971         /* PHY_BASIC_FEATURES */
3972         .driver_data    = &ksz8041_type,
3973         .probe          = kszphy_probe,
3974         .config_init    = kszphy_config_init,
3975         .config_intr    = kszphy_config_intr,
3976         .handle_interrupt = kszphy_handle_interrupt,
3977         .get_sset_count = kszphy_get_sset_count,
3978         .get_strings    = kszphy_get_strings,
3979         .get_stats      = kszphy_get_stats,
3980         .suspend        = kszphy_suspend,
3981         .resume         = kszphy_resume,
3982 }, {
3983         .name           = "Micrel KSZ8051",
3984         /* PHY_BASIC_FEATURES */
3985         .driver_data    = &ksz8051_type,
3986         .probe          = kszphy_probe,
3987         .config_init    = kszphy_config_init,
3988         .config_intr    = kszphy_config_intr,
3989         .handle_interrupt = kszphy_handle_interrupt,
3990         .get_sset_count = kszphy_get_sset_count,
3991         .get_strings    = kszphy_get_strings,
3992         .get_stats      = kszphy_get_stats,
3993         .match_phy_device = ksz8051_match_phy_device,
3994         .suspend        = kszphy_suspend,
3995         .resume         = kszphy_resume,
3996 }, {
3997         .phy_id         = PHY_ID_KSZ8001,
3998         .name           = "Micrel KSZ8001 or KS8721",
3999         .phy_id_mask    = 0x00fffffc,
4000         /* PHY_BASIC_FEATURES */
4001         .driver_data    = &ksz8041_type,
4002         .probe          = kszphy_probe,
4003         .config_init    = kszphy_config_init,
4004         .config_intr    = kszphy_config_intr,
4005         .handle_interrupt = kszphy_handle_interrupt,
4006         .get_sset_count = kszphy_get_sset_count,
4007         .get_strings    = kszphy_get_strings,
4008         .get_stats      = kszphy_get_stats,
4009         .suspend        = kszphy_suspend,
4010         .resume         = kszphy_resume,
4011 }, {
4012         .phy_id         = PHY_ID_KSZ8081,
4013         .name           = "Micrel KSZ8081 or KSZ8091",
4014         .phy_id_mask    = MICREL_PHY_ID_MASK,
4015         .flags          = PHY_POLL_CABLE_TEST,
4016         /* PHY_BASIC_FEATURES */
4017         .driver_data    = &ksz8081_type,
4018         .probe          = kszphy_probe,
4019         .config_init    = ksz8081_config_init,
4020         .soft_reset     = genphy_soft_reset,
4021         .config_aneg    = ksz8081_config_aneg,
4022         .read_status    = ksz8081_read_status,
4023         .config_intr    = kszphy_config_intr,
4024         .handle_interrupt = kszphy_handle_interrupt,
4025         .get_sset_count = kszphy_get_sset_count,
4026         .get_strings    = kszphy_get_strings,
4027         .get_stats      = kszphy_get_stats,
4028         .suspend        = kszphy_suspend,
4029         .resume         = kszphy_resume,
4030         .cable_test_start       = ksz886x_cable_test_start,
4031         .cable_test_get_status  = ksz886x_cable_test_get_status,
4032 }, {
4033         .phy_id         = PHY_ID_KSZ8061,
4034         .name           = "Micrel KSZ8061",
4035         .phy_id_mask    = MICREL_PHY_ID_MASK,
4036         /* PHY_BASIC_FEATURES */
4037         .probe          = kszphy_probe,
4038         .config_init    = ksz8061_config_init,
4039         .config_intr    = kszphy_config_intr,
4040         .handle_interrupt = kszphy_handle_interrupt,
4041         .suspend        = kszphy_suspend,
4042         .resume         = kszphy_resume,
4043 }, {
4044         .phy_id         = PHY_ID_KSZ9021,
4045         .phy_id_mask    = 0x000ffffe,
4046         .name           = "Micrel KSZ9021 Gigabit PHY",
4047         /* PHY_GBIT_FEATURES */
4048         .driver_data    = &ksz9021_type,
4049         .probe          = kszphy_probe,
4050         .get_features   = ksz9031_get_features,
4051         .config_init    = ksz9021_config_init,
4052         .config_intr    = kszphy_config_intr,
4053         .handle_interrupt = kszphy_handle_interrupt,
4054         .get_sset_count = kszphy_get_sset_count,
4055         .get_strings    = kszphy_get_strings,
4056         .get_stats      = kszphy_get_stats,
4057         .suspend        = kszphy_suspend,
4058         .resume         = kszphy_resume,
4059         .read_mmd       = genphy_read_mmd_unsupported,
4060         .write_mmd      = genphy_write_mmd_unsupported,
4061 }, {
4062         .phy_id         = PHY_ID_KSZ9031,
4063         .phy_id_mask    = MICREL_PHY_ID_MASK,
4064         .name           = "Micrel KSZ9031 Gigabit PHY",
4065         .flags          = PHY_POLL_CABLE_TEST,
4066         .driver_data    = &ksz9021_type,
4067         .probe          = kszphy_probe,
4068         .get_features   = ksz9031_get_features,
4069         .config_init    = ksz9031_config_init,
4070         .soft_reset     = genphy_soft_reset,
4071         .read_status    = ksz9031_read_status,
4072         .config_intr    = kszphy_config_intr,
4073         .handle_interrupt = kszphy_handle_interrupt,
4074         .get_sset_count = kszphy_get_sset_count,
4075         .get_strings    = kszphy_get_strings,
4076         .get_stats      = kszphy_get_stats,
4077         .suspend        = kszphy_suspend,
4078         .resume         = kszphy_resume,
4079         .cable_test_start       = ksz9x31_cable_test_start,
4080         .cable_test_get_status  = ksz9x31_cable_test_get_status,
4081 }, {
4082         .phy_id         = PHY_ID_LAN8814,
4083         .phy_id_mask    = MICREL_PHY_ID_MASK,
4084         .name           = "Microchip INDY Gigabit Quad PHY",
4085         .flags          = PHY_POLL_CABLE_TEST,
4086         .config_init    = lan8814_config_init,
4087         .driver_data    = &lan8814_type,
4088         .probe          = lan8814_probe,
4089         .soft_reset     = genphy_soft_reset,
4090         .read_status    = ksz9031_read_status,
4091         .get_sset_count = kszphy_get_sset_count,
4092         .get_strings    = kszphy_get_strings,
4093         .get_stats      = kszphy_get_stats,
4094         .suspend        = genphy_suspend,
4095         .resume         = kszphy_resume,
4096         .config_intr    = lan8814_config_intr,
4097         .handle_interrupt = lan8814_handle_interrupt,
4098         .cable_test_start       = lan8814_cable_test_start,
4099         .cable_test_get_status  = ksz886x_cable_test_get_status,
4100 }, {
4101         .phy_id         = PHY_ID_LAN8804,
4102         .phy_id_mask    = MICREL_PHY_ID_MASK,
4103         .name           = "Microchip LAN966X Gigabit PHY",
4104         .config_init    = lan8804_config_init,
4105         .driver_data    = &ksz9021_type,
4106         .probe          = kszphy_probe,
4107         .soft_reset     = genphy_soft_reset,
4108         .read_status    = ksz9031_read_status,
4109         .get_sset_count = kszphy_get_sset_count,
4110         .get_strings    = kszphy_get_strings,
4111         .get_stats      = kszphy_get_stats,
4112         .suspend        = genphy_suspend,
4113         .resume         = kszphy_resume,
4114         .config_intr    = lan8804_config_intr,
4115         .handle_interrupt = lan8804_handle_interrupt,
4116 }, {
4117         .phy_id         = PHY_ID_LAN8841,
4118         .phy_id_mask    = MICREL_PHY_ID_MASK,
4119         .name           = "Microchip LAN8841 Gigabit PHY",
4120         .flags          = PHY_POLL_CABLE_TEST,
4121         .driver_data    = &lan8841_type,
4122         .config_init    = lan8841_config_init,
4123         .probe          = lan8841_probe,
4124         .soft_reset     = genphy_soft_reset,
4125         .config_intr    = lan8841_config_intr,
4126         .handle_interrupt = lan8841_handle_interrupt,
4127         .get_sset_count = kszphy_get_sset_count,
4128         .get_strings    = kszphy_get_strings,
4129         .get_stats      = kszphy_get_stats,
4130         .suspend        = genphy_suspend,
4131         .resume         = genphy_resume,
4132         .cable_test_start       = lan8814_cable_test_start,
4133         .cable_test_get_status  = ksz886x_cable_test_get_status,
4134 }, {
4135         .phy_id         = PHY_ID_KSZ9131,
4136         .phy_id_mask    = MICREL_PHY_ID_MASK,
4137         .name           = "Microchip KSZ9131 Gigabit PHY",
4138         /* PHY_GBIT_FEATURES */
4139         .flags          = PHY_POLL_CABLE_TEST,
4140         .driver_data    = &ksz9131_type,
4141         .probe          = kszphy_probe,
4142         .config_init    = ksz9131_config_init,
4143         .config_intr    = kszphy_config_intr,
4144         .config_aneg    = ksz9131_config_aneg,
4145         .read_status    = ksz9131_read_status,
4146         .handle_interrupt = kszphy_handle_interrupt,
4147         .get_sset_count = kszphy_get_sset_count,
4148         .get_strings    = kszphy_get_strings,
4149         .get_stats      = kszphy_get_stats,
4150         .suspend        = kszphy_suspend,
4151         .resume         = kszphy_resume,
4152         .cable_test_start       = ksz9x31_cable_test_start,
4153         .cable_test_get_status  = ksz9x31_cable_test_get_status,
4154         .get_features   = ksz9477_get_features,
4155 }, {
4156         .phy_id         = PHY_ID_KSZ8873MLL,
4157         .phy_id_mask    = MICREL_PHY_ID_MASK,
4158         .name           = "Micrel KSZ8873MLL Switch",
4159         /* PHY_BASIC_FEATURES */
4160         .config_init    = kszphy_config_init,
4161         .config_aneg    = ksz8873mll_config_aneg,
4162         .read_status    = ksz8873mll_read_status,
4163         .suspend        = genphy_suspend,
4164         .resume         = genphy_resume,
4165 }, {
4166         .phy_id         = PHY_ID_KSZ886X,
4167         .phy_id_mask    = MICREL_PHY_ID_MASK,
4168         .name           = "Micrel KSZ8851 Ethernet MAC or KSZ886X Switch",
4169         .driver_data    = &ksz886x_type,
4170         /* PHY_BASIC_FEATURES */
4171         .flags          = PHY_POLL_CABLE_TEST,
4172         .config_init    = kszphy_config_init,
4173         .config_aneg    = ksz886x_config_aneg,
4174         .read_status    = ksz886x_read_status,
4175         .suspend        = genphy_suspend,
4176         .resume         = genphy_resume,
4177         .cable_test_start       = ksz886x_cable_test_start,
4178         .cable_test_get_status  = ksz886x_cable_test_get_status,
4179 }, {
4180         .name           = "Micrel KSZ87XX Switch",
4181         /* PHY_BASIC_FEATURES */
4182         .config_init    = kszphy_config_init,
4183         .match_phy_device = ksz8795_match_phy_device,
4184         .suspend        = genphy_suspend,
4185         .resume         = genphy_resume,
4186 }, {
4187         .phy_id         = PHY_ID_KSZ9477,
4188         .phy_id_mask    = MICREL_PHY_ID_MASK,
4189         .name           = "Microchip KSZ9477",
4190         /* PHY_GBIT_FEATURES */
4191         .config_init    = kszphy_config_init,
4192         .config_intr    = kszphy_config_intr,
4193         .handle_interrupt = kszphy_handle_interrupt,
4194         .suspend        = genphy_suspend,
4195         .resume         = genphy_resume,
4196         .get_features   = ksz9477_get_features,
4197 } };
4198
4199 module_phy_driver(ksphy_driver);
4200
4201 MODULE_DESCRIPTION("Micrel PHY driver");
4202 MODULE_AUTHOR("David J. Choi");
4203 MODULE_LICENSE("GPL");
4204
4205 static struct mdio_device_id __maybe_unused micrel_tbl[] = {
4206         { PHY_ID_KSZ9021, 0x000ffffe },
4207         { PHY_ID_KSZ9031, MICREL_PHY_ID_MASK },
4208         { PHY_ID_KSZ9131, MICREL_PHY_ID_MASK },
4209         { PHY_ID_KSZ8001, 0x00fffffc },
4210         { PHY_ID_KS8737, MICREL_PHY_ID_MASK },
4211         { PHY_ID_KSZ8021, 0x00ffffff },
4212         { PHY_ID_KSZ8031, 0x00ffffff },
4213         { PHY_ID_KSZ8041, MICREL_PHY_ID_MASK },
4214         { PHY_ID_KSZ8051, MICREL_PHY_ID_MASK },
4215         { PHY_ID_KSZ8061, MICREL_PHY_ID_MASK },
4216         { PHY_ID_KSZ8081, MICREL_PHY_ID_MASK },
4217         { PHY_ID_KSZ8873MLL, MICREL_PHY_ID_MASK },
4218         { PHY_ID_KSZ886X, MICREL_PHY_ID_MASK },
4219         { PHY_ID_LAN8814, MICREL_PHY_ID_MASK },
4220         { PHY_ID_LAN8804, MICREL_PHY_ID_MASK },
4221         { PHY_ID_LAN8841, MICREL_PHY_ID_MASK },
4222         { }
4223 };
4224
4225 MODULE_DEVICE_TABLE(mdio, micrel_tbl);