1 files changed, 103 insertions, 0 deletions

diff --git a/drivers/net/dsa/microchip/ksz_common.c b/drivers/net/dsa/microchip/ksz_common.c
index 6f728429eec6..eecfbf30cdc2 100644
--- a/drivers/net/dsa/microchip/ksz_common.c
+++ b/drivers/net/dsa/microchip/ksz_common.c
@@ -40,6 +40,85 @@ void ksz_update_port_member(struct ksz_device *dev, int port)
 }
 EXPORT_SYMBOL_GPL(ksz_update_port_member);
 
+static void port_r_cnt(struct ksz_device *dev, int port)
+{
+        struct ksz_port_mib *mib = &dev->ports[port].mib;
+        u64 *dropped;
+
+        /* Some ports may not have MIB counters before SWITCH_COUNTER_NUM. */
+        while (mib->cnt_ptr < dev->reg_mib_cnt) {
+                dev->dev_ops->r_mib_cnt(dev, port, mib->cnt_ptr,
+                                        &mib->counters[mib->cnt_ptr]);
+                ++mib->cnt_ptr;
+        }
+
+        /* last one in storage */
+        dropped = &mib->counters[dev->mib_cnt];
+
+        /* Some ports may not have MIB counters after SWITCH_COUNTER_NUM. */
+        while (mib->cnt_ptr < dev->mib_cnt) {
+                dev->dev_ops->r_mib_pkt(dev, port, mib->cnt_ptr,
+                                        dropped, &mib->counters[mib->cnt_ptr]);
+                ++mib->cnt_ptr;
+        }
+        mib->cnt_ptr = 0;
+}
+
+static void ksz_mib_read_work(struct work_struct *work)
+{
+        struct ksz_device *dev = container_of(work, struct ksz_device,
+                                              mib_read);
+        struct ksz_port_mib *mib;
+        struct ksz_port *p;
+        int i;
+
+        for (i = 0; i < dev->mib_port_cnt; i++) {
+                p = &dev->ports[i];
+                mib = &p->mib;
+                mutex_lock(&mib->cnt_mutex);
+
+                /* Only read MIB counters when the port is told to do.
+                 * If not, read only dropped counters when link is not up.
+                 */
+                if (!p->read) {
+                        const struct dsa_port *dp = dsa_to_port(dev->ds, i);
+
+                        if (!netif_carrier_ok(dp->slave))
+                                mib->cnt_ptr = dev->reg_mib_cnt;
+                }
+                port_r_cnt(dev, i);
+                p->read = false;
+                mutex_unlock(&mib->cnt_mutex);
+        }
+}
+
+static void mib_monitor(struct timer_list *t)
+{
+        struct ksz_device *dev = from_timer(dev, t, mib_read_timer);
+
+        mod_timer(&dev->mib_read_timer, jiffies + dev->mib_read_interval);
+        schedule_work(&dev->mib_read);
+}
+
+void ksz_init_mib_timer(struct ksz_device *dev)
+{
+        int i;
+
+        /* Read MIB counters every 30 seconds to avoid overflow. */
+        dev->mib_read_interval = msecs_to_jiffies(30000);
+
+        INIT_WORK(&dev->mib_read, ksz_mib_read_work);
+        timer_setup(&dev->mib_read_timer, mib_monitor, 0);
+
+        for (i = 0; i < dev->mib_port_cnt; i++)
+                dev->dev_ops->port_init_cnt(dev, i);
+
+        /* Start the timer 2 seconds later. */
+        dev->mib_read_timer.expires = jiffies + msecs_to_jiffies(2000);
+        add_timer(&dev->mib_read_timer);
+}
+EXPORT_SYMBOL_GPL(ksz_init_mib_timer);
+
 int ksz_phy_read16(struct dsa_switch *ds, int addr, int reg)
 {
         struct ksz_device *dev = ds->priv;
@@ -72,6 +151,24 @@ int ksz_sset_count(struct dsa_switch *ds, int port, int sset)
 }
 EXPORT_SYMBOL_GPL(ksz_sset_count);
 
+void ksz_get_ethtool_stats(struct dsa_switch *ds, int port, uint64_t *buf)
+{
+        const struct dsa_port *dp = dsa_to_port(ds, port);
+        struct ksz_device *dev = ds->priv;
+        struct ksz_port_mib *mib;
+
+        mib = &dev->ports[port].mib;
+        mutex_lock(&mib->cnt_mutex);
+
+        /* Only read dropped counters if no link. */
+        if (!netif_carrier_ok(dp->slave))
+                mib->cnt_ptr = dev->reg_mib_cnt;
+        port_r_cnt(dev, port);
+        memcpy(buf, mib->counters, dev->mib_cnt * sizeof(u64));
+        mutex_unlock(&mib->cnt_mutex);
+}
+EXPORT_SYMBOL_GPL(ksz_get_ethtool_stats);
+
 int ksz_port_bridge_join(struct dsa_switch *ds, int port,
                          struct net_device *br)
 {
@@ -339,6 +436,12 @@ EXPORT_SYMBOL(ksz_switch_register);
 
 void ksz_switch_remove(struct ksz_device *dev)
 {
+        /* timer started */
+        if (dev->mib_read_timer.expires) {
+                del_timer_sync(&dev->mib_read_timer);
+                flush_work(&dev->mib_read);
+        }
+
         dev->dev_ops->exit(dev);
         dsa_unregister_switch(dev->ds);