From b505f9a736c3850aa1dfefd7a803d797c2233aab Mon Sep 17 00:00:00 2001
From: Razvan Stefanescu <razvan.stefanescu@nxp.com>
Date: Thu, 13 Apr 2017 13:10:34 +0300
Subject: [PATCH] staging: dpaa2-evb: Improve ethtool support

Improve ethtool support by adding ops for:
- driver info
- link status
- auto-negotiation setting and result
- speed setting and result

Signed-off-by: Razvan Stefanescu <razvan.stefanescu@nxp.com>
---
 drivers/staging/fsl-dpaa2/evb/evb.c | 114 ++++++++++++++++++++++++++++++++++++
 1 file changed, 114 insertions(+)

--- a/drivers/staging/fsl-dpaa2/evb/evb.c
+++ b/drivers/staging/fsl-dpaa2/evb/evb.c
@@ -43,6 +43,8 @@
 #include "dpdmux.h"
 #include "dpdmux-cmd.h"
 
+static const char evb_drv_version[] = "0.1";
+
 /* Minimal supported DPDMUX version */
 #define DPDMUX_MIN_VER_MAJOR			6
 #define DPDMUX_MIN_VER_MINOR			0
@@ -860,6 +862,114 @@ static const struct net_device_ops evb_p
 	.ndo_change_mtu		= &evb_change_mtu,
 };
 
+static void evb_get_drvinfo(struct net_device *netdev,
+			    struct ethtool_drvinfo *drvinfo)
+{
+	struct evb_port_priv *port_priv = netdev_priv(netdev);
+	u16 version_major, version_minor;
+	int err;
+
+	strlcpy(drvinfo->driver, KBUILD_MODNAME, sizeof(drvinfo->driver));
+	strlcpy(drvinfo->version, evb_drv_version, sizeof(drvinfo->version));
+
+	err = dpdmux_get_api_version(port_priv->evb_priv->mc_io, 0,
+				     &version_major,
+				     &version_minor);
+	if (err)
+		strlcpy(drvinfo->fw_version, "N/A",
+			sizeof(drvinfo->fw_version));
+	else
+		snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+			 "%u.%u", version_major, version_minor);
+
+	strlcpy(drvinfo->bus_info, dev_name(netdev->dev.parent->parent),
+		sizeof(drvinfo->bus_info));
+}
+
+static int evb_get_link_ksettings(struct net_device *netdev,
+				  struct ethtool_link_ksettings *link_settings)
+{
+	struct evb_port_priv *port_priv = netdev_priv(netdev);
+	struct dpdmux_link_state state = {0};
+	int err = 0;
+
+	err = dpdmux_if_get_link_state(port_priv->evb_priv->mc_io, 0,
+				       port_priv->evb_priv->mux_handle,
+				       port_priv->port_index,
+				       &state);
+	if (err) {
+		netdev_err(netdev, "ERROR %d getting link state", err);
+		goto out;
+	}
+
+	/* At the moment, we have no way of interrogating the DPMAC
+	 * from the DPDMUX side or there may not exist a DPMAC at all.
+	 * Report only autoneg state, duplexity and speed.
+	 */
+	if (state.options & DPDMUX_LINK_OPT_AUTONEG)
+		link_settings->base.autoneg = AUTONEG_ENABLE;
+	if (!(state.options & DPDMUX_LINK_OPT_HALF_DUPLEX))
+		link_settings->base.duplex = DUPLEX_FULL;
+	link_settings->base.speed = state.rate;
+
+out:
+	return err;
+}
+
+static int evb_set_link_ksettings(struct net_device *netdev,
+				  const struct ethtool_link_ksettings *link_settings)
+{
+	struct evb_port_priv *port_priv = netdev_priv(netdev);
+	struct dpdmux_link_state state = {0};
+	struct dpdmux_link_cfg cfg = {0};
+	int err = 0;
+
+	netdev_dbg(netdev, "Setting link parameters...");
+
+	err = dpdmux_if_get_link_state(port_priv->evb_priv->mc_io, 0,
+				       port_priv->evb_priv->mux_handle,
+				       port_priv->port_index,
+				       &state);
+	if (err) {
+		netdev_err(netdev, "ERROR %d getting link state", err);
+		goto out;
+	}
+
+	/* Due to a temporary MC limitation, the DPDMUX port must be down
+	 * in order to be able to change link settings. Taking steps to let
+	 * the user know that.
+	 */
+	if (netif_running(netdev)) {
+		netdev_info(netdev,
+			    "Sorry, interface must be brought down first.\n");
+		return -EACCES;
+	}
+
+	cfg.options = state.options;
+	cfg.rate = link_settings->base.speed;
+	if (link_settings->base.autoneg == AUTONEG_ENABLE)
+		cfg.options |= DPDMUX_LINK_OPT_AUTONEG;
+	else
+		cfg.options &= ~DPDMUX_LINK_OPT_AUTONEG;
+	if (link_settings->base.duplex == DUPLEX_HALF)
+		cfg.options |= DPDMUX_LINK_OPT_HALF_DUPLEX;
+	else
+		cfg.options &= ~DPDMUX_LINK_OPT_HALF_DUPLEX;
+
+	err = dpdmux_if_set_link_cfg(port_priv->evb_priv->mc_io, 0,
+				     port_priv->evb_priv->mux_handle,
+				     port_priv->port_index,
+				     &cfg);
+	if (err)
+		/* ethtool will be loud enough if we return an error; no point
+		 * in putting our own error message on the console by default
+		 */
+		netdev_dbg(netdev, "ERROR %d setting link cfg", err);
+
+out:
+	return err;
+}
+
 static struct {
 	enum dpdmux_counter_type id;
 	char name[ETH_GSTRING_LEN];
@@ -923,6 +1033,10 @@ static void evb_ethtool_get_stats(struct
 }
 
 static const struct ethtool_ops evb_port_ethtool_ops = {
+	.get_drvinfo		= &evb_get_drvinfo,
+	.get_link		= &ethtool_op_get_link,
+	.get_link_ksettings	= &evb_get_link_ksettings,
+	.set_link_ksettings	= &evb_set_link_ksettings,
 	.get_strings		= &evb_ethtool_get_strings,
 	.get_ethtool_stats	= &evb_ethtool_get_stats,
 	.get_sset_count		= &evb_ethtool_get_sset_count,