[PATCH] sk98: ethtool pause param support

The basic stuff comes from the newer code from SysKonnect, but there
code had ugly way to find the PNMI instance. This driver seems full
of places where they do unnecessary board/port bookeeping (*sigh*)
Signed-off-by: Stephen Hemminger <shemminger@osdl.org>
Signed-off-by: Jeff Garzik <jgarzik@pobox.com>

drivers/net/sk98lin/skethtool.c

@@ -163,6 +163,15 @@ static int getSettings(struct net_device *dev, struct ethtool_cmd *ecmd)
 	return 0;
 }
 
+/*
+ * MIB infrastructure uses instance value starting at 1
+ * based on board and port.
+ */
+static inline u32 pnmiInstance(const DEV_NET *pNet)
+{
+	return 1 + (pNet->pAC->RlmtNets == 2) + pNet->PortNr;
+}
+
 /*****************************************************************************
  *
  *	setSettings - configures the settings of a selected adapter
@@ -200,7 +209,7 @@ static int setSettings(struct net_device *dev, struct ethtool_cmd *ecmd)
 		*buf = (ecmd->duplex == DUPLEX_FULL) 
 			? SK_LMODE_AUTOFULL : SK_LMODE_AUTOHALF;
 	
-	instance = 1 + (pAC->RlmtNets == 2) + pNet->PortNr;
+	instance = pnmiInstance(pNet);
 	if (SkPnmiSetVar(pAC, pAC->IoBase, OID_SKGE_LINK_MODE, 
 			   &buf, &len, instance, pNet->NetNr) != SK_PNMI_ERR_OK)
 		return -EINVAL;
@@ -437,6 +446,102 @@ static int locateDevice(struct net_device *dev, u32 data)
 	return 0;
 }
 
+/*****************************************************************************
+ *
+ * 	getPauseParams - retrieves the pause parameters
+ *
+ * Description:
+ *	All current pause parameters of a selected adapter are placed 
+ *	in the passed ethtool_pauseparam structure and are returned.
+ *
+ * Returns:	N/A
+ *
+ */
+static void getPauseParams(struct net_device *dev, struct ethtool_pauseparam *epause) 
+{
+	DEV_NET	*pNet = netdev_priv(dev);
+	SK_AC *pAC = pNet->pAC;
+	SK_GEPORT *pPort = &pAC->GIni.GP[pNet->PortNr];
+
+	epause->rx_pause = (pPort->PFlowCtrlMode == SK_FLOW_MODE_SYMMETRIC) ||
+		  (pPort->PFlowCtrlMode == SK_FLOW_MODE_SYM_OR_REM);
+
+	epause->tx_pause = epause->rx_pause || (pPort->PFlowCtrlMode == SK_FLOW_MODE_LOC_SEND);
+	epause->autoneg = epause->rx_pause || epause->tx_pause;
+}
+
+/*****************************************************************************
+ *
+ *	setPauseParams - configures the pause parameters of an adapter
+ *
+ * Description:
+ *	This function sets the Rx or Tx pause parameters 
+ *
+ * Returns:
+ *	==0:	everything fine, no error
+ *	!=0:	the return value is the error code of the failure 
+ */
+static int setPauseParams(struct net_device *dev , struct ethtool_pauseparam *epause)
+{
+	DEV_NET	*pNet = netdev_priv(dev);
+	SK_AC *pAC = pNet->pAC;
+	SK_GEPORT *pPort = &pAC->GIni.GP[pNet->PortNr];
+	u32	instance = pnmiInstance(pNet);
+	struct ethtool_pauseparam old;
+	u8	oldspeed = pPort->PLinkSpeedUsed;
+	char	buf[4];
+	int	len = 1;
+	int ret;
+
+	/*
+	** we have to determine the current settings to see if 
+	** the operator requested any modification of the flow 
+	** control parameters...
+	*/
+	getPauseParams(dev, &old);
+
+	/*
+	** perform modifications regarding the changes 
+	** requested by the operator
+	*/
+	if (epause->autoneg != old.autoneg) 
+		*buf = epause->autoneg ? SK_FLOW_MODE_NONE : SK_FLOW_MODE_SYMMETRIC;
+	else {
+		if (epause->rx_pause && epause->tx_pause) 
+			*buf = SK_FLOW_MODE_SYMMETRIC;
+		else if (epause->rx_pause && !epause->tx_pause)
+			*buf =  SK_FLOW_MODE_SYM_OR_REM;
+		else if (!epause->rx_pause && epause->tx_pause)
+			*buf =  SK_FLOW_MODE_LOC_SEND;
+		else
+			*buf = SK_FLOW_MODE_NONE;
+	}
+
+	ret = SkPnmiSetVar(pAC, pAC->IoBase, OID_SKGE_FLOWCTRL_MODE,
+			 &buf, &len, instance, pNet->NetNr);
+
+	if (ret != SK_PNMI_ERR_OK) {
+		SK_DBG_MSG(NULL, SK_DBGMOD_DRV, SK_DBGCAT_CTRL,
+			   ("ethtool (sk98lin): error changing rx/tx pause (%i)\n", ret));
+		goto err;
+	}
+
+	/*
+	** It may be that autoneg has been disabled! Therefore
+	** set the speed to the previously used value...
+	*/
+	if (!epause->autoneg) {
+		len = 1;
+		ret = SkPnmiSetVar(pAC, pAC->IoBase, OID_SKGE_SPEED_MODE, 
+				   &oldspeed, &len, instance, pNet->NetNr);
+		if (ret != SK_PNMI_ERR_OK) 
+			SK_DBG_MSG(NULL, SK_DBGMOD_DRV, SK_DBGCAT_CTRL,
+				   ("ethtool (sk98lin): error setting speed (%i)\n", ret));
+	}
+ err:
+        return ret ? -EIO : 0;
+}
+
 struct ethtool_ops SkGeEthtoolOps = {
 	.get_settings		= getSettings,
 	.set_settings		= setSettings,
@@ -445,4 +550,6 @@ struct ethtool_ops SkGeEthtoolOps = {
 	.get_stats_count	= getStatsCount,
 	.get_ethtool_stats	= getEthtoolStats,
 	.phys_id		= locateDevice,
+	.get_pauseparam		= getPauseParams,
+	.set_pauseparam		= setPauseParams,
 };