/drivers/net/phy/marvell.c

Large files files are truncated, but you can click here to view the full file

   1// SPDX-License-Identifier: GPL-2.0+
   2/*
   3 * drivers/net/phy/marvell.c
   4 *
   5 * Driver for Marvell PHYs
   6 *
   7 * Author: Andy Fleming
   8 *
   9 * Copyright (c) 2004 Freescale Semiconductor, Inc.
  10 *
  11 * Copyright (c) 2013 Michael Stapelberg <michael@stapelberg.de>
  12 */
  13#include <linux/kernel.h>
  14#include <linux/string.h>
  15#include <linux/ctype.h>
  16#include <linux/errno.h>
  17#include <linux/unistd.h>
  18#include <linux/hwmon.h>
  19#include <linux/interrupt.h>
  20#include <linux/init.h>
  21#include <linux/delay.h>
  22#include <linux/netdevice.h>
  23#include <linux/etherdevice.h>
  24#include <linux/skbuff.h>
  25#include <linux/spinlock.h>
  26#include <linux/mm.h>
  27#include <linux/module.h>
  28#include <linux/mii.h>
  29#include <linux/ethtool.h>
  30#include <linux/phy.h>
  31#include <linux/marvell_phy.h>
  32#include <linux/bitfield.h>
  33#include <linux/of.h>
  34
  35#include <linux/io.h>
  36#include <asm/irq.h>
  37#include <linux/uaccess.h>
  38
  39#define MII_MARVELL_PHY_PAGE		22
  40#define MII_MARVELL_COPPER_PAGE		0x00
  41#define MII_MARVELL_FIBER_PAGE		0x01
  42#define MII_MARVELL_MSCR_PAGE		0x02
  43#define MII_MARVELL_LED_PAGE		0x03
  44#define MII_MARVELL_MISC_TEST_PAGE	0x06
  45#define MII_MARVELL_WOL_PAGE		0x11
  46
  47#define MII_M1011_IEVENT		0x13
  48#define MII_M1011_IEVENT_CLEAR		0x0000
  49
  50#define MII_M1011_IMASK			0x12
  51#define MII_M1011_IMASK_INIT		0x6400
  52#define MII_M1011_IMASK_CLEAR		0x0000
  53
  54#define MII_M1011_PHY_SCR			0x10
  55#define MII_M1011_PHY_SCR_DOWNSHIFT_EN		BIT(11)
  56#define MII_M1011_PHY_SCR_DOWNSHIFT_MASK	GENMASK(14, 12)
  57#define MII_M1011_PHY_SCR_DOWNSHIFT_MAX		8
  58#define MII_M1011_PHY_SCR_MDI			(0x0 << 5)
  59#define MII_M1011_PHY_SCR_MDI_X			(0x1 << 5)
  60#define MII_M1011_PHY_SCR_AUTO_CROSS		(0x3 << 5)
  61
  62#define MII_M1011_PHY_SSR			0x11
  63#define MII_M1011_PHY_SSR_DOWNSHIFT		BIT(5)
  64
  65#define MII_M1111_PHY_LED_CONTROL	0x18
  66#define MII_M1111_PHY_LED_DIRECT	0x4100
  67#define MII_M1111_PHY_LED_COMBINE	0x411c
  68#define MII_M1111_PHY_EXT_CR		0x14
  69#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK	GENMASK(11, 9)
  70#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_MAX	8
  71#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN	BIT(8)
  72#define MII_M1111_RGMII_RX_DELAY	BIT(7)
  73#define MII_M1111_RGMII_TX_DELAY	BIT(1)
  74#define MII_M1111_PHY_EXT_SR		0x1b
  75
  76#define MII_M1111_HWCFG_MODE_MASK		0xf
  77#define MII_M1111_HWCFG_MODE_FIBER_RGMII	0x3
  78#define MII_M1111_HWCFG_MODE_SGMII_NO_CLK	0x4
  79#define MII_M1111_HWCFG_MODE_RTBI		0x7
  80#define MII_M1111_HWCFG_MODE_COPPER_RTBI	0x9
  81#define MII_M1111_HWCFG_MODE_COPPER_RGMII	0xb
  82#define MII_M1111_HWCFG_FIBER_COPPER_RES	BIT(13)
  83#define MII_M1111_HWCFG_FIBER_COPPER_AUTO	BIT(15)
  84
  85#define MII_88E1121_PHY_MSCR_REG	21
  86#define MII_88E1121_PHY_MSCR_RX_DELAY	BIT(5)
  87#define MII_88E1121_PHY_MSCR_TX_DELAY	BIT(4)
  88#define MII_88E1121_PHY_MSCR_DELAY_MASK	(BIT(5) | BIT(4))
  89
  90#define MII_88E1121_MISC_TEST				0x1a
  91#define MII_88E1510_MISC_TEST_TEMP_THRESHOLD_MASK	0x1f00
  92#define MII_88E1510_MISC_TEST_TEMP_THRESHOLD_SHIFT	8
  93#define MII_88E1510_MISC_TEST_TEMP_IRQ_EN		BIT(7)
  94#define MII_88E1510_MISC_TEST_TEMP_IRQ			BIT(6)
  95#define MII_88E1121_MISC_TEST_TEMP_SENSOR_EN		BIT(5)
  96#define MII_88E1121_MISC_TEST_TEMP_MASK			0x1f
  97
  98#define MII_88E1510_TEMP_SENSOR		0x1b
  99#define MII_88E1510_TEMP_SENSOR_MASK	0xff
 100
 101#define MII_88E1540_COPPER_CTRL3	0x1a
 102#define MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_MASK	GENMASK(11, 10)
 103#define MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_00MS	0
 104#define MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_10MS	1
 105#define MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_20MS	2
 106#define MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_40MS	3
 107#define MII_88E1540_COPPER_CTRL3_FAST_LINK_DOWN		BIT(9)
 108
 109#define MII_88E6390_MISC_TEST		0x1b
 110#define MII_88E6390_MISC_TEST_SAMPLE_1S		0
 111#define MII_88E6390_MISC_TEST_SAMPLE_10MS	BIT(14)
 112#define MII_88E6390_MISC_TEST_SAMPLE_DISABLE	BIT(15)
 113#define MII_88E6390_MISC_TEST_SAMPLE_ENABLE	0
 114#define MII_88E6390_MISC_TEST_SAMPLE_MASK	(0x3 << 14)
 115
 116#define MII_88E6390_TEMP_SENSOR		0x1c
 117#define MII_88E6390_TEMP_SENSOR_MASK	0xff
 118#define MII_88E6390_TEMP_SENSOR_SAMPLES 10
 119
 120#define MII_88E1318S_PHY_MSCR1_REG	16
 121#define MII_88E1318S_PHY_MSCR1_PAD_ODD	BIT(6)
 122
 123/* Copper Specific Interrupt Enable Register */
 124#define MII_88E1318S_PHY_CSIER				0x12
 125/* WOL Event Interrupt Enable */
 126#define MII_88E1318S_PHY_CSIER_WOL_EIE			BIT(7)
 127
 128/* LED Timer Control Register */
 129#define MII_88E1318S_PHY_LED_TCR			0x12
 130#define MII_88E1318S_PHY_LED_TCR_FORCE_INT		BIT(15)
 131#define MII_88E1318S_PHY_LED_TCR_INTn_ENABLE		BIT(7)
 132#define MII_88E1318S_PHY_LED_TCR_INT_ACTIVE_LOW		BIT(11)
 133
 134/* Magic Packet MAC address registers */
 135#define MII_88E1318S_PHY_MAGIC_PACKET_WORD2		0x17
 136#define MII_88E1318S_PHY_MAGIC_PACKET_WORD1		0x18
 137#define MII_88E1318S_PHY_MAGIC_PACKET_WORD0		0x19
 138
 139#define MII_88E1318S_PHY_WOL_CTRL				0x10
 140#define MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS		BIT(12)
 141#define MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE	BIT(14)
 142
 143#define MII_PHY_LED_CTRL	        16
 144#define MII_88E1121_PHY_LED_DEF		0x0030
 145#define MII_88E1510_PHY_LED_DEF		0x1177
 146#define MII_88E1510_PHY_LED0_LINK_LED1_ACTIVE	0x1040
 147
 148#define MII_M1011_PHY_STATUS		0x11
 149#define MII_M1011_PHY_STATUS_1000	0x8000
 150#define MII_M1011_PHY_STATUS_100	0x4000
 151#define MII_M1011_PHY_STATUS_SPD_MASK	0xc000
 152#define MII_M1011_PHY_STATUS_FULLDUPLEX	0x2000
 153#define MII_M1011_PHY_STATUS_RESOLVED	0x0800
 154#define MII_M1011_PHY_STATUS_LINK	0x0400
 155
 156#define MII_88E3016_PHY_SPEC_CTRL	0x10
 157#define MII_88E3016_DISABLE_SCRAMBLER	0x0200
 158#define MII_88E3016_AUTO_MDIX_CROSSOVER	0x0030
 159
 160#define MII_88E1510_GEN_CTRL_REG_1		0x14
 161#define MII_88E1510_GEN_CTRL_REG_1_MODE_MASK	0x7
 162#define MII_88E1510_GEN_CTRL_REG_1_MODE_SGMII	0x1	/* SGMII to copper */
 163#define MII_88E1510_GEN_CTRL_REG_1_RESET	0x8000	/* Soft reset */
 164
 165#define LPA_PAUSE_FIBER		0x180
 166#define LPA_PAUSE_ASYM_FIBER	0x100
 167
 168#define NB_FIBER_STATS	1
 169
 170MODULE_DESCRIPTION("Marvell PHY driver");
 171MODULE_AUTHOR("Andy Fleming");
 172MODULE_LICENSE("GPL");
 173
 174struct marvell_hw_stat {
 175	const char *string;
 176	u8 page;
 177	u8 reg;
 178	u8 bits;
 179};
 180
 181static struct marvell_hw_stat marvell_hw_stats[] = {
 182	{ "phy_receive_errors_copper", 0, 21, 16},
 183	{ "phy_idle_errors", 0, 10, 8 },
 184	{ "phy_receive_errors_fiber", 1, 21, 16},
 185};
 186
 187struct marvell_priv {
 188	u64 stats[ARRAY_SIZE(marvell_hw_stats)];
 189	char *hwmon_name;
 190	struct device *hwmon_dev;
 191};
 192
 193static int marvell_read_page(struct phy_device *phydev)
 194{
 195	return __phy_read(phydev, MII_MARVELL_PHY_PAGE);
 196}
 197
 198static int marvell_write_page(struct phy_device *phydev, int page)
 199{
 200	return __phy_write(phydev, MII_MARVELL_PHY_PAGE, page);
 201}
 202
 203static int marvell_set_page(struct phy_device *phydev, int page)
 204{
 205	return phy_write(phydev, MII_MARVELL_PHY_PAGE, page);
 206}
 207
 208static int marvell_ack_interrupt(struct phy_device *phydev)
 209{
 210	int err;
 211
 212	/* Clear the interrupts by reading the reg */
 213	err = phy_read(phydev, MII_M1011_IEVENT);
 214
 215	if (err < 0)
 216		return err;
 217
 218	return 0;
 219}
 220
 221static int marvell_config_intr(struct phy_device *phydev)
 222{
 223	int err;
 224
 225	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
 226		err = phy_write(phydev, MII_M1011_IMASK,
 227				MII_M1011_IMASK_INIT);
 228	else
 229		err = phy_write(phydev, MII_M1011_IMASK,
 230				MII_M1011_IMASK_CLEAR);
 231
 232	return err;
 233}
 234
 235static int marvell_set_polarity(struct phy_device *phydev, int polarity)
 236{
 237	int reg;
 238	int err;
 239	int val;
 240
 241	/* get the current settings */
 242	reg = phy_read(phydev, MII_M1011_PHY_SCR);
 243	if (reg < 0)
 244		return reg;
 245
 246	val = reg;
 247	val &= ~MII_M1011_PHY_SCR_AUTO_CROSS;
 248	switch (polarity) {
 249	case ETH_TP_MDI:
 250		val |= MII_M1011_PHY_SCR_MDI;
 251		break;
 252	case ETH_TP_MDI_X:
 253		val |= MII_M1011_PHY_SCR_MDI_X;
 254		break;
 255	case ETH_TP_MDI_AUTO:
 256	case ETH_TP_MDI_INVALID:
 257	default:
 258		val |= MII_M1011_PHY_SCR_AUTO_CROSS;
 259		break;
 260	}
 261
 262	if (val != reg) {
 263		/* Set the new polarity value in the register */
 264		err = phy_write(phydev, MII_M1011_PHY_SCR, val);
 265		if (err)
 266			return err;
 267	}
 268
 269	return val != reg;
 270}
 271
 272static int marvell_config_aneg(struct phy_device *phydev)
 273{
 274	int changed = 0;
 275	int err;
 276
 277	err = marvell_set_polarity(phydev, phydev->mdix_ctrl);
 278	if (err < 0)
 279		return err;
 280
 281	changed = err;
 282
 283	err = phy_write(phydev, MII_M1111_PHY_LED_CONTROL,
 284			MII_M1111_PHY_LED_DIRECT);
 285	if (err < 0)
 286		return err;
 287
 288	err = genphy_config_aneg(phydev);
 289	if (err < 0)
 290		return err;
 291
 292	if (phydev->autoneg != AUTONEG_ENABLE || changed) {
 293		/* A write to speed/duplex bits (that is performed by
 294		 * genphy_config_aneg() call above) must be followed by
 295		 * a software reset. Otherwise, the write has no effect.
 296		 */
 297		err = genphy_soft_reset(phydev);
 298		if (err < 0)
 299			return err;
 300	}
 301
 302	return 0;
 303}
 304
 305static int m88e1101_config_aneg(struct phy_device *phydev)
 306{
 307	int err;
 308
 309	/* This Marvell PHY has an errata which requires
 310	 * that certain registers get written in order
 311	 * to restart autonegotiation
 312	 */
 313	err = genphy_soft_reset(phydev);
 314	if (err < 0)
 315		return err;
 316
 317	err = phy_write(phydev, 0x1d, 0x1f);
 318	if (err < 0)
 319		return err;
 320
 321	err = phy_write(phydev, 0x1e, 0x200c);
 322	if (err < 0)
 323		return err;
 324
 325	err = phy_write(phydev, 0x1d, 0x5);
 326	if (err < 0)
 327		return err;
 328
 329	err = phy_write(phydev, 0x1e, 0);
 330	if (err < 0)
 331		return err;
 332
 333	err = phy_write(phydev, 0x1e, 0x100);
 334	if (err < 0)
 335		return err;
 336
 337	return marvell_config_aneg(phydev);
 338}
 339
 340#ifdef CONFIG_OF_MDIO
 341/* Set and/or override some configuration registers based on the
 342 * marvell,reg-init property stored in the of_node for the phydev.
 343 *
 344 * marvell,reg-init = <reg-page reg mask value>,...;
 345 *
 346 * There may be one or more sets of <reg-page reg mask value>:
 347 *
 348 * reg-page: which register bank to use.
 349 * reg: the register.
 350 * mask: if non-zero, ANDed with existing register value.
 351 * value: ORed with the masked value and written to the regiser.
 352 *
 353 */
 354static int marvell_of_reg_init(struct phy_device *phydev)
 355{
 356	const __be32 *paddr;
 357	int len, i, saved_page, current_page, ret = 0;
 358
 359	if (!phydev->mdio.dev.of_node)
 360		return 0;
 361
 362	paddr = of_get_property(phydev->mdio.dev.of_node,
 363				"marvell,reg-init", &len);
 364	if (!paddr || len < (4 * sizeof(*paddr)))
 365		return 0;
 366
 367	saved_page = phy_save_page(phydev);
 368	if (saved_page < 0)
 369		goto err;
 370	current_page = saved_page;
 371
 372	len /= sizeof(*paddr);
 373	for (i = 0; i < len - 3; i += 4) {
 374		u16 page = be32_to_cpup(paddr + i);
 375		u16 reg = be32_to_cpup(paddr + i + 1);
 376		u16 mask = be32_to_cpup(paddr + i + 2);
 377		u16 val_bits = be32_to_cpup(paddr + i + 3);
 378		int val;
 379
 380		if (page != current_page) {
 381			current_page = page;
 382			ret = marvell_write_page(phydev, page);
 383			if (ret < 0)
 384				goto err;
 385		}
 386
 387		val = 0;
 388		if (mask) {
 389			val = __phy_read(phydev, reg);
 390			if (val < 0) {
 391				ret = val;
 392				goto err;
 393			}
 394			val &= mask;
 395		}
 396		val |= val_bits;
 397
 398		ret = __phy_write(phydev, reg, val);
 399		if (ret < 0)
 400			goto err;
 401	}
 402err:
 403	return phy_restore_page(phydev, saved_page, ret);
 404}
 405#else
 406static int marvell_of_reg_init(struct phy_device *phydev)
 407{
 408	return 0;
 409}
 410#endif /* CONFIG_OF_MDIO */
 411
 412static int m88e1121_config_aneg_rgmii_delays(struct phy_device *phydev)
 413{
 414	int mscr;
 415
 416	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
 417		mscr = MII_88E1121_PHY_MSCR_RX_DELAY |
 418		       MII_88E1121_PHY_MSCR_TX_DELAY;
 419	else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
 420		mscr = MII_88E1121_PHY_MSCR_RX_DELAY;
 421	else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
 422		mscr = MII_88E1121_PHY_MSCR_TX_DELAY;
 423	else
 424		mscr = 0;
 425
 426	return phy_modify_paged(phydev, MII_MARVELL_MSCR_PAGE,
 427				MII_88E1121_PHY_MSCR_REG,
 428				MII_88E1121_PHY_MSCR_DELAY_MASK, mscr);
 429}
 430
 431static int m88e1121_config_aneg(struct phy_device *phydev)
 432{
 433	int changed = 0;
 434	int err = 0;
 435
 436	if (phy_interface_is_rgmii(phydev)) {
 437		err = m88e1121_config_aneg_rgmii_delays(phydev);
 438		if (err < 0)
 439			return err;
 440	}
 441
 442	err = marvell_set_polarity(phydev, phydev->mdix_ctrl);
 443	if (err < 0)
 444		return err;
 445
 446	changed = err;
 447
 448	err = genphy_config_aneg(phydev);
 449	if (err < 0)
 450		return err;
 451
 452	if (phydev->autoneg != AUTONEG_ENABLE || changed) {
 453		/* A software reset is used to ensure a "commit" of the
 454		 * changes is done.
 455		 */
 456		err = genphy_soft_reset(phydev);
 457		if (err < 0)
 458			return err;
 459	}
 460
 461	return 0;
 462}
 463
 464static int m88e1318_config_aneg(struct phy_device *phydev)
 465{
 466	int err;
 467
 468	err = phy_modify_paged(phydev, MII_MARVELL_MSCR_PAGE,
 469			       MII_88E1318S_PHY_MSCR1_REG,
 470			       0, MII_88E1318S_PHY_MSCR1_PAD_ODD);
 471	if (err < 0)
 472		return err;
 473
 474	return m88e1121_config_aneg(phydev);
 475}
 476
 477/**
 478 * linkmode_adv_to_fiber_adv_t
 479 * @advertise: the linkmode advertisement settings
 480 *
 481 * A small helper function that translates linkmode advertisement
 482 * settings to phy autonegotiation advertisements for the MII_ADV
 483 * register for fiber link.
 484 */
 485static inline u32 linkmode_adv_to_fiber_adv_t(unsigned long *advertise)
 486{
 487	u32 result = 0;
 488
 489	if (linkmode_test_bit(ETHTOOL_LINK_MODE_1000baseT_Half_BIT, advertise))
 490		result |= ADVERTISE_1000XHALF;
 491	if (linkmode_test_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT, advertise))
 492		result |= ADVERTISE_1000XFULL;
 493
 494	if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, advertise) &&
 495	    linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertise))
 496		result |= ADVERTISE_1000XPSE_ASYM;
 497	else if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertise))
 498		result |= ADVERTISE_1000XPAUSE;
 499
 500	return result;
 501}
 502
 503/**
 504 * marvell_config_aneg_fiber - restart auto-negotiation or write BMCR
 505 * @phydev: target phy_device struct
 506 *
 507 * Description: If auto-negotiation is enabled, we configure the
 508 *   advertising, and then restart auto-negotiation.  If it is not
 509 *   enabled, then we write the BMCR. Adapted for fiber link in
 510 *   some Marvell's devices.
 511 */
 512static int marvell_config_aneg_fiber(struct phy_device *phydev)
 513{
 514	int changed = 0;
 515	int err;
 516	u16 adv;
 517
 518	if (phydev->autoneg != AUTONEG_ENABLE)
 519		return genphy_setup_forced(phydev);
 520
 521	/* Only allow advertising what this PHY supports */
 522	linkmode_and(phydev->advertising, phydev->advertising,
 523		     phydev->supported);
 524
 525	adv = linkmode_adv_to_fiber_adv_t(phydev->advertising);
 526
 527	/* Setup fiber advertisement */
 528	err = phy_modify_changed(phydev, MII_ADVERTISE,
 529				 ADVERTISE_1000XHALF | ADVERTISE_1000XFULL |
 530				 ADVERTISE_1000XPAUSE | ADVERTISE_1000XPSE_ASYM,
 531				 adv);
 532	if (err < 0)
 533		return err;
 534	if (err > 0)
 535		changed = 1;
 536
 537	return genphy_check_and_restart_aneg(phydev, changed);
 538}
 539
 540static int m88e1510_config_aneg(struct phy_device *phydev)
 541{
 542	int err;
 543
 544	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 545	if (err < 0)
 546		goto error;
 547
 548	/* Configure the copper link first */
 549	err = m88e1318_config_aneg(phydev);
 550	if (err < 0)
 551		goto error;
 552
 553	/* Do not touch the fiber page if we're in copper->sgmii mode */
 554	if (phydev->interface == PHY_INTERFACE_MODE_SGMII)
 555		return 0;
 556
 557	/* Then the fiber link */
 558	err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
 559	if (err < 0)
 560		goto error;
 561
 562	err = marvell_config_aneg_fiber(phydev);
 563	if (err < 0)
 564		goto error;
 565
 566	return marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 567
 568error:
 569	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 570	return err;
 571}
 572
 573static void marvell_config_led(struct phy_device *phydev)
 574{
 575	u16 def_config;
 576	int err;
 577
 578	switch (MARVELL_PHY_FAMILY_ID(phydev->phy_id)) {
 579	/* Default PHY LED config: LED[0] .. Link, LED[1] .. Activity */
 580	case MARVELL_PHY_FAMILY_ID(MARVELL_PHY_ID_88E1121R):
 581	case MARVELL_PHY_FAMILY_ID(MARVELL_PHY_ID_88E1318S):
 582		def_config = MII_88E1121_PHY_LED_DEF;
 583		break;
 584	/* Default PHY LED config:
 585	 * LED[0] .. 1000Mbps Link
 586	 * LED[1] .. 100Mbps Link
 587	 * LED[2] .. Blink, Activity
 588	 */
 589	case MARVELL_PHY_FAMILY_ID(MARVELL_PHY_ID_88E1510):
 590		if (phydev->dev_flags & MARVELL_PHY_LED0_LINK_LED1_ACTIVE)
 591			def_config = MII_88E1510_PHY_LED0_LINK_LED1_ACTIVE;
 592		else
 593			def_config = MII_88E1510_PHY_LED_DEF;
 594		break;
 595	default:
 596		return;
 597	}
 598
 599	err = phy_write_paged(phydev, MII_MARVELL_LED_PAGE, MII_PHY_LED_CTRL,
 600			      def_config);
 601	if (err < 0)
 602		phydev_warn(phydev, "Fail to config marvell phy LED.\n");
 603}
 604
 605static int marvell_config_init(struct phy_device *phydev)
 606{
 607	/* Set defalut LED */
 608	marvell_config_led(phydev);
 609
 610	/* Set registers from marvell,reg-init DT property */
 611	return marvell_of_reg_init(phydev);
 612}
 613
 614static int m88e3016_config_init(struct phy_device *phydev)
 615{
 616	int ret;
 617
 618	/* Enable Scrambler and Auto-Crossover */
 619	ret = phy_modify(phydev, MII_88E3016_PHY_SPEC_CTRL,
 620			 MII_88E3016_DISABLE_SCRAMBLER,
 621			 MII_88E3016_AUTO_MDIX_CROSSOVER);
 622	if (ret < 0)
 623		return ret;
 624
 625	return marvell_config_init(phydev);
 626}
 627
 628static int m88e1111_config_init_hwcfg_mode(struct phy_device *phydev,
 629					   u16 mode,
 630					   int fibre_copper_auto)
 631{
 632	if (fibre_copper_auto)
 633		mode |= MII_M1111_HWCFG_FIBER_COPPER_AUTO;
 634
 635	return phy_modify(phydev, MII_M1111_PHY_EXT_SR,
 636			  MII_M1111_HWCFG_MODE_MASK |
 637			  MII_M1111_HWCFG_FIBER_COPPER_AUTO |
 638			  MII_M1111_HWCFG_FIBER_COPPER_RES,
 639			  mode);
 640}
 641
 642static int m88e1111_config_init_rgmii_delays(struct phy_device *phydev)
 643{
 644	int delay;
 645
 646	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
 647		delay = MII_M1111_RGMII_RX_DELAY | MII_M1111_RGMII_TX_DELAY;
 648	} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) {
 649		delay = MII_M1111_RGMII_RX_DELAY;
 650	} else if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID) {
 651		delay = MII_M1111_RGMII_TX_DELAY;
 652	} else {
 653		delay = 0;
 654	}
 655
 656	return phy_modify(phydev, MII_M1111_PHY_EXT_CR,
 657			  MII_M1111_RGMII_RX_DELAY | MII_M1111_RGMII_TX_DELAY,
 658			  delay);
 659}
 660
 661static int m88e1111_config_init_rgmii(struct phy_device *phydev)
 662{
 663	int temp;
 664	int err;
 665
 666	err = m88e1111_config_init_rgmii_delays(phydev);
 667	if (err < 0)
 668		return err;
 669
 670	temp = phy_read(phydev, MII_M1111_PHY_EXT_SR);
 671	if (temp < 0)
 672		return temp;
 673
 674	temp &= ~(MII_M1111_HWCFG_MODE_MASK);
 675
 676	if (temp & MII_M1111_HWCFG_FIBER_COPPER_RES)
 677		temp |= MII_M1111_HWCFG_MODE_FIBER_RGMII;
 678	else
 679		temp |= MII_M1111_HWCFG_MODE_COPPER_RGMII;
 680
 681	return phy_write(phydev, MII_M1111_PHY_EXT_SR, temp);
 682}
 683
 684static int m88e1111_config_init_sgmii(struct phy_device *phydev)
 685{
 686	int err;
 687
 688	err = m88e1111_config_init_hwcfg_mode(
 689		phydev,
 690		MII_M1111_HWCFG_MODE_SGMII_NO_CLK,
 691		MII_M1111_HWCFG_FIBER_COPPER_AUTO);
 692	if (err < 0)
 693		return err;
 694
 695	/* make sure copper is selected */
 696	return marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 697}
 698
 699static int m88e1111_config_init_rtbi(struct phy_device *phydev)
 700{
 701	int err;
 702
 703	err = m88e1111_config_init_rgmii_delays(phydev);
 704	if (err < 0)
 705		return err;
 706
 707	err = m88e1111_config_init_hwcfg_mode(
 708		phydev,
 709		MII_M1111_HWCFG_MODE_RTBI,
 710		MII_M1111_HWCFG_FIBER_COPPER_AUTO);
 711	if (err < 0)
 712		return err;
 713
 714	/* soft reset */
 715	err = genphy_soft_reset(phydev);
 716	if (err < 0)
 717		return err;
 718
 719	return m88e1111_config_init_hwcfg_mode(
 720		phydev,
 721		MII_M1111_HWCFG_MODE_RTBI,
 722		MII_M1111_HWCFG_FIBER_COPPER_AUTO);
 723}
 724
 725static int m88e1111_config_init(struct phy_device *phydev)
 726{
 727	int err;
 728
 729	if (phy_interface_is_rgmii(phydev)) {
 730		err = m88e1111_config_init_rgmii(phydev);
 731		if (err < 0)
 732			return err;
 733	}
 734
 735	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
 736		err = m88e1111_config_init_sgmii(phydev);
 737		if (err < 0)
 738			return err;
 739	}
 740
 741	if (phydev->interface == PHY_INTERFACE_MODE_RTBI) {
 742		err = m88e1111_config_init_rtbi(phydev);
 743		if (err < 0)
 744			return err;
 745	}
 746
 747	err = marvell_of_reg_init(phydev);
 748	if (err < 0)
 749		return err;
 750
 751	return genphy_soft_reset(phydev);
 752}
 753
 754static int m88e1111_get_downshift(struct phy_device *phydev, u8 *data)
 755{
 756	int val, cnt, enable;
 757
 758	val = phy_read(phydev, MII_M1111_PHY_EXT_CR);
 759	if (val < 0)
 760		return val;
 761
 762	enable = FIELD_GET(MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN, val);
 763	cnt = FIELD_GET(MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK, val) + 1;
 764
 765	*data = enable ? cnt : DOWNSHIFT_DEV_DISABLE;
 766
 767	return 0;
 768}
 769
 770static int m88e1111_set_downshift(struct phy_device *phydev, u8 cnt)
 771{
 772	int val;
 773
 774	if (cnt > MII_M1111_PHY_EXT_CR_DOWNSHIFT_MAX)
 775		return -E2BIG;
 776
 777	if (!cnt)
 778		return phy_clear_bits(phydev, MII_M1111_PHY_EXT_CR,
 779				      MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN);
 780
 781	val = MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN;
 782	val |= FIELD_PREP(MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK, cnt - 1);
 783
 784	return phy_modify(phydev, MII_M1111_PHY_EXT_CR,
 785			  MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN |
 786			  MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK,
 787			  val);
 788}
 789
 790static int m88e1111_get_tunable(struct phy_device *phydev,
 791				struct ethtool_tunable *tuna, void *data)
 792{
 793	switch (tuna->id) {
 794	case ETHTOOL_PHY_DOWNSHIFT:
 795		return m88e1111_get_downshift(phydev, data);
 796	default:
 797		return -EOPNOTSUPP;
 798	}
 799}
 800
 801static int m88e1111_set_tunable(struct phy_device *phydev,
 802				struct ethtool_tunable *tuna, const void *data)
 803{
 804	switch (tuna->id) {
 805	case ETHTOOL_PHY_DOWNSHIFT:
 806		return m88e1111_set_downshift(phydev, *(const u8 *)data);
 807	default:
 808		return -EOPNOTSUPP;
 809	}
 810}
 811
 812static int m88e1011_get_downshift(struct phy_device *phydev, u8 *data)
 813{
 814	int val, cnt, enable;
 815
 816	val = phy_read(phydev, MII_M1011_PHY_SCR);
 817	if (val < 0)
 818		return val;
 819
 820	enable = FIELD_GET(MII_M1011_PHY_SCR_DOWNSHIFT_EN, val);
 821	cnt = FIELD_GET(MII_M1011_PHY_SCR_DOWNSHIFT_MASK, val) + 1;
 822
 823	*data = enable ? cnt : DOWNSHIFT_DEV_DISABLE;
 824
 825	return 0;
 826}
 827
 828static int m88e1011_set_downshift(struct phy_device *phydev, u8 cnt)
 829{
 830	int val;
 831
 832	if (cnt > MII_M1011_PHY_SCR_DOWNSHIFT_MAX)
 833		return -E2BIG;
 834
 835	if (!cnt)
 836		return phy_clear_bits(phydev, MII_M1011_PHY_SCR,
 837				      MII_M1011_PHY_SCR_DOWNSHIFT_EN);
 838
 839	val = MII_M1011_PHY_SCR_DOWNSHIFT_EN;
 840	val |= FIELD_PREP(MII_M1011_PHY_SCR_DOWNSHIFT_MASK, cnt - 1);
 841
 842	return phy_modify(phydev, MII_M1011_PHY_SCR,
 843			  MII_M1011_PHY_SCR_DOWNSHIFT_EN |
 844			  MII_M1011_PHY_SCR_DOWNSHIFT_MASK,
 845			  val);
 846}
 847
 848static int m88e1011_get_tunable(struct phy_device *phydev,
 849				struct ethtool_tunable *tuna, void *data)
 850{
 851	switch (tuna->id) {
 852	case ETHTOOL_PHY_DOWNSHIFT:
 853		return m88e1011_get_downshift(phydev, data);
 854	default:
 855		return -EOPNOTSUPP;
 856	}
 857}
 858
 859static int m88e1011_set_tunable(struct phy_device *phydev,
 860				struct ethtool_tunable *tuna, const void *data)
 861{
 862	switch (tuna->id) {
 863	case ETHTOOL_PHY_DOWNSHIFT:
 864		return m88e1011_set_downshift(phydev, *(const u8 *)data);
 865	default:
 866		return -EOPNOTSUPP;
 867	}
 868}
 869
 870static int m88e1116r_config_init(struct phy_device *phydev)
 871{
 872	int err;
 873
 874	err = genphy_soft_reset(phydev);
 875	if (err < 0)
 876		return err;
 877
 878	msleep(500);
 879
 880	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 881	if (err < 0)
 882		return err;
 883
 884	err = marvell_set_polarity(phydev, phydev->mdix_ctrl);
 885	if (err < 0)
 886		return err;
 887
 888	err = m88e1011_set_downshift(phydev, 8);
 889	if (err < 0)
 890		return err;
 891
 892	if (phy_interface_is_rgmii(phydev)) {
 893		err = m88e1121_config_aneg_rgmii_delays(phydev);
 894		if (err < 0)
 895			return err;
 896	}
 897
 898	err = genphy_soft_reset(phydev);
 899	if (err < 0)
 900		return err;
 901
 902	return marvell_config_init(phydev);
 903}
 904
 905static int m88e1318_config_init(struct phy_device *phydev)
 906{
 907	if (phy_interrupt_is_valid(phydev)) {
 908		int err = phy_modify_paged(
 909			phydev, MII_MARVELL_LED_PAGE,
 910			MII_88E1318S_PHY_LED_TCR,
 911			MII_88E1318S_PHY_LED_TCR_FORCE_INT,
 912			MII_88E1318S_PHY_LED_TCR_INTn_ENABLE |
 913			MII_88E1318S_PHY_LED_TCR_INT_ACTIVE_LOW);
 914		if (err < 0)
 915			return err;
 916	}
 917
 918	return marvell_config_init(phydev);
 919}
 920
 921static int m88e1510_config_init(struct phy_device *phydev)
 922{
 923	int err;
 924
 925	/* SGMII-to-Copper mode initialization */
 926	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
 927		/* Select page 18 */
 928		err = marvell_set_page(phydev, 18);
 929		if (err < 0)
 930			return err;
 931
 932		/* In reg 20, write MODE[2:0] = 0x1 (SGMII to Copper) */
 933		err = phy_modify(phydev, MII_88E1510_GEN_CTRL_REG_1,
 934				 MII_88E1510_GEN_CTRL_REG_1_MODE_MASK,
 935				 MII_88E1510_GEN_CTRL_REG_1_MODE_SGMII);
 936		if (err < 0)
 937			return err;
 938
 939		/* PHY reset is necessary after changing MODE[2:0] */
 940		err = phy_modify(phydev, MII_88E1510_GEN_CTRL_REG_1, 0,
 941				 MII_88E1510_GEN_CTRL_REG_1_RESET);
 942		if (err < 0)
 943			return err;
 944
 945		/* Reset page selection */
 946		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
 947		if (err < 0)
 948			return err;
 949	}
 950
 951	return m88e1318_config_init(phydev);
 952}
 953
 954static int m88e1118_config_aneg(struct phy_device *phydev)
 955{
 956	int err;
 957
 958	err = genphy_soft_reset(phydev);
 959	if (err < 0)
 960		return err;
 961
 962	err = marvell_set_polarity(phydev, phydev->mdix_ctrl);
 963	if (err < 0)
 964		return err;
 965
 966	err = genphy_config_aneg(phydev);
 967	return 0;
 968}
 969
 970static int m88e1118_config_init(struct phy_device *phydev)
 971{
 972	int err;
 973
 974	/* Change address */
 975	err = marvell_set_page(phydev, MII_MARVELL_MSCR_PAGE);
 976	if (err < 0)
 977		return err;
 978
 979	/* Enable 1000 Mbit */
 980	err = phy_write(phydev, 0x15, 0x1070);
 981	if (err < 0)
 982		return err;
 983
 984	/* Change address */
 985	err = marvell_set_page(phydev, MII_MARVELL_LED_PAGE);
 986	if (err < 0)
 987		return err;
 988
 989	/* Adjust LED Control */
 990	if (phydev->dev_flags & MARVELL_PHY_M1118_DNS323_LEDS)
 991		err = phy_write(phydev, 0x10, 0x1100);
 992	else
 993		err = phy_write(phydev, 0x10, 0x021e);
 994	if (err < 0)
 995		return err;
 996
 997	err = marvell_of_reg_init(phydev);
 998	if (err < 0)
 999		return err;
1000
1001	/* Reset address */
1002	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1003	if (err < 0)
1004		return err;
1005
1006	return genphy_soft_reset(phydev);
1007}
1008
1009static int m88e1149_config_init(struct phy_device *phydev)
1010{
1011	int err;
1012
1013	/* Change address */
1014	err = marvell_set_page(phydev, MII_MARVELL_MSCR_PAGE);
1015	if (err < 0)
1016		return err;
1017
1018	/* Enable 1000 Mbit */
1019	err = phy_write(phydev, 0x15, 0x1048);
1020	if (err < 0)
1021		return err;
1022
1023	err = marvell_of_reg_init(phydev);
1024	if (err < 0)
1025		return err;
1026
1027	/* Reset address */
1028	err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1029	if (err < 0)
1030		return err;
1031
1032	return genphy_soft_reset(phydev);
1033}
1034
1035static int m88e1145_config_init_rgmii(struct phy_device *phydev)
1036{
1037	int err;
1038
1039	err = m88e1111_config_init_rgmii_delays(phydev);
1040	if (err < 0)
1041		return err;
1042
1043	if (phydev->dev_flags & MARVELL_PHY_M1145_FLAGS_RESISTANCE) {
1044		err = phy_write(phydev, 0x1d, 0x0012);
1045		if (err < 0)
1046			return err;
1047
1048		err = phy_modify(phydev, 0x1e, 0x0fc0,
1049				 2 << 9 | /* 36 ohm */
1050				 2 << 6); /* 39 ohm */
1051		if (err < 0)
1052			return err;
1053
1054		err = phy_write(phydev, 0x1d, 0x3);
1055		if (err < 0)
1056			return err;
1057
1058		err = phy_write(phydev, 0x1e, 0x8000);
1059	}
1060	return err;
1061}
1062
1063static int m88e1145_config_init_sgmii(struct phy_device *phydev)
1064{
1065	return m88e1111_config_init_hwcfg_mode(
1066		phydev, MII_M1111_HWCFG_MODE_SGMII_NO_CLK,
1067		MII_M1111_HWCFG_FIBER_COPPER_AUTO);
1068}
1069
1070static int m88e1145_config_init(struct phy_device *phydev)
1071{
1072	int err;
1073
1074	/* Take care of errata E0 & E1 */
1075	err = phy_write(phydev, 0x1d, 0x001b);
1076	if (err < 0)
1077		return err;
1078
1079	err = phy_write(phydev, 0x1e, 0x418f);
1080	if (err < 0)
1081		return err;
1082
1083	err = phy_write(phydev, 0x1d, 0x0016);
1084	if (err < 0)
1085		return err;
1086
1087	err = phy_write(phydev, 0x1e, 0xa2da);
1088	if (err < 0)
1089		return err;
1090
1091	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
1092		err = m88e1145_config_init_rgmii(phydev);
1093		if (err < 0)
1094			return err;
1095	}
1096
1097	if (phydev->interface == PHY_INTERFACE_MODE_SGMII) {
1098		err = m88e1145_config_init_sgmii(phydev);
1099		if (err < 0)
1100			return err;
1101	}
1102
1103	err = marvell_of_reg_init(phydev);
1104	if (err < 0)
1105		return err;
1106
1107	return 0;
1108}
1109
1110static int m88e1540_get_fld(struct phy_device *phydev, u8 *msecs)
1111{
1112	int val;
1113
1114	val = phy_read(phydev, MII_88E1540_COPPER_CTRL3);
1115	if (val < 0)
1116		return val;
1117
1118	if (!(val & MII_88E1540_COPPER_CTRL3_FAST_LINK_DOWN)) {
1119		*msecs = ETHTOOL_PHY_FAST_LINK_DOWN_OFF;
1120		return 0;
1121	}
1122
1123	val = FIELD_GET(MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_MASK, val);
1124
1125	switch (val) {
1126	case MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_00MS:
1127		*msecs = 0;
1128		break;
1129	case MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_10MS:
1130		*msecs = 10;
1131		break;
1132	case MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_20MS:
1133		*msecs = 20;
1134		break;
1135	case MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_40MS:
1136		*msecs = 40;
1137		break;
1138	default:
1139		return -EINVAL;
1140	}
1141
1142	return 0;
1143}
1144
1145static int m88e1540_set_fld(struct phy_device *phydev, const u8 *msecs)
1146{
1147	struct ethtool_eee eee;
1148	int val, ret;
1149
1150	if (*msecs == ETHTOOL_PHY_FAST_LINK_DOWN_OFF)
1151		return phy_clear_bits(phydev, MII_88E1540_COPPER_CTRL3,
1152				      MII_88E1540_COPPER_CTRL3_FAST_LINK_DOWN);
1153
1154	/* According to the Marvell data sheet EEE must be disabled for
1155	 * Fast Link Down detection to work properly
1156	 */
1157	ret = phy_ethtool_get_eee(phydev, &eee);
1158	if (!ret && eee.eee_enabled) {
1159		phydev_warn(phydev, "Fast Link Down detection requires EEE to be disabled!\n");
1160		return -EBUSY;
1161	}
1162
1163	if (*msecs <= 5)
1164		val = MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_00MS;
1165	else if (*msecs <= 15)
1166		val = MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_10MS;
1167	else if (*msecs <= 30)
1168		val = MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_20MS;
1169	else
1170		val = MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_40MS;
1171
1172	val = FIELD_PREP(MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_MASK, val);
1173
1174	ret = phy_modify(phydev, MII_88E1540_COPPER_CTRL3,
1175			 MII_88E1540_COPPER_CTRL3_LINK_DOWN_DELAY_MASK, val);
1176	if (ret)
1177		return ret;
1178
1179	return phy_set_bits(phydev, MII_88E1540_COPPER_CTRL3,
1180			    MII_88E1540_COPPER_CTRL3_FAST_LINK_DOWN);
1181}
1182
1183static int m88e1540_get_tunable(struct phy_device *phydev,
1184				struct ethtool_tunable *tuna, void *data)
1185{
1186	switch (tuna->id) {
1187	case ETHTOOL_PHY_FAST_LINK_DOWN:
1188		return m88e1540_get_fld(phydev, data);
1189	case ETHTOOL_PHY_DOWNSHIFT:
1190		return m88e1011_get_downshift(phydev, data);
1191	default:
1192		return -EOPNOTSUPP;
1193	}
1194}
1195
1196static int m88e1540_set_tunable(struct phy_device *phydev,
1197				struct ethtool_tunable *tuna, const void *data)
1198{
1199	switch (tuna->id) {
1200	case ETHTOOL_PHY_FAST_LINK_DOWN:
1201		return m88e1540_set_fld(phydev, data);
1202	case ETHTOOL_PHY_DOWNSHIFT:
1203		return m88e1011_set_downshift(phydev, *(const u8 *)data);
1204	default:
1205		return -EOPNOTSUPP;
1206	}
1207}
1208
1209/* The VOD can be out of specification on link up. Poke an
1210 * undocumented register, in an undocumented page, with a magic value
1211 * to fix this.
1212 */
1213static int m88e6390_errata(struct phy_device *phydev)
1214{
1215	int err;
1216
1217	err = phy_write(phydev, MII_BMCR,
1218			BMCR_ANENABLE | BMCR_SPEED1000 | BMCR_FULLDPLX);
1219	if (err)
1220		return err;
1221
1222	usleep_range(300, 400);
1223
1224	err = phy_write_paged(phydev, 0xf8, 0x08, 0x36);
1225	if (err)
1226		return err;
1227
1228	return genphy_soft_reset(phydev);
1229}
1230
1231static int m88e6390_config_aneg(struct phy_device *phydev)
1232{
1233	int err;
1234
1235	err = m88e6390_errata(phydev);
1236	if (err)
1237		return err;
1238
1239	return m88e1510_config_aneg(phydev);
1240}
1241
1242/**
1243 * fiber_lpa_mod_linkmode_lpa_t
1244 * @advertising: the linkmode advertisement settings
1245 * @lpa: value of the MII_LPA register for fiber link
1246 *
1247 * A small helper function that translates MII_LPA bits to linkmode LP
1248 * advertisement settings. Other bits in advertising are left
1249 * unchanged.
1250 */
1251static void fiber_lpa_mod_linkmode_lpa_t(unsigned long *advertising, u32 lpa)
1252{
1253	linkmode_mod_bit(ETHTOOL_LINK_MODE_1000baseT_Half_BIT,
1254			 advertising, lpa & LPA_1000XHALF);
1255
1256	linkmode_mod_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
1257			 advertising, lpa & LPA_1000XFULL);
1258}
1259
1260static int marvell_read_status_page_an(struct phy_device *phydev,
1261				       int fiber, int status)
1262{
1263	int lpa;
1264	int err;
1265
1266	if (!(status & MII_M1011_PHY_STATUS_RESOLVED)) {
1267		phydev->link = 0;
1268		return 0;
1269	}
1270
1271	if (status & MII_M1011_PHY_STATUS_FULLDUPLEX)
1272		phydev->duplex = DUPLEX_FULL;
1273	else
1274		phydev->duplex = DUPLEX_HALF;
1275
1276	switch (status & MII_M1011_PHY_STATUS_SPD_MASK) {
1277	case MII_M1011_PHY_STATUS_1000:
1278		phydev->speed = SPEED_1000;
1279		break;
1280
1281	case MII_M1011_PHY_STATUS_100:
1282		phydev->speed = SPEED_100;
1283		break;
1284
1285	default:
1286		phydev->speed = SPEED_10;
1287		break;
1288	}
1289
1290	if (!fiber) {
1291		err = genphy_read_lpa(phydev);
1292		if (err < 0)
1293			return err;
1294
1295		phy_resolve_aneg_pause(phydev);
1296	} else {
1297		lpa = phy_read(phydev, MII_LPA);
1298		if (lpa < 0)
1299			return lpa;
1300
1301		/* The fiber link is only 1000M capable */
1302		fiber_lpa_mod_linkmode_lpa_t(phydev->lp_advertising, lpa);
1303
1304		if (phydev->duplex == DUPLEX_FULL) {
1305			if (!(lpa & LPA_PAUSE_FIBER)) {
1306				phydev->pause = 0;
1307				phydev->asym_pause = 0;
1308			} else if ((lpa & LPA_PAUSE_ASYM_FIBER)) {
1309				phydev->pause = 1;
1310				phydev->asym_pause = 1;
1311			} else {
1312				phydev->pause = 1;
1313				phydev->asym_pause = 0;
1314			}
1315		}
1316	}
1317
1318	return 0;
1319}
1320
1321/* marvell_read_status_page
1322 *
1323 * Description:
1324 *   Check the link, then figure out the current state
1325 *   by comparing what we advertise with what the link partner
1326 *   advertises.  Start by checking the gigabit possibilities,
1327 *   then move on to 10/100.
1328 */
1329static int marvell_read_status_page(struct phy_device *phydev, int page)
1330{
1331	int status;
1332	int fiber;
1333	int err;
1334
1335	status = phy_read(phydev, MII_M1011_PHY_STATUS);
1336	if (status < 0)
1337		return status;
1338
1339	/* Use the generic register for copper link status,
1340	 * and the PHY status register for fiber link status.
1341	 */
1342	if (page == MII_MARVELL_FIBER_PAGE) {
1343		phydev->link = !!(status & MII_M1011_PHY_STATUS_LINK);
1344	} else {
1345		err = genphy_update_link(phydev);
1346		if (err)
1347			return err;
1348	}
1349
1350	if (page == MII_MARVELL_FIBER_PAGE)
1351		fiber = 1;
1352	else
1353		fiber = 0;
1354
1355	linkmode_zero(phydev->lp_advertising);
1356	phydev->pause = 0;
1357	phydev->asym_pause = 0;
1358	phydev->speed = SPEED_UNKNOWN;
1359	phydev->duplex = DUPLEX_UNKNOWN;
1360
1361	if (phydev->autoneg == AUTONEG_ENABLE)
1362		err = marvell_read_status_page_an(phydev, fiber, status);
1363	else
1364		err = genphy_read_status_fixed(phydev);
1365
1366	return err;
1367}
1368
1369/* marvell_read_status
1370 *
1371 * Some Marvell's phys have two modes: fiber and copper.
1372 * Both need status checked.
1373 * Description:
1374 *   First, check the fiber link and status.
1375 *   If the fiber link is down, check the copper link and status which
1376 *   will be the default value if both link are down.
1377 */
1378static int marvell_read_status(struct phy_device *phydev)
1379{
1380	int err;
1381
1382	/* Check the fiber mode first */
1383	if (linkmode_test_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
1384			      phydev->supported) &&
1385	    phydev->interface != PHY_INTERFACE_MODE_SGMII) {
1386		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
1387		if (err < 0)
1388			goto error;
1389
1390		err = marvell_read_status_page(phydev, MII_MARVELL_FIBER_PAGE);
1391		if (err < 0)
1392			goto error;
1393
1394		/* If the fiber link is up, it is the selected and
1395		 * used link. In this case, we need to stay in the
1396		 * fiber page. Please to be careful about that, avoid
1397		 * to restore Copper page in other functions which
1398		 * could break the behaviour for some fiber phy like
1399		 * 88E1512.
1400		 */
1401		if (phydev->link)
1402			return 0;
1403
1404		/* If fiber link is down, check and save copper mode state */
1405		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1406		if (err < 0)
1407			goto error;
1408	}
1409
1410	return marvell_read_status_page(phydev, MII_MARVELL_COPPER_PAGE);
1411
1412error:
1413	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1414	return err;
1415}
1416
1417/* marvell_suspend
1418 *
1419 * Some Marvell's phys have two modes: fiber and copper.
1420 * Both need to be suspended
1421 */
1422static int marvell_suspend(struct phy_device *phydev)
1423{
1424	int err;
1425
1426	/* Suspend the fiber mode first */
1427	if (!linkmode_test_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
1428			       phydev->supported)) {
1429		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
1430		if (err < 0)
1431			goto error;
1432
1433		/* With the page set, use the generic suspend */
1434		err = genphy_suspend(phydev);
1435		if (err < 0)
1436			goto error;
1437
1438		/* Then, the copper link */
1439		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1440		if (err < 0)
1441			goto error;
1442	}
1443
1444	/* With the page set, use the generic suspend */
1445	return genphy_suspend(phydev);
1446
1447error:
1448	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1449	return err;
1450}
1451
1452/* marvell_resume
1453 *
1454 * Some Marvell's phys have two modes: fiber and copper.
1455 * Both need to be resumed
1456 */
1457static int marvell_resume(struct phy_device *phydev)
1458{
1459	int err;
1460
1461	/* Resume the fiber mode first */
1462	if (!linkmode_test_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
1463			       phydev->supported)) {
1464		err = marvell_set_page(phydev, MII_MARVELL_FIBER_PAGE);
1465		if (err < 0)
1466			goto error;
1467
1468		/* With the page set, use the generic resume */
1469		err = genphy_resume(phydev);
1470		if (err < 0)
1471			goto error;
1472
1473		/* Then, the copper link */
1474		err = marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1475		if (err < 0)
1476			goto error;
1477	}
1478
1479	/* With the page set, use the generic resume */
1480	return genphy_resume(phydev);
1481
1482error:
1483	marvell_set_page(phydev, MII_MARVELL_COPPER_PAGE);
1484	return err;
1485}
1486
1487static int marvell_aneg_done(struct phy_device *phydev)
1488{
1489	int retval = phy_read(phydev, MII_M1011_PHY_STATUS);
1490
1491	return (retval < 0) ? retval : (retval & MII_M1011_PHY_STATUS_RESOLVED);
1492}
1493
1494static int m88e1121_did_interrupt(struct phy_device *phydev)
1495{
1496	int imask;
1497
1498	imask = phy_read(phydev, MII_M1011_IEVENT);
1499
1500	if (imask & MII_M1011_IMASK_INIT)
1501		return 1;
1502
1503	return 0;
1504}
1505
1506static void m88e1318_get_wol(struct phy_device *phydev,
1507			     struct ethtool_wolinfo *wol)
1508{
1509	int oldpage, ret = 0;
1510
1511	wol->supported = WAKE_MAGIC;
1512	wol->wolopts = 0;
1513
1514	oldpage = phy_select_page(phydev, MII_MARVELL_WOL_PAGE);
1515	if (oldpage < 0)
1516		goto error;
1517
1518	ret = __phy_read(phydev, MII_88E1318S_PHY_WOL_CTRL);
1519	if (ret & MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE)
1520		wol->wolopts |= WAKE_MAGIC;
1521
1522error:
1523	phy_restore_page(phydev, oldpage, ret);
1524}
1525
1526static int m88e1318_set_wol(struct phy_device *phydev,
1527			    struct ethtool_wolinfo *wol)
1528{
1529	int err = 0, oldpage;
1530
1531	oldpage = phy_save_page(phydev);
1532	if (oldpage < 0)
1533		goto error;
1534
1535	if (wol->wolopts & WAKE_MAGIC) {
1536		/* Explicitly switch to page 0x00, just to be sure */
1537		err = marvell_write_page(phydev, MII_MARVELL_COPPER_PAGE);
1538		if (err < 0)
1539			goto error;
1540
1541		/* If WOL event happened once, the LED[2] interrupt pin
1542		 * will not be cleared unless we reading the interrupt status
1543		 * register. If interrupts are in use, the normal interrupt
1544		 * handling will clear the WOL event. Clear the WOL event
1545		 * before enabling it if !phy_interrupt_is_valid()
1546		 */
1547		if (!phy_interrupt_is_valid(phydev))
1548			__phy_read(phydev, MII_M1011_IEVENT);
1549
1550		/* Enable the WOL interrupt */
1551		err = __phy_modify(phydev, MII_88E1318S_PHY_CSIER, 0,
1552				   MII_88E1318S_PHY_CSIER_WOL_EIE);
1553		if (err < 0)
1554			goto error;
1555
1556		err = marvell_write_page(phydev, MII_MARVELL_LED_PAGE);
1557		if (err < 0)
1558			goto error;
1559
1560		/* Setup LED[2] as interrupt pin (active low) */
1561		err = __phy_modify(phydev, MII_88E1318S_PHY_LED_TCR,
1562				   MII_88E1318S_PHY_LED_TCR_FORCE_INT,
1563				   MII_88E1318S_PHY_LED_TCR_INTn_ENABLE |
1564				   MII_88E1318S_PHY_LED_TCR_INT_ACTIVE_LOW);
1565		if (err < 0)
1566			goto error;
1567
1568		err = marvell_write_page(phydev, MII_MARVELL_WOL_PAGE);
1569		if (err < 0)
1570			goto error;
1571
1572		/* Store the device address for the magic packet */
1573		err = __phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD2,
1574				((phydev->attached_dev->dev_addr[5] << 8) |
1575				 phydev->attached_dev->dev_addr[4]));
1576		if (err < 0)
1577			goto error;
1578		err = __phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD1,
1579				((phydev->attached_dev->dev_addr[3] << 8) |
1580				 phydev->attached_dev->dev_addr[2]));
1581		if (err < 0)
1582			goto error;
1583		err = __phy_write(phydev, MII_88E1318S_PHY_MAGIC_PACKET_WORD0,
1584				((phydev->attached_dev->dev_addr[1] << 8) |
1585				 phydev->attached_dev->dev_addr[0]));
1586		if (err < 0)
1587			goto error;
1588
1589		/* Clear WOL status and enable magic packet matching */
1590		err = __phy_modify(phydev, MII_88E1318S_PHY_WOL_CTRL, 0,
1591				   MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS |
1592				   MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE);
1593		if (err < 0)
1594			goto error;
1595	} else {
1596		err = marvell_write_page(phydev, MII_MARVELL_WOL_PAGE);
1597		if (err < 0)
1598			goto error;
1599
1600		/* Clear WOL status and disable magic packet matching */
1601		err = __phy_modify(phydev, MII_88E1318S_PHY_WOL_CTRL,
1602				   MII_88E1318S_PHY_WOL_CTRL_MAGIC_PACKET_MATCH_ENABLE,
1603				   MII_88E1318S_PHY_WOL_CTRL_CLEAR_WOL_STATUS);
1604		if (err < 0)
1605			goto error;
1606	}
1607
1608error:
1609	return phy_restore_page(phydev, oldpage, err);
1610}
1611
1612static int marvell_get_sset_count(struct phy_device *phydev)
1613{
1614	if (linkmode_test_bit(ETHTOOL_LINK_MODE_FIBRE_BIT,
1615			      phydev->supported))
1616		return ARRAY_SIZE(marvell_hw_stats);
1617	else
1618		return ARRAY_SIZE(marvell_hw_stats) - NB_FIBER_STATS;
1619}
1620
1621static void marvell_get_strings(struct phy_device *phydev, u8 *data)
1622{
1623	int count = marvell_get_sset_count(phydev);
1624	int i;
1625
1626	for (i = 0; i < count; i++) {
1627		strlcpy(data + i * ETH_GSTRING_LEN,
1628			marvell_hw_stats[i].string, ETH_GSTRING_LEN);
1629	}
1630}
1631
1632static u64 marvell_get_stat(struct phy_device *phydev, int i)
1633{
1634	struct marvell_hw_stat stat = marvell_hw_stats[i];
1635	struct marvell_priv *priv = phydev->priv;
1636	int val;
1637	u64 ret;
1638
1639	val = phy_read_paged(phydev, stat.page, stat.reg);
1640	if (val < 0) {
1641		ret = U64_MAX;
1642	} else {
1643		val = val & ((1 << stat.bits) - 1);
1644		priv->stats[i] += val;
1645		ret = priv->stats[i];
1646	}
1647
1648	return ret;
1649}
1650
1651static void marvell_get_stats(struct phy_device *phydev,
1652			      struct ethtool_stats *stats, u64 *data)
1653{
1654	int count = marvell_get_sset_count(phydev);
1655	int i;
1656
1657	for (i = 0; i < count; i++)
1658		data[i] = marvell_get_stat(phydev, i);
1659}
1660
1661#ifdef CONFIG_HWMON
1662static int m88e1121_get_temp(struct phy_device *phydev, long *temp)
1663{
1664	int oldpage;
1665	int ret = 0;
1666	int val;
1667
1668	*temp = 0;
1669
1670	oldpage = phy_select_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
1671	if (oldpage < 0)
1672		goto error;
1673
1674	/* Enable temperature sensor */
1675	ret = __phy_read(phydev, MII_88E1121_MISC_TEST);
1676	if (ret < 0)
1677		goto error;
1678
1679	ret = __phy_write(phydev, MII_88E1121_MISC_TEST,
1680			  ret | MII_88E1121_MISC_TEST_TEMP_SENSOR_EN);
1681	if (ret < 0)
1682		goto error;
1683
1684	/* Wait for temperature to stabilize */
1685	usleep_range(10000, 12000);
1686
1687	val = __phy_read(phydev, MII_88E1121_MISC_TEST);
1688	if (val < 0) {
1689		ret = val;
1690		goto error;
1691	}
1692
1693	/* Disable temperature sensor */
1694	ret = __phy_write(phydev, MII_88E1121_MISC_TEST,
1695			  ret & ~MII_88E1121_MISC_TEST_TEMP_SENSOR_EN);
1696	if (ret < 0)
1697		goto error;
1698
1699	*temp = ((val & MII_88E1121_MISC_TEST_TEMP_MASK) - 5) * 5000;
1700
1701error:
1702	return phy_restore_page(phydev, oldpage, ret);
1703}
1704
1705static int m88e1121_hwmon_read(struct device *dev,
1706			       enum hwmon_sensor_types type,
1707			       u32 attr, int channel, long *temp)
1708{
1709	struct phy_device *phydev = dev_get_drvdata(dev);
1710	int err;
1711
1712	switch (attr) {
1713	case hwmon_temp_input:
1714		err = m88e1121_get_temp(phydev, temp);
1715		break;
1716	default:
1717		return -EOPNOTSUPP;
1718	}
1719
1720	return err;
1721}
1722
1723static umode_t m88e1121_hwmon_is_visible(const void *data,
1724					 enum hwmon_sensor_types type,
1725					 u32 attr, int channel)
1726{
1727	if (type != hwmon_temp)
1728		return 0;
1729
1730	switch (attr) {
1731	case hwmon_temp_input:
1732		return 0444;
1733	default:
1734		return 0;
1735	}
1736}
1737
1738static u32 m88e1121_hwmon_chip_config[] = {
1739	HWMON_C_REGISTER_TZ,
1740	0
1741};
1742
1743static const struct hwmon_channel_info m88e1121_hwmon_chip = {
1744	.type = hwmon_chip,
1745	.config = m88e1121_hwmon_chip_config,
1746};
1747
1748static u32 m88e1121_hwmon_temp_config[] = {
1749	HWMON_T_INPUT,
1750	0
1751};
1752
1753static const struct hwmon_channel_info m88e1121_hwmon_temp = {
1754	.type = hwmon_temp,
1755	.config = m88e1121_hwmon_temp_config,
1756};
1757
1758static const struct hwmon_channel_info *m88e1121_hwmon_info[] = {
1759	&m88e1121_hwmon_chip,
1760	&m88e1121_hwmon_temp,
1761	NULL
1762};
1763
1764static const struct hwmon_ops m88e1121_hwmon_hwmon_ops = {
1765	.is_visible = m88e1121_hwmon_is_visible,
1766	.read = m88e1121_hwmon_read,
1767};
1768
1769static const struct hwmon_chip_info m88e1121_hwmon_chip_info = {
1770	.ops = &m88e1121_hwmon_hwmon_ops,
1771	.info = m88e1121_hwmon_info,
1772};
1773
1774static int m88e1510_get_temp(struct phy_device *phydev, long *temp)
1775{
1776	int ret;
1777
1778	*temp = 0;
1779
1780	ret = phy_read_paged(phydev, MII_MARVELL_MISC_TEST_PAGE,
1781			     MII_88E1510_TEMP_SENSOR);
1782	if (ret < 0)
1783		return ret;
1784
1785	*temp = ((ret & MII_88E1510_TEMP_SENSOR_MASK) - 25) * 1000;
1786
1787	return 0;
1788}
1789
1790static int m88e1510_get_temp_critical(struct phy_device *phydev, long *temp)
1791{
1792	int ret;
1793
1794	*temp = 0;
1795
1796	ret = phy_read_paged(phydev, MII_MARVELL_MISC_TEST_PAGE,
1797			     MII_88E1121_MISC_TEST);
1798	if (ret < 0)
1799		return ret;
1800
1801	*temp = (((ret & MII_88E1510_MISC_TEST_TEMP_THRESHOLD_MASK) >>
1802		  MII_88E1510_MISC_TEST_TEMP_THRESHOLD_SHIFT) * 5) - 25;
1803	/* convert to mC */
1804	*temp *= 1000;
1805
1806	return 0;
1807}
1808
1809static int m88e1510_set_temp_critical(struct phy_device *phydev, long temp)
1810{
1811	temp = temp / 1000;
1812	temp = clamp_val(DIV_ROUND_CLOSEST(temp, 5) + 5, 0, 0x1f);
1813
1814	return phy_modify_paged(phydev, MII_MARVELL_MISC_TEST_PAGE,
1815				MII_88E1121_MISC_TEST,
1816				MII_88E1510_MISC_TEST_TEMP_THRESHOLD_MASK,
1817				temp << MII_88E1510_MISC_TEST_TEMP_THRESHOLD_SHIFT);
1818}
1819
1820static int m88e1510_get_temp_alarm(struct phy_device *phydev, long *alarm)
1821{
1822	int ret;
1823
1824	*alarm = false;
1825
1826	ret = phy_read_paged(phydev, MII_MARVELL_MISC_TEST_PAGE,
1827			     MII_88E1121_MISC_TEST);
1828	if (ret < 0)
1829		return ret;
1830
1831	*alarm = !!(ret & MII_88E1510_MISC_TEST_TEMP_IRQ);
1832
1833	return 0;
1834}
1835
1836static int m88e1510_hwmon_read(struct device *dev,
1837			       enum hwmon_sensor_types type,
1838			       u32 attr, int channel, long *temp)
1839{
1840	struct phy_device *phydev = dev_get_drvdata(dev);
1841	int err;
1842
1843	switch (attr) {
1844	case hwmon_temp_input:
1845		err = m88e1510_get_temp(phydev, temp);
1846		break;
1847	case hwmon_temp_crit:
1848		err = m88e1510_get_temp_critical(phydev, temp);
1849		break;
1850	case hwmon_temp_max_alarm:
1851		err = m88e1510_get_temp_alarm(phydev, temp);
1852		break;
1853	default:
1854		return -EOPNOTSUPP;
1855	}
1856
1857	return err;
1858}
1859
1860static int m88e1510_hwmon_write(struct device *dev,
1861				enum hwmon_sensor_types type,
1862				u32 attr, int channel, long temp)
1863{
1864	struct phy_device *phydev = dev_get_drvdata(dev);
1865	int err;
1866
1867	switch (attr) {
1868	case hwmon_temp_crit:
1869		err = m88e1510_set_temp_critical(phydev, temp);
1870		break;
1871	default:
1872		return -EOPNOTSUPP;
1873	}
1874	return err;
1875}
1876
1877static umode_t m88e1510_hwmon_is_visible(const void *data,
1878					 enum hwmon_sensor_types type,
1879					 u32 attr, int channel)
1880{
1881	if (type != hwmon_temp)
1882		return 0;
1883
1884	switch (attr) {
1885	case hwmon_temp_input:
1886	case hwmon_temp_max_alarm:
1887		return 0444;
1888	case hwmon_temp_crit:
1889		return 0644;
1890	default:
1891		return 0;
1892	}
1893}
1894
1895static u32 m88e1510_hwmon_temp_config[] = {
1896	HWMON_T_INPUT | HWMON_T_CRIT | HWMON_T_MAX_ALARM,
1897	0
1898};
1899
1900static const struct hwmon_channel_info m88e1510_hwmon_temp = {
1901	.type = hwmon_temp,
1902	.config = m88e1510_hwmon_temp_config,
1903};
1904
1905static const struct hwmon_channel_info *m88e1510_hwmon_info[] = {
1906	&m88e1121_hwmon_chip,
1907	&m88e1510_hwmon_temp,
1908	NULL
1909};
1910
1911static const struct hwmon_ops m88e1510_hwmon_hwmon_ops = {
1912	.is_visible = m88e1510_hwmon_is_visible,
1913	.read = m88e1510_hwmon_read,
1914	.write = m88e1510_hwmon_write,
1915};
1916
1917static const struct hwmon_chip_info m88e1510_hwmon_chip_info = {
1918	.ops = &m88e1510_hwmon_hwmon_ops,
1919	.info = m88e1510_hwmon_info,
1920};
1921
1922static int m88e6390_get_temp(struct phy_device *phydev, long *temp)
1923{
1924	int sum = 0;
1925	int oldpage;
1926	int ret = 0;
1927	int i;
1928
1929	*temp = 0;
1930
1931	oldpage = phy_select_page(phydev, MII_MARVELL_MISC_TEST_PAGE);
1932	if (oldpage < 0)
1933		goto error;
1934
1935	/* Enable temperature sensor */
1936	ret = __phy_read(phydev, MII_88E6390_MISC_TEST);
1937	if (ret < 0)
1938		goto error;
1939
1940	ret = ret & ~MII_88E6390_MISC_TEST_SAMPLE_MASK;
1941	ret |= MII_88E6390_MISC_TEST_SAMPLE_ENABLE |
1942		MII_88E6390_MISC_TEST_SAMPLE_1S;
1943
1944	ret = __phy_write(phydev, MII_88E6390_MISC_TEST, ret);
1945	if (ret < 0)
1946		goto error;
1947
1948	/* Wait for temperature to stabilize */
1949	usleep_range(10000, 12000);
1950
1951	/* Reading the temperature sense has an errata. You need to read
1952	 * a number of times and take an average.
1953	 */
1954	for (i = 0; i < MII_88E6390_TEMP_SENSOR_SAMPLES; i++) {
1955		ret = __phy_read(phydev, MII_88E6390_TEMP_SENSOR);
1956		if (ret < 0)
1957			goto error;
1958		sum += ret & MII_88E6390_TEMP_SENSOR_MASK;
1959	}
1960
1961	sum /= MII_88E6390_TEMP_SENSOR_SAMPLES;
1962	*temp = (sum  - 75) * 1000;
1963
1964	/* Disable temperature sensor */
1965	ret = __phy_read(phydev, MII_88E6390_MISC_TEST);
1966	if (ret < 0)
1967		goto error;
1968
1969	ret = ret & ~MII_88E6390_MISC_TEST_SAMPLE_MASK;
1970	ret |= MII_88E6390_MISC_TEST_SAMPLE_DISABLE;
1971
1972	ret = __phy_write(phydev, MII_88E6390_MISC_TEST, ret);
1973
1974error:
1975	phy_restore_page(phydev, oldpage, ret);
1976
1977	return ret;
1978}
1979
1980static int m88e6390_hwmon_read(struct device *dev,
1981			       enum hwmon_sensor_types type,
1982			       u32 attr, int channel, long *temp)
1983{
1984	struct phy_device *phydev = dev_get_drvdata(dev);
1985	int err;
1986
1987	switch (attr) {
1988	case hwmon_temp_input:
1989		err = m88e6390_get_temp(phydev, temp);
1990		break;
1991	default:
1992		return -EOPNOTSUPP;
1993	}
1994
1995	return err;
1996}
1997
1998static umode_t m88e6390_hwmon_is_visible(const void *data,
1999					 enum hwmon_sensor_types type,
2000					 u32 attr, int channel)
2001{
2002	if (type != hwmon_temp)
2003		return 0;
2004
2005	switch (attr) {
2006	case hwmon_temp_input:
2007		return 0444;
2008	default:
2009		return 0;
2010	}
2011}
2012
2013static u32 m88e6390_hwmon_temp_config[] = {
2014	HWMON_T_INPUT,
2015	0
2016};
2017
2018static const struct hwmon_channel_info m88e6390_hwmon_temp = {
2019	.type = hwmon_temp,
2020	.config = m88e6390_hwmon_temp_config,
2021};
2022
2023static const struct hwmon_channel_info *m88e6390_hwmon_info[] = {
2024	&m88e1121_hwmon_chip,
2025	&m88e6390_hwmon_temp,
2026	NULL
2027};
2028
2029static const struct hwmon_ops m88e6390_hwmon_hwmon_ops = {
2030	.is_visible = m88e6390_hwmon_is_visible,
2031	.read = m88e6390_hwmon_read,
2032};
2033
2034static const struct hwmon_chip_info m88e6390_hwmon_chip_info = {
2035	.ops = &m88e6390_hwmon_hwmon_ops,
2036	.info = m88e6390_hwmon_info,
2037};
2038
2039static int marvell_hwmon_name(struct phy_device *phydev)
2040{
2041	struct marvell_priv *priv = phydev->priv;
2042	struct device *dev = &phydev->mdio.dev;
2043	const char *devname = dev_name(dev);
2044	size_t len = strlen(devname);
2045	int i, j;
2046
2047	priv->hwmon_name = devm_kzalloc(dev, len, GFP_KERNEL);
2048	if (!priv->hwmon_name)
2049		return -ENOMEM;
2050
2051	for (i = j = 0; i < len && devname[i]; i++) {
2052		if (isalnum(devname[i]))
2053			priv->hwmon_name[j++] = devname[i];
2054	}
2055
2056	return 0;
2057}
2058
2059static int marvell_hwmon_probe(struct phy_device *phydev,
2060			       const struct hwmon_chip_info *chip)
2061{
2062	struct marvell_priv *priv = phydev->priv;
2063	struct device *dev = &phydev->mdio.dev;
2064	int err;
2065
2066	err = marvell_hwmon_name(phydev);
2067	if (err)
2068		return err;
2069
2070	priv->hwmon_dev = devm_hwmon_device_register_with_info(
2071		dev, priv->hwmon_name, phydev, chip, NULL);
2072
2073	return PTR_ERR_OR_ZERO(priv->hwmon_dev);
2074}
2075
2076static int m88e1121_hwmon_probe(struct phy_device *phydev)
2077{
2078	return marvell_hwmon_probe(phydev, &m88e1121_hwmon_chip_info);
2079}
2080
2081static int m88e1510_hwmon_probe(struct phy_device *phydev)
2082{
2083	return marvell_hwmon_probe(phydev, &m88e1510_hwmon_chip_info);
2084}
2085
2086static int m88e6390_hwmon_probe(struct phy_device *phydev)
2087{
2088	return marvell_hwmon_probe(phydev, &m88e6390_hwmon_chip_info);
2089}
2090#else
2091static int m88e1121_hwmon_probe(struct phy_device *phydev)
2092{
2093	return 0;
2094}
2095
2096static int m88e1510_hwmon_probe(struct phy_device *phydev)
2097{
2098	return 0;
2099}
2100
2101static int m88e6390_hwmon_probe(struct phy_device *phydev)
2102{
2103	return 0;
2104}
2105#endif
2106
2107static int marvell_probe(struct phy_device *phydev)
2108{
2109	struct marvell_priv *priv;
2110
2111	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
2112	if (!priv)
2113		return -ENOMEM;
2114
2115	phydev->priv = priv;
2116
2117	return 0;
2118}
2119
2120static int m88e1121_probe(struct phy_device *phydev)
2121{
2122	int err;
2123
2124	err = marvell_probe(phydev);
2125	if (err)
2126		return err;
2127
2128	return m88e1121_hwmon_probe(phydev);
2129}
2130
2131static int m88e1510_probe(struct phy_device *phydev)
2132{
2133	int err;
2134
2135	err =…