diff mbox series

[02/15] phy: mvebu-cp110-comphy: Add SMC call support

Message ID 20190401165131.23370-3-miquel.raynal@bootlin.com
State New, archived
Headers show
Series Enhance CP110 COMPHY support | expand

Commit Message

Miquel Raynal April 1, 2019, 4:51 p.m. UTC
Keep the exact same list of supported configurations but first try to
use the firmware's implementation. If it fails, try the legacy method:
Linux implementation.

Signed-off-by: Miquel Raynal <miquel.raynal@bootlin.com>
---
 drivers/phy/marvell/phy-mvebu-cp110-comphy.c | 200 +++++++++++++++----
 1 file changed, 164 insertions(+), 36 deletions(-)

Comments

Grzegorz Jaszczyk April 3, 2019, 9:02 a.m. UTC | #1
Hi Miquel,

pon., 1 kwi 2019 o 18:52 Miquel Raynal <miquel.raynal@bootlin.com> napisał(a):
> +static int mvebu_comphy_power_on(struct phy *phy)
> +{
> +       struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
> +       struct mvebu_comphy_priv *priv = lane->priv;
> +       u32 fw_param = 0;
> +       int fw_mode;
> +       int ret;
> +
> +       fw_mode = mvebu_comphy_get_fw_mode(lane->id, lane->port,
> +                                          lane->mode, lane->submode);
> +       if (fw_mode < 0)
> +               goto try_legacy;
> +
> +       /* Try SMC flow first */
> +       switch (lane->mode) {
> +       case PHY_MODE_ETHERNET:
> +               dev_dbg(priv->dev, "set lane %d to Ethernet mode\n", lane->id);
> +               switch (lane->submode) {
> +               case PHY_INTERFACE_MODE_SGMII:
> +               case PHY_INTERFACE_MODE_2500BASEX:
> +               case PHY_INTERFACE_MODE_10GKR:
> +                       fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);

It seems it will not work for SGMII and 2500BASEX. It is because you
are setting max speed for all comphy modes inside COMPHY_FW_PARAM
macro which is wrong. Did you test it?
BTW can you point to some public temporary git branch where this
patchset can be found?

regards,
Grzegorz
Miquel Raynal April 3, 2019, 9:24 a.m. UTC | #2
Hi Grzegorz,

Grzegorz Jaszczyk <jaz@semihalf.com> wrote on Wed, 3 Apr 2019 11:02:15
+0200:

> Hi Miquel,
> 
> pon., 1 kwi 2019 o 18:52 Miquel Raynal <miquel.raynal@bootlin.com> napisał(a):
> > +static int mvebu_comphy_power_on(struct phy *phy)
> > +{
> > +       struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
> > +       struct mvebu_comphy_priv *priv = lane->priv;
> > +       u32 fw_param = 0;
> > +       int fw_mode;
> > +       int ret;
> > +
> > +       fw_mode = mvebu_comphy_get_fw_mode(lane->id, lane->port,
> > +                                          lane->mode, lane->submode);
> > +       if (fw_mode < 0)
> > +               goto try_legacy;
> > +
> > +       /* Try SMC flow first */
> > +       switch (lane->mode) {
> > +       case PHY_MODE_ETHERNET:
> > +               dev_dbg(priv->dev, "set lane %d to Ethernet mode\n", lane->id);
> > +               switch (lane->submode) {
> > +               case PHY_INTERFACE_MODE_SGMII:
> > +               case PHY_INTERFACE_MODE_2500BASEX:
> > +               case PHY_INTERFACE_MODE_10GKR:
> > +                       fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);
> 
> It seems it will not work for SGMII and 2500BASEX. It is because you
> are setting max speed for all comphy modes inside COMPHY_FW_PARAM
> macro which is wrong. Did you test it?

I misread ATF implementation, I was sure the speed was derived from the
type and this parameter was useless. I'll do the modification.

> BTW can you point to some public temporary git branch where this
> patchset can be found?

Just pushed there if you want to take a look:
https://github.com/miquelraynal/linux.git marvell/5.1-rc1/cp110-comphy


Thanks for the review!
Miquèl
Russell King - ARM Linux admin April 3, 2019, 9:48 a.m. UTC | #3
On Mon, Apr 01, 2019 at 06:51:18PM +0200, Miquel Raynal wrote:
> Keep the exact same list of supported configurations but first try to
> use the firmware's implementation. If it fails, try the legacy method:
> Linux implementation.
> 
> Signed-off-by: Miquel Raynal <miquel.raynal@bootlin.com>
> ---
>  drivers/phy/marvell/phy-mvebu-cp110-comphy.c | 200 +++++++++++++++----
>  1 file changed, 164 insertions(+), 36 deletions(-)
> 
> diff --git a/drivers/phy/marvell/phy-mvebu-cp110-comphy.c b/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
> index 3f50a1521f01..4aaf161503d6 100644
> --- a/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
> +++ b/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
> @@ -5,6 +5,7 @@
>   * Antoine Tenart <antoine.tenart@free-electrons.com>
>   */
>  
> +#include <linux/arm-smccc.h>
>  #include <linux/io.h>
>  #include <linux/iopoll.h>
>  #include <linux/mfd/syscon.h>
> @@ -115,51 +116,88 @@
>  #define MVEBU_COMPHY_LANES	6
>  #define MVEBU_COMPHY_PORTS	3
>  
> +#define COMPHY_SIP_POWER_ON	0x82000001
> +#define COMPHY_SIP_POWER_OFF	0x82000002
> +
> +/*
> + * A lane is described by the following bitfields:
> + * [ 1- 0]: COMPHY polarity invertion
> + * [ 2- 7]: COMPHY speed
> + * [ 5-11]: COMPHY port index
> + * [12-16]: COMPHY mode
> + * [17]: Clock source
> + */
> +#define COMPHY_FW_POL_OFFSET	0
> +#define COMPHY_FW_POL_MASK	GENMASK(1, 0)
> +#define COMPHY_FW_SPEED_OFFSET	2
> +#define COMPHY_FW_SPEED_MASK	GENMASK(7, 2)
> +#define COMPHY_FW_SPEED_MAX	COMPHY_FW_SPEED_MASK
> +#define COMPHY_FW_PORT_OFFSET	8
> +#define COMPHY_FW_PORT_MASK	GENMASK(11, 8)
> +#define COMPHY_FW_MODE_OFFSET	12
> +#define COMPHY_FW_MODE_MASK	GENMASK(16, 12)
> +
> +#define COMPHY_FW_PARAM_FULL(mode, port, speed, pol)			\
> +	((((pol) << COMPHY_FW_POL_OFFSET) & COMPHY_FW_POL_MASK) |	\
> +	 (((mode) << COMPHY_FW_MODE_OFFSET) & COMPHY_FW_MODE_MASK) |	\
> +	 (((port) << COMPHY_FW_PORT_OFFSET) & COMPHY_FW_PORT_MASK) |	\
> +	 (((speed) << COMPHY_FW_SPEED_OFFSET) & COMPHY_FW_SPEED_MASK))
> +
> +#define COMPHY_FW_PARAM(mode, port)					\
> +	COMPHY_FW_PARAM_FULL(mode, port, COMPHY_FW_SPEED_MAX, 0)
> +
> +#define COMPHY_FW_MODE_SGMII		0x2 /* SGMII 1G */
> +#define COMPHY_FW_MODE_HS_SGMII		0x3 /* SGMII 2.5G */
> +#define COMPHY_FW_MODE_SFI		0x9 /* XFI: 0x8 (is treated like SFI) */
> +
>  struct mvebu_comphy_conf {
>  	enum phy_mode mode;
>  	int submode;
>  	unsigned lane;
>  	unsigned port;
>  	u32 mux;
> +	u32 fw_mode;
>  };
>  
> -#define MVEBU_COMPHY_CONF(_lane, _port, _submode, _mux)	\
> +#define MVEBU_COMPHY_CONF(_lane, _port, _submode, _mux, _fw)	\
>  	{						\
>  		.lane = _lane,				\
>  		.port = _port,				\
>  		.mode = PHY_MODE_ETHERNET,		\
>  		.submode = _submode,			\
>  		.mux = _mux,				\
> +		.fw_mode = _fw,				\
>  	}
>  
>  static const struct mvebu_comphy_conf mvebu_comphy_cp110_modes[] = {
>  	/* lane 0 */
> -	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_SGMII, 0x1),
> -	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_2500BASEX, 0x1),
> +	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
>  	/* lane 1 */
> -	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_SGMII, 0x1),
> -	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1),
> +	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
>  	/* lane 2 */
> -	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_SGMII, 0x1),
> -	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_2500BASEX, 0x1),
> -	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_10GKR, 0x1),
> +	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
> +	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_10GKR, 0x1, COMPHY_FW_MODE_SFI),
>  	/* lane 3 */
> -	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_SGMII, 0x2),
> -	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_2500BASEX, 0x2),
> +	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
>  	/* lane 4 */
> -	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_SGMII, 0x2),
> -	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_2500BASEX, 0x2),
> -	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_10GKR, 0x2),
> -	MVEBU_COMPHY_CONF(4, 1, PHY_INTERFACE_MODE_SGMII, 0x1),
> +	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
> +	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_10GKR, 0x2, COMPHY_FW_MODE_SFI),
> +	MVEBU_COMPHY_CONF(4, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
>  	/* lane 5 */
> -	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_SGMII, 0x1),
> -	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1),
> +	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
> +	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
>  };
>  
>  struct mvebu_comphy_priv {
>  	void __iomem *base;
>  	struct regmap *regmap;
>  	struct device *dev;
> +	unsigned long cp_phys;
>  };
>  
>  struct mvebu_comphy_lane {
> @@ -170,8 +208,18 @@ struct mvebu_comphy_lane {
>  	int port;
>  };
>  
> -static int mvebu_comphy_get_mux(int lane, int port,
> -				enum phy_mode mode, int submode)
> +static int mvebu_comphy_smc(unsigned long function, unsigned long phys,
> +			    unsigned long lane, unsigned long mode)
> +{
> +	struct arm_smccc_res res;
> +
> +	arm_smccc_smc(function, phys, lane, mode, 0, 0, 0, 0, &res);

Before we switch to using the ATF implementation, I have serious
concerns about the API here.  The physical address is passed from
the kernel to ATF for the hardware, and ATF does very little
validation of that address.

This means that it's trivially possible to overrun arrays in ATF,
and even get ATF to read/write other areas of memory.

I would rather this issue was resolved before the kernel starts to
use what is a badly defined interface.

> +
> +	return res.a0;
> +}
> +
> +static int mvebu_comphy_get_mode(bool fw_mode, int lane, int port,
> +				 enum phy_mode mode, int submode)
>  {
>  	int i, n = ARRAY_SIZE(mvebu_comphy_cp110_modes);
>  
> @@ -190,7 +238,22 @@ static int mvebu_comphy_get_mux(int lane, int port,
>  	if (i == n)
>  		return -EINVAL;
>  
> -	return mvebu_comphy_cp110_modes[i].mux;
> +	if (fw_mode)
> +		return mvebu_comphy_cp110_modes[i].fw_mode;
> +	else
> +		return mvebu_comphy_cp110_modes[i].mux;
> +}
> +
> +static inline int mvebu_comphy_get_mux(int lane, int port,
> +				       enum phy_mode mode, int submode)
> +{
> +	return mvebu_comphy_get_mode(false, lane, port, mode, submode);
> +}
> +
> +static inline int mvebu_comphy_get_fw_mode(int lane, int port,
> +					   enum phy_mode mode, int submode)
> +{
> +	return mvebu_comphy_get_mode(true, lane, port, mode, submode);
>  }
>  
>  static void mvebu_comphy_ethernet_init_reset(struct mvebu_comphy_lane *lane)
> @@ -476,7 +539,7 @@ static int mvebu_comphy_set_mode_10gkr(struct phy *phy)
>  	return mvebu_comphy_init_plls(lane);
>  }
>  
> -static int mvebu_comphy_power_on(struct phy *phy)
> +static int mvebu_comphy_power_on_legacy(struct phy *phy)
>  {
>  	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
>  	struct mvebu_comphy_priv *priv = lane->priv;
> @@ -517,6 +580,50 @@ static int mvebu_comphy_power_on(struct phy *phy)
>  	return ret;
>  }
>  
> +static int mvebu_comphy_power_on(struct phy *phy)
> +{
> +	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
> +	struct mvebu_comphy_priv *priv = lane->priv;
> +	u32 fw_param = 0;
> +	int fw_mode;
> +	int ret;
> +
> +	fw_mode = mvebu_comphy_get_fw_mode(lane->id, lane->port,
> +					   lane->mode, lane->submode);
> +	if (fw_mode < 0)
> +		goto try_legacy;
> +
> +	/* Try SMC flow first */
> +	switch (lane->mode) {
> +	case PHY_MODE_ETHERNET:
> +		dev_dbg(priv->dev, "set lane %d to Ethernet mode\n", lane->id);
> +		switch (lane->submode) {
> +		case PHY_INTERFACE_MODE_SGMII:
> +		case PHY_INTERFACE_MODE_2500BASEX:
> +		case PHY_INTERFACE_MODE_10GKR:
> +			fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);
> +			break;
> +		default:
> +			dev_err(priv->dev, "unsupported Ethernet mode (%d)\n",
> +				lane->submode);
> +			return -ENOTSUPP;
> +		}
> +		break;
> +	default:
> +		dev_err(priv->dev, "unsupported PHY mode (%d)\n", lane->mode);
> +		return -ENOTSUPP;
> +	}
> +
> +	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_ON, priv->cp_phys, lane->id,
> +			       fw_param);
> +	if (!ret)
> +		return ret;
> +
> +try_legacy:
> +	/* Fallback to Linux's implementation */
> +	return mvebu_comphy_power_on_legacy(phy);
> +}
> +
>  static int mvebu_comphy_set_mode(struct phy *phy,
>  				 enum phy_mode mode, int submode)
>  {
> @@ -528,7 +635,7 @@ static int mvebu_comphy_set_mode(struct phy *phy,
>  	if (submode == PHY_INTERFACE_MODE_1000BASEX)
>  		submode = PHY_INTERFACE_MODE_SGMII;
>  
> -	if (mvebu_comphy_get_mux(lane->id, lane->port, mode, submode) < 0)
> +	if (mvebu_comphy_get_fw_mode(lane->id, lane->port, mode, submode) < 0)
>  		return -EINVAL;
>  
>  	lane->mode = mode;
> @@ -536,27 +643,42 @@ static int mvebu_comphy_set_mode(struct phy *phy,
>  	return 0;
>  }
>  
> +static int mvebu_comphy_power_off_legacy(struct phy *phy)
> +{
> +	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
> +	struct mvebu_comphy_priv *priv = lane->priv;
> +	u32 val;
> +
> +	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
> +	val &= ~(MVEBU_COMPHY_SERDES_CFG1_RESET |
> +		 MVEBU_COMPHY_SERDES_CFG1_CORE_RESET |
> +		 MVEBU_COMPHY_SERDES_CFG1_RF_RESET);
> +	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
> +
> +	regmap_read(priv->regmap, MVEBU_COMPHY_SELECTOR, &val);
> +	val &= ~(0xf << MVEBU_COMPHY_SELECTOR_PHY(lane->id));
> +	regmap_write(priv->regmap, MVEBU_COMPHY_SELECTOR, val);
> +
> +	regmap_read(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, &val);
> +	val &= ~(0xf << MVEBU_COMPHY_PIPE_SELECTOR_PIPE(lane->id));
> +	regmap_write(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, val);
> +
> +	return 0;
> +}
> +
>  static int mvebu_comphy_power_off(struct phy *phy)
>  {
>  	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
>  	struct mvebu_comphy_priv *priv = lane->priv;
> -	u32 val;
> +	int ret;
>  
> -	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
> -	val &= ~(MVEBU_COMPHY_SERDES_CFG1_RESET |
> -		 MVEBU_COMPHY_SERDES_CFG1_CORE_RESET |
> -		 MVEBU_COMPHY_SERDES_CFG1_RF_RESET);
> -	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
> +	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_OFF, priv->cp_phys,
> +			       lane->id, 0);
> +	if (!ret)
> +		return ret;
>  
> -	regmap_read(priv->regmap, MVEBU_COMPHY_SELECTOR, &val);
> -	val &= ~(0xf << MVEBU_COMPHY_SELECTOR_PHY(lane->id));
> -	regmap_write(priv->regmap, MVEBU_COMPHY_SELECTOR, val);
> -
> -	regmap_read(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, &val);
> -	val &= ~(0xf << MVEBU_COMPHY_PIPE_SELECTOR_PIPE(lane->id));
> -	regmap_write(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, val);
> -
> -	return 0;
> +	/* Fallback to Linux's implementation */
> +	return mvebu_comphy_power_off_legacy(phy);
>  }
>  
>  static const struct phy_ops mvebu_comphy_ops = {
> @@ -607,6 +729,12 @@ static int mvebu_comphy_probe(struct platform_device *pdev)
>  	if (IS_ERR(priv->base))
>  		return PTR_ERR(priv->base);
>  
> +	/*
> +	 * Hack to retrieve a physical offset relative to this CP that will be
> +	 * given to the firmware
> +	 */
> +	priv->cp_phys = res->start;
> +
>  	for_each_available_child_of_node(pdev->dev.of_node, child) {
>  		struct mvebu_comphy_lane *lane;
>  		struct phy *phy;
> -- 
> 2.19.1
> 
>
diff mbox series

Patch

diff --git a/drivers/phy/marvell/phy-mvebu-cp110-comphy.c b/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
index 3f50a1521f01..4aaf161503d6 100644
--- a/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
+++ b/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
@@ -5,6 +5,7 @@ 
  * Antoine Tenart <antoine.tenart@free-electrons.com>
  */
 
+#include <linux/arm-smccc.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
 #include <linux/mfd/syscon.h>
@@ -115,51 +116,88 @@ 
 #define MVEBU_COMPHY_LANES	6
 #define MVEBU_COMPHY_PORTS	3
 
+#define COMPHY_SIP_POWER_ON	0x82000001
+#define COMPHY_SIP_POWER_OFF	0x82000002
+
+/*
+ * A lane is described by the following bitfields:
+ * [ 1- 0]: COMPHY polarity invertion
+ * [ 2- 7]: COMPHY speed
+ * [ 5-11]: COMPHY port index
+ * [12-16]: COMPHY mode
+ * [17]: Clock source
+ */
+#define COMPHY_FW_POL_OFFSET	0
+#define COMPHY_FW_POL_MASK	GENMASK(1, 0)
+#define COMPHY_FW_SPEED_OFFSET	2
+#define COMPHY_FW_SPEED_MASK	GENMASK(7, 2)
+#define COMPHY_FW_SPEED_MAX	COMPHY_FW_SPEED_MASK
+#define COMPHY_FW_PORT_OFFSET	8
+#define COMPHY_FW_PORT_MASK	GENMASK(11, 8)
+#define COMPHY_FW_MODE_OFFSET	12
+#define COMPHY_FW_MODE_MASK	GENMASK(16, 12)
+
+#define COMPHY_FW_PARAM_FULL(mode, port, speed, pol)			\
+	((((pol) << COMPHY_FW_POL_OFFSET) & COMPHY_FW_POL_MASK) |	\
+	 (((mode) << COMPHY_FW_MODE_OFFSET) & COMPHY_FW_MODE_MASK) |	\
+	 (((port) << COMPHY_FW_PORT_OFFSET) & COMPHY_FW_PORT_MASK) |	\
+	 (((speed) << COMPHY_FW_SPEED_OFFSET) & COMPHY_FW_SPEED_MASK))
+
+#define COMPHY_FW_PARAM(mode, port)					\
+	COMPHY_FW_PARAM_FULL(mode, port, COMPHY_FW_SPEED_MAX, 0)
+
+#define COMPHY_FW_MODE_SGMII		0x2 /* SGMII 1G */
+#define COMPHY_FW_MODE_HS_SGMII		0x3 /* SGMII 2.5G */
+#define COMPHY_FW_MODE_SFI		0x9 /* XFI: 0x8 (is treated like SFI) */
+
 struct mvebu_comphy_conf {
 	enum phy_mode mode;
 	int submode;
 	unsigned lane;
 	unsigned port;
 	u32 mux;
+	u32 fw_mode;
 };
 
-#define MVEBU_COMPHY_CONF(_lane, _port, _submode, _mux)	\
+#define MVEBU_COMPHY_CONF(_lane, _port, _submode, _mux, _fw)	\
 	{						\
 		.lane = _lane,				\
 		.port = _port,				\
 		.mode = PHY_MODE_ETHERNET,		\
 		.submode = _submode,			\
 		.mux = _mux,				\
+		.fw_mode = _fw,				\
 	}
 
 static const struct mvebu_comphy_conf mvebu_comphy_cp110_modes[] = {
 	/* lane 0 */
-	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_SGMII, 0x1),
-	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_2500BASEX, 0x1),
+	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(0, 1, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
 	/* lane 1 */
-	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_SGMII, 0x1),
-	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1),
+	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(1, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
 	/* lane 2 */
-	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_SGMII, 0x1),
-	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_2500BASEX, 0x1),
-	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_10GKR, 0x1),
+	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
+	MVEBU_COMPHY_CONF(2, 0, PHY_INTERFACE_MODE_10GKR, 0x1, COMPHY_FW_MODE_SFI),
 	/* lane 3 */
-	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_SGMII, 0x2),
-	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_2500BASEX, 0x2),
+	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(3, 1, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
 	/* lane 4 */
-	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_SGMII, 0x2),
-	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_2500BASEX, 0x2),
-	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_10GKR, 0x2),
-	MVEBU_COMPHY_CONF(4, 1, PHY_INTERFACE_MODE_SGMII, 0x1),
+	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
+	MVEBU_COMPHY_CONF(4, 0, PHY_INTERFACE_MODE_10GKR, 0x2, COMPHY_FW_MODE_SFI),
+	MVEBU_COMPHY_CONF(4, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
 	/* lane 5 */
-	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_SGMII, 0x1),
-	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1),
+	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
+	MVEBU_COMPHY_CONF(5, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
 };
 
 struct mvebu_comphy_priv {
 	void __iomem *base;
 	struct regmap *regmap;
 	struct device *dev;
+	unsigned long cp_phys;
 };
 
 struct mvebu_comphy_lane {
@@ -170,8 +208,18 @@  struct mvebu_comphy_lane {
 	int port;
 };
 
-static int mvebu_comphy_get_mux(int lane, int port,
-				enum phy_mode mode, int submode)
+static int mvebu_comphy_smc(unsigned long function, unsigned long phys,
+			    unsigned long lane, unsigned long mode)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(function, phys, lane, mode, 0, 0, 0, 0, &res);
+
+	return res.a0;
+}
+
+static int mvebu_comphy_get_mode(bool fw_mode, int lane, int port,
+				 enum phy_mode mode, int submode)
 {
 	int i, n = ARRAY_SIZE(mvebu_comphy_cp110_modes);
 
@@ -190,7 +238,22 @@  static int mvebu_comphy_get_mux(int lane, int port,
 	if (i == n)
 		return -EINVAL;
 
-	return mvebu_comphy_cp110_modes[i].mux;
+	if (fw_mode)
+		return mvebu_comphy_cp110_modes[i].fw_mode;
+	else
+		return mvebu_comphy_cp110_modes[i].mux;
+}
+
+static inline int mvebu_comphy_get_mux(int lane, int port,
+				       enum phy_mode mode, int submode)
+{
+	return mvebu_comphy_get_mode(false, lane, port, mode, submode);
+}
+
+static inline int mvebu_comphy_get_fw_mode(int lane, int port,
+					   enum phy_mode mode, int submode)
+{
+	return mvebu_comphy_get_mode(true, lane, port, mode, submode);
 }
 
 static void mvebu_comphy_ethernet_init_reset(struct mvebu_comphy_lane *lane)
@@ -476,7 +539,7 @@  static int mvebu_comphy_set_mode_10gkr(struct phy *phy)
 	return mvebu_comphy_init_plls(lane);
 }
 
-static int mvebu_comphy_power_on(struct phy *phy)
+static int mvebu_comphy_power_on_legacy(struct phy *phy)
 {
 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
 	struct mvebu_comphy_priv *priv = lane->priv;
@@ -517,6 +580,50 @@  static int mvebu_comphy_power_on(struct phy *phy)
 	return ret;
 }
 
+static int mvebu_comphy_power_on(struct phy *phy)
+{
+	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
+	struct mvebu_comphy_priv *priv = lane->priv;
+	u32 fw_param = 0;
+	int fw_mode;
+	int ret;
+
+	fw_mode = mvebu_comphy_get_fw_mode(lane->id, lane->port,
+					   lane->mode, lane->submode);
+	if (fw_mode < 0)
+		goto try_legacy;
+
+	/* Try SMC flow first */
+	switch (lane->mode) {
+	case PHY_MODE_ETHERNET:
+		dev_dbg(priv->dev, "set lane %d to Ethernet mode\n", lane->id);
+		switch (lane->submode) {
+		case PHY_INTERFACE_MODE_SGMII:
+		case PHY_INTERFACE_MODE_2500BASEX:
+		case PHY_INTERFACE_MODE_10GKR:
+			fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);
+			break;
+		default:
+			dev_err(priv->dev, "unsupported Ethernet mode (%d)\n",
+				lane->submode);
+			return -ENOTSUPP;
+		}
+		break;
+	default:
+		dev_err(priv->dev, "unsupported PHY mode (%d)\n", lane->mode);
+		return -ENOTSUPP;
+	}
+
+	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_ON, priv->cp_phys, lane->id,
+			       fw_param);
+	if (!ret)
+		return ret;
+
+try_legacy:
+	/* Fallback to Linux's implementation */
+	return mvebu_comphy_power_on_legacy(phy);
+}
+
 static int mvebu_comphy_set_mode(struct phy *phy,
 				 enum phy_mode mode, int submode)
 {
@@ -528,7 +635,7 @@  static int mvebu_comphy_set_mode(struct phy *phy,
 	if (submode == PHY_INTERFACE_MODE_1000BASEX)
 		submode = PHY_INTERFACE_MODE_SGMII;
 
-	if (mvebu_comphy_get_mux(lane->id, lane->port, mode, submode) < 0)
+	if (mvebu_comphy_get_fw_mode(lane->id, lane->port, mode, submode) < 0)
 		return -EINVAL;
 
 	lane->mode = mode;
@@ -536,27 +643,42 @@  static int mvebu_comphy_set_mode(struct phy *phy,
 	return 0;
 }
 
+static int mvebu_comphy_power_off_legacy(struct phy *phy)
+{
+	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
+	struct mvebu_comphy_priv *priv = lane->priv;
+	u32 val;
+
+	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
+	val &= ~(MVEBU_COMPHY_SERDES_CFG1_RESET |
+		 MVEBU_COMPHY_SERDES_CFG1_CORE_RESET |
+		 MVEBU_COMPHY_SERDES_CFG1_RF_RESET);
+	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
+
+	regmap_read(priv->regmap, MVEBU_COMPHY_SELECTOR, &val);
+	val &= ~(0xf << MVEBU_COMPHY_SELECTOR_PHY(lane->id));
+	regmap_write(priv->regmap, MVEBU_COMPHY_SELECTOR, val);
+
+	regmap_read(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, &val);
+	val &= ~(0xf << MVEBU_COMPHY_PIPE_SELECTOR_PIPE(lane->id));
+	regmap_write(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, val);
+
+	return 0;
+}
+
 static int mvebu_comphy_power_off(struct phy *phy)
 {
 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
 	struct mvebu_comphy_priv *priv = lane->priv;
-	u32 val;
+	int ret;
 
-	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
-	val &= ~(MVEBU_COMPHY_SERDES_CFG1_RESET |
-		 MVEBU_COMPHY_SERDES_CFG1_CORE_RESET |
-		 MVEBU_COMPHY_SERDES_CFG1_RF_RESET);
-	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
+	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_OFF, priv->cp_phys,
+			       lane->id, 0);
+	if (!ret)
+		return ret;
 
-	regmap_read(priv->regmap, MVEBU_COMPHY_SELECTOR, &val);
-	val &= ~(0xf << MVEBU_COMPHY_SELECTOR_PHY(lane->id));
-	regmap_write(priv->regmap, MVEBU_COMPHY_SELECTOR, val);
-
-	regmap_read(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, &val);
-	val &= ~(0xf << MVEBU_COMPHY_PIPE_SELECTOR_PIPE(lane->id));
-	regmap_write(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, val);
-
-	return 0;
+	/* Fallback to Linux's implementation */
+	return mvebu_comphy_power_off_legacy(phy);
 }
 
 static const struct phy_ops mvebu_comphy_ops = {
@@ -607,6 +729,12 @@  static int mvebu_comphy_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->base))
 		return PTR_ERR(priv->base);
 
+	/*
+	 * Hack to retrieve a physical offset relative to this CP that will be
+	 * given to the firmware
+	 */
+	priv->cp_phys = res->start;
+
 	for_each_available_child_of_node(pdev->dev.of_node, child) {
 		struct mvebu_comphy_lane *lane;
 		struct phy *phy;