[PATCH] mv643xx_eth: use MII library for ethtool functions

Use the common ethtool support functions of the MII library.
Add generic MII ioctl handler.
Add PHY parameter speed/duplex/negotiation initialization and modification.

Signed-off-by: James Chapman <jchapman@katalix.com>
Signed-off-by: Dale Farnsworth <dale@farnsworth.org>
Signed-off-by: Jeff Garzik <jgarzik@pobox.com>

1 files changed, 210 insertions, 141 deletions

diff --git a/drivers/net/mv643xx_eth.c b/drivers/net/mv643xx_eth.c
index bca7257c707a..ff2b613a7436 100644
--- a/drivers/net/mv643xx_eth.c
+++ b/drivers/net/mv643xx_eth.c
@@ -101,6 +101,7 @@ static void ethernet_phy_set(unsigned int eth_port_num, int phy_addr);
 static int ethernet_phy_detect(unsigned int eth_port_num);
 static int mv643xx_mdio_read(struct net_device *dev, int phy_id, int location);
 static void mv643xx_mdio_write(struct net_device *dev, int phy_id, int location, int val);
+static int mv643xx_eth_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
 static struct ethtool_ops mv643xx_ethtool_ops;
 
 static char mv643xx_driver_name[] = "mv643xx_eth";
@@ -457,6 +458,56 @@ static int mv643xx_eth_receive_queue(struct net_device *dev)
         return received_packets;
 }
 
+/* Set the mv643xx port configuration register for the speed/duplex mode. */
+static void mv643xx_eth_update_pscr(struct net_device *dev,
+                                    struct ethtool_cmd *ecmd)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+        int port_num = mp->port_num;
+        u32 o_pscr, n_pscr;
+        unsigned int channels;
+
+        o_pscr = mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num));
+        n_pscr = o_pscr;
+
+        /* clear speed, duplex and rx buffer size fields */
+        n_pscr &= ~(MV643XX_ETH_SET_MII_SPEED_TO_100  |
+                   MV643XX_ETH_SET_GMII_SPEED_TO_1000 |
+                   MV643XX_ETH_SET_FULL_DUPLEX_MODE   |
+                   MV643XX_ETH_MAX_RX_PACKET_MASK);
+
+        if (ecmd->duplex == DUPLEX_FULL)
+                n_pscr |= MV643XX_ETH_SET_FULL_DUPLEX_MODE;
+
+        if (ecmd->speed == SPEED_1000)
+                n_pscr |= MV643XX_ETH_SET_GMII_SPEED_TO_1000 |
+                          MV643XX_ETH_MAX_RX_PACKET_9700BYTE;
+        else {
+                if (ecmd->speed == SPEED_100)
+                        n_pscr |= MV643XX_ETH_SET_MII_SPEED_TO_100;
+                n_pscr |= MV643XX_ETH_MAX_RX_PACKET_1522BYTE;
+        }
+
+        if (n_pscr != o_pscr) {
+                if ((o_pscr & MV643XX_ETH_SERIAL_PORT_ENABLE) == 0)
+                        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
+                                                                n_pscr);
+                else {
+                        channels = mv643xx_eth_port_disable_tx(port_num);
+
+                        o_pscr &= ~MV643XX_ETH_SERIAL_PORT_ENABLE;
+                        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
+                                                                o_pscr);
+                        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
+                                                                n_pscr);
+                        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
+                                                                n_pscr);
+                        if (channels)
+                                mv643xx_eth_port_enable_tx(port_num, channels);
+                }
+        }
+}
+
 /*
  * mv643xx_eth_int_handler
  *
@@ -539,13 +590,19 @@ static irqreturn_t mv643xx_eth_int_handler(int irq, void *dev_id,
         }
         /* PHY status changed */
         if (eth_int_cause_ext & (BIT16 | BIT20)) {
+                struct ethtool_cmd cmd;
+
                 if (mii_link_ok(&mp->mii)) {
+                        mii_ethtool_gset(&mp->mii, &cmd);
+                        mv643xx_eth_update_pscr(dev, &cmd);
                         if (!netif_carrier_ok(dev)) {
                                 netif_carrier_on(dev);
-                                netif_wake_queue(dev);
-                                /* Start TX queue */
-                                mv643xx_eth_port_enable_tx(port_num,
-                                                mp->port_tx_queue_command);
+                                if (mp->tx_ring_size > mp->tx_desc_count +
+                                                        MAX_DESCS_PER_SKB) {
+                                        netif_wake_queue(dev);
+                                        /* Start TX queue */
+                                        mv643xx_eth_port_enable_tx(port_num, mp->port_tx_queue_command);
+                                }
                         }
                 } else if (netif_carrier_ok(dev)) {
                         netif_stop_queue(dev);
@@ -729,6 +786,34 @@ static void ether_init_tx_desc_ring(struct mv643xx_private *mp)
         mp->port_tx_queue_command = 1;
 }
 
+static int mv643xx_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+        int err;
+
+        spin_lock_irq(&mp->lock);
+        err = mii_ethtool_sset(&mp->mii, cmd);
+        spin_unlock_irq(&mp->lock);
+
+        return err;
+}
+
+static int mv643xx_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+        int err;
+
+        spin_lock_irq(&mp->lock);
+        err = mii_ethtool_gset(&mp->mii, cmd);
+        spin_unlock_irq(&mp->lock);
+
+        /* The PHY may support 1000baseT_Half, but the mv643xx does not */
+        cmd->supported &= ~SUPPORTED_1000baseT_Half;
+        cmd->advertising &= ~ADVERTISED_1000baseT_Half;
+
+        return err;
+}
+
 /*
  * mv643xx_eth_open
  *
@@ -842,6 +927,10 @@ static int mv643xx_eth_open(struct net_device *dev)
 
         mv643xx_eth_rx_task(dev);       /* Fill RX ring with skb's */
 
+        /* Clear any pending ethernet port interrupts */
+        mv_write(MV643XX_ETH_INTERRUPT_CAUSE_REG(port_num), 0);
+        mv_write(MV643XX_ETH_INTERRUPT_CAUSE_EXTEND_REG(port_num), 0);
+
         eth_port_start(dev);
 
         /* Interrupt Coalescing */
@@ -854,16 +943,13 @@ static int mv643xx_eth_open(struct net_device *dev)
         mp->tx_int_coal =
                 eth_port_set_tx_coal(port_num, 133000000, MV643XX_TX_COAL);
 
-        /* Clear any pending ethernet port interrupts */
-        mv_write(MV643XX_ETH_INTERRUPT_CAUSE_REG(port_num), 0);
-        mv_write(MV643XX_ETH_INTERRUPT_CAUSE_EXTEND_REG(port_num), 0);
-
         /* Unmask phy and link status changes interrupts */
         mv_write(MV643XX_ETH_INTERRUPT_EXTEND_MASK_REG(port_num),
                                                 INT_UNMASK_ALL_EXT);
 
         /* Unmask RX buffer and TX end interrupt */
         mv_write(MV643XX_ETH_INTERRUPT_MASK_REG(port_num), INT_UNMASK_ALL);
+
         return 0;
 
 out_free_tx_skb:
@@ -1318,6 +1404,35 @@ static void mv643xx_netpoll(struct net_device *netdev)
 }
 #endif
 
+static void mv643xx_init_ethtool_cmd(struct net_device *dev, int phy_address,
+                                     int speed, int duplex,
+                                     struct ethtool_cmd *cmd)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+
+        memset(cmd, 0, sizeof(*cmd));
+
+        cmd->port = PORT_MII;
+        cmd->transceiver = XCVR_INTERNAL;
+        cmd->phy_address = phy_address;
+
+        if (speed == 0) {
+                cmd->autoneg = AUTONEG_ENABLE;
+                /* mii lib checks, but doesn't use speed on AUTONEG_ENABLE */
+                cmd->speed = SPEED_100;
+                cmd->advertising = ADVERTISED_10baseT_Half  |
+                                   ADVERTISED_10baseT_Full  |
+                                   ADVERTISED_100baseT_Half |
+                                   ADVERTISED_100baseT_Full;
+                if (mp->mii.supports_gmii)
+                        cmd->advertising |= ADVERTISED_1000baseT_Full;
+        } else {
+                cmd->autoneg = AUTONEG_DISABLE;
+                cmd->speed = speed;
+                cmd->duplex = duplex;
+        }
+}
+
 /*/
  * mv643xx_eth_probe
  *
@@ -1338,6 +1453,10 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
         u8 *p;
         struct resource *res;
         int err;
+        struct ethtool_cmd cmd;
+        u32 pscr;
+        int duplex;
+        int speed;
 
         dev = alloc_etherdev(sizeof(struct mv643xx_private));
         if (!dev)
@@ -1375,6 +1494,7 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
         dev->tx_queue_len = mp->tx_ring_size;
         dev->base_addr = 0;
         dev->change_mtu = mv643xx_eth_change_mtu;
+        dev->do_ioctl = mv643xx_eth_do_ioctl;
         SET_ETHTOOL_OPS(dev, &mv643xx_ethtool_ops);
 
 #ifdef MV643XX_CHECKSUM_OFFLOAD_TX
@@ -1452,10 +1572,35 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
                 pr_debug("MV643xx ethernet port %d: "
                                         "No PHY detected at addr %d\n",
                                         port_num, ethernet_phy_get(port_num));
-                return err;
+                goto out;
         }
 
+        pscr = mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num));
+        pscr &= ~MV643XX_ETH_SERIAL_PORT_ENABLE;
+        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), pscr);
+        pscr = mp->port_serial_control;
+        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), pscr);
+
+        if (!(pscr & MV643XX_ETH_DISABLE_AUTO_NEG_FOR_DUPLX) &&
+            !(pscr & MV643XX_ETH_DISABLE_AUTO_NEG_SPEED_GMII))
+                speed = 0;
+        else if (pscr & MV643XX_ETH_PORT_STATUS_GMII_1000)
+                speed = SPEED_1000;
+        else if (pscr & MV643XX_ETH_PORT_STATUS_MII_100)
+                speed = SPEED_100;
+        else
+                speed = SPEED_10;
+
+        if (pscr & MV643XX_ETH_PORT_STATUS_FULL_DUPLEX)
+                duplex = DUPLEX_FULL;
+        else
+                duplex = DUPLEX_HALF;
+
+        ethernet_phy_reset(mp->port_num);
         mp->mii.supports_gmii = mii_check_gmii_support(&mp->mii);
+        mv643xx_init_ethtool_cmd(dev, mp->mii.phy_id, speed, duplex, &cmd);
+        mv643xx_eth_update_pscr(dev, &cmd);
+        mv643xx_set_settings(dev, &cmd);
 
         err = register_netdev(dev);
         if (err)
@@ -1775,8 +1920,6 @@ static void eth_port_init(struct mv643xx_private *mp)
         eth_port_reset(mp->port_num);
 
         eth_port_init_mac_tables(mp->port_num);
-
-        ethernet_phy_reset(mp->port_num);
 }
 
 /*
@@ -1811,6 +1954,8 @@ static void eth_port_start(struct net_device *dev)
         struct mv643xx_private *mp = netdev_priv(dev);
         unsigned int port_num = mp->port_num;
         int tx_curr_desc, rx_curr_desc;
+        u32 pscr;
+        struct ethtool_cmd ethtool_cmd;
 
         /* Assignment of Tx CTRP of given queue */
         tx_curr_desc = mp->tx_curr_desc_q;
@@ -1828,31 +1973,35 @@ static void eth_port_start(struct net_device *dev)
         /* Assign port configuration and command. */
         mv_write(MV643XX_ETH_PORT_CONFIG_REG(port_num), mp->port_config);
 
-        mv_write(MV643XX_ETH_PORT_CONFIG_EXTEND_REG(port_num),
-                                                mp->port_config_extend);
+        pscr = mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num));
+        pscr &= ~MV643XX_ETH_SERIAL_PORT_ENABLE;
+        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), pscr);
 
+        pscr &= ~MV643XX_ETH_FORCE_LINK_PASS;
+        pscr |= MV643XX_ETH_DISABLE_AUTO_NEG_FOR_FLOW_CTRL |
+                MV643XX_ETH_DISABLE_AUTO_NEG_SPEED_GMII    |
+                MV643XX_ETH_DISABLE_AUTO_NEG_FOR_DUPLX     |
+                MV643XX_ETH_DO_NOT_FORCE_LINK_FAIL         |
+                MV643XX_ETH_SERIAL_PORT_CONTROL_RESERVED;
 
-        /* Increase the Rx side buffer size if supporting GigE */
-        if (mp->port_serial_control & MV643XX_ETH_SET_GMII_SPEED_TO_1000)
-                mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
-                        (mp->port_serial_control & 0xfff1ffff) | (0x5 << 17));
-        else
-                mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
-                                                mp->port_serial_control);
+        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), pscr);
 
-        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num),
-                mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num)) |
-                                                MV643XX_ETH_SERIAL_PORT_ENABLE);
+        pscr |= MV643XX_ETH_SERIAL_PORT_ENABLE;
+        mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), pscr);
 
         /* Assign port SDMA configuration */
-        mv_write(MV643XX_ETH_SDMA_CONFIG_REG(port_num),
-                                                        mp->port_sdma_config);
+        mv_write(MV643XX_ETH_SDMA_CONFIG_REG(port_num), mp->port_sdma_config);
 
         /* Enable port Rx. */
         mv643xx_eth_port_enable_rx(port_num, mp->port_rx_queue_command);
 
         /* Disable port bandwidth limits by clearing MTU register */
         mv_write(MV643XX_ETH_MAXIMUM_TRANSMIT_UNIT(port_num), 0);
+
+        /* save phy settings across reset */
+        mv643xx_get_settings(dev, &ethtool_cmd);
+        ethernet_phy_reset(mp->port_num);
+        mv643xx_set_settings(dev, &ethtool_cmd);
 }
 
 /*
@@ -2324,6 +2473,12 @@ static void ethernet_phy_reset(unsigned int eth_port_num)
         eth_port_read_smi_reg(eth_port_num, 0, &phy_reg_data);
         phy_reg_data |= 0x8000; /* Set bit 15 to reset the PHY */
         eth_port_write_smi_reg(eth_port_num, 0, phy_reg_data);
+
+        /* wait for PHY to come out of reset */
+        do {
+                udelay(1);
+                eth_port_read_smi_reg(eth_port_num, 0, &phy_reg_data);
+        } while (phy_reg_data & 0x8000);
 }
 
 static void mv643xx_eth_port_enable_tx(unsigned int port_num,
@@ -2417,20 +2572,13 @@ static void eth_port_reset(unsigned int port_num)
 
         /* Reset the Enable bit in the Configuration Register */
         reg_data = mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num));
-        reg_data &= ~MV643XX_ETH_SERIAL_PORT_ENABLE;
+        reg_data &= ~(MV643XX_ETH_SERIAL_PORT_ENABLE            |
+                        MV643XX_ETH_DO_NOT_FORCE_LINK_FAIL      |
+                        MV643XX_ETH_FORCE_LINK_PASS);
         mv_write(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num), reg_data);
 }
 
 
-static int eth_port_autoneg_supported(unsigned int eth_port_num)
-{
-        unsigned int phy_reg_data0;
-
-        eth_port_read_smi_reg(eth_port_num, 0, &phy_reg_data0);
-
-        return phy_reg_data0 & 0x1000;
-}
-
 /*
  * eth_port_read_smi_reg - Read PHY registers
  *
@@ -2989,111 +3137,6 @@ static const struct mv643xx_stats mv643xx_gstrings_stats[] = {
 #define MV643XX_STATS_LEN       \
         sizeof(mv643xx_gstrings_stats) / sizeof(struct mv643xx_stats)
 
-static int
-mv643xx_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
-{
-        struct mv643xx_private *mp = netdev->priv;
-        int port_num = mp->port_num;
-        int autoneg = eth_port_autoneg_supported(port_num);
-        int mode_10_bit;
-        int auto_duplex;
-        int half_duplex = 0;
-        int full_duplex = 0;
-        int auto_speed;
-        int speed_10 = 0;
-        int speed_100 = 0;
-        int speed_1000 = 0;
-
-        u32 pcs = mv_read(MV643XX_ETH_PORT_SERIAL_CONTROL_REG(port_num));
-        u32 psr = mv_read(MV643XX_ETH_PORT_STATUS_REG(port_num));
-
-        mode_10_bit = psr & MV643XX_ETH_PORT_STATUS_MODE_10_BIT;
-
-        if (mode_10_bit) {
-                ecmd->supported = SUPPORTED_10baseT_Half;
-        } else {
-                ecmd->supported = (SUPPORTED_10baseT_Half               |
-                                   SUPPORTED_10baseT_Full               |
-                                   SUPPORTED_100baseT_Half              |
-                                   SUPPORTED_100baseT_Full              |
-                                   SUPPORTED_1000baseT_Full             |
-                                   (autoneg ? SUPPORTED_Autoneg : 0)    |
-                                   SUPPORTED_TP);
-
-                auto_duplex = !(pcs & MV643XX_ETH_DISABLE_AUTO_NEG_FOR_DUPLX);
-                auto_speed = !(pcs & MV643XX_ETH_DISABLE_AUTO_NEG_SPEED_GMII);
-
-                ecmd->advertising = ADVERTISED_TP;
-
-                if (autoneg) {
-                        ecmd->advertising |= ADVERTISED_Autoneg;
-
-                        if (auto_duplex) {
-                                half_duplex = 1;
-                                full_duplex = 1;
-                        } else {
-                                if (pcs & MV643XX_ETH_SET_FULL_DUPLEX_MODE)
-                                        full_duplex = 1;
-                                else
-                                        half_duplex = 1;
-                        }
-
-                        if (auto_speed) {
-                                speed_10 = 1;
-                                speed_100 = 1;
-                                speed_1000 = 1;
-                        } else {
-                                if (pcs & MV643XX_ETH_SET_GMII_SPEED_TO_1000)
-                                        speed_1000 = 1;
-                                else if (pcs & MV643XX_ETH_SET_MII_SPEED_TO_100)
-                                        speed_100 = 1;
-                                else
-                                        speed_10 = 1;
-                        }
-
-                        if (speed_10 & half_duplex)
-                                ecmd->advertising |= ADVERTISED_10baseT_Half;
-                        if (speed_10 & full_duplex)
-                                ecmd->advertising |= ADVERTISED_10baseT_Full;
-                        if (speed_100 & half_duplex)
-                                ecmd->advertising |= ADVERTISED_100baseT_Half;
-                        if (speed_100 & full_duplex)
-                                ecmd->advertising |= ADVERTISED_100baseT_Full;
-                        if (speed_1000)
-                                ecmd->advertising |= ADVERTISED_1000baseT_Full;
-                }
-        }
-
-        ecmd->port = PORT_TP;
-        ecmd->phy_address = ethernet_phy_get(port_num);
-
-        ecmd->transceiver = XCVR_EXTERNAL;
-
-        if (netif_carrier_ok(netdev)) {
-                if (mode_10_bit)
-                        ecmd->speed = SPEED_10;
-                else {
-                        if (psr & MV643XX_ETH_PORT_STATUS_GMII_1000)
-                                ecmd->speed = SPEED_1000;
-                        else if (psr & MV643XX_ETH_PORT_STATUS_MII_100)
-                                ecmd->speed = SPEED_100;
-                        else
-                                ecmd->speed = SPEED_10;
-                }
-
-                if (psr & MV643XX_ETH_PORT_STATUS_FULL_DUPLEX)
-                        ecmd->duplex = DUPLEX_FULL;
-                else
-                        ecmd->duplex = DUPLEX_HALF;
-        } else {
-                ecmd->speed = -1;
-                ecmd->duplex = -1;
-        }
-
-        ecmd->autoneg = autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
-        return 0;
-}
-
 static void mv643xx_get_drvinfo(struct net_device *netdev,
                                 struct ethtool_drvinfo *drvinfo)
 {
@@ -3140,15 +3183,41 @@ static void mv643xx_get_strings(struct net_device *netdev, uint32_t stringset,
         }
 }
 
+static u32 mv643xx_eth_get_link(struct net_device *dev)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+
+        return mii_link_ok(&mp->mii);
+}
+
+static int mv643xx_eth_nway_restart(struct net_device *dev)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+
+        return mii_nway_restart(&mp->mii);
+}
+
+static int mv643xx_eth_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+        struct mv643xx_private *mp = netdev_priv(dev);
+
+        return generic_mii_ioctl(&mp->mii, if_mii(ifr), cmd, NULL);
+}
+
 static struct ethtool_ops mv643xx_ethtool_ops = {
         .get_settings           = mv643xx_get_settings,
+        .set_settings           = mv643xx_set_settings,
         .get_drvinfo            = mv643xx_get_drvinfo,
-        .get_link               = ethtool_op_get_link,
+        .get_link               = mv643xx_eth_get_link,
         .get_sg                 = ethtool_op_get_sg,
         .set_sg                 = ethtool_op_set_sg,
         .get_strings            = mv643xx_get_strings,
         .get_stats_count        = mv643xx_get_stats_count,
         .get_ethtool_stats      = mv643xx_get_ethtool_stats,
+        .get_strings            = mv643xx_get_strings,
+        .get_stats_count        = mv643xx_get_stats_count,
+        .get_ethtool_stats      = mv643xx_get_ethtool_stats,
+        .nway_reset             = mv643xx_eth_nway_restart,
 };
 
 /************* End ethtool support *************************/