2 files changed, 82 insertions, 3 deletions

diff --git a/include/linux/ethtool.h b/include/linux/ethtool.h
index ead7dcb1bf1e..c04d1316d221 100644
--- a/include/linux/ethtool.h
+++ b/include/linux/ethtool.h
@@ -663,6 +663,22 @@ struct ethtool_rx_ntuple_list {
         unsigned int            count;
 };
 
+/**
+ * enum ethtool_phys_id_state - indicator state for physical identification
+ * @ETHTOOL_ID_INACTIVE: Physical ID indicator should be deactivated
+ * @ETHTOOL_ID_ACTIVE: Physical ID indicator should be activated
+ * @ETHTOOL_ID_ON: LED should be turned on (used iff %ETHTOOL_ID_ACTIVE
+ *      is not supported)
+ * @ETHTOOL_ID_OFF: LED should be turned off (used iff %ETHTOOL_ID_ACTIVE
+ *      is not supported)
+ */
+enum ethtool_phys_id_state {
+        ETHTOOL_ID_INACTIVE,
+        ETHTOOL_ID_ACTIVE,
+        ETHTOOL_ID_ON,
+        ETHTOOL_ID_OFF
+};
+
 struct net_device;
 
 /* Some generic methods drivers may use in their ethtool_ops */
@@ -741,7 +757,18 @@ bool ethtool_invalid_flags(struct net_device *dev, u32 data, u32 supported);
  *      segmentation offload on or off.  Returns a negative error code or zero.
  * @self_test: Run specified self-tests
  * @get_strings: Return a set of strings that describe the requested objects
- * @phys_id: Identify the physical device, e.g. by flashing an LED
+ * @set_phys_id: Identify the physical devices, e.g. by flashing an LED
+ *      attached to it.  The implementation may update the indicator
+ *      asynchronously or synchronously, but in either case it must return
+ *      quickly.  It is initially called with the argument %ETHTOOL_ID_ACTIVE,
+ *      and must either activate asynchronous updates or return -%EINVAL.
+ *      If it returns -%EINVAL then it will be called again at intervals with
+ *      argument %ETHTOOL_ID_ON or %ETHTOOL_ID_OFF and should set the state of
+ *      the indicator accordingly.  Finally, it is called with the argument
+ *      %ETHTOOL_ID_INACTIVE and must deactivate the indicator.  Returns a
+ *      negative error code or zero.
+ * @phys_id: Deprecated in favour of @set_phys_id.
+ *      Identify the physical device, e.g. by flashing an LED
  *      attached to it until interrupted by a signal or the given time
  *      (in seconds) elapses.  If the given time is zero, use a default
  *      time limit.  Returns a negative error code or zero.  Being
@@ -830,6 +857,7 @@ struct ethtool_ops {
         int     (*set_tso)(struct net_device *, u32);
         void    (*self_test)(struct net_device *, struct ethtool_test *, u64 *);
         void    (*get_strings)(struct net_device *, u32 stringset, u8 *);
+        int     (*set_phys_id)(struct net_device *, enum ethtool_phys_id_state);
         int     (*phys_id)(struct net_device *, u32);
         void    (*get_ethtool_stats)(struct net_device *,
                                      struct ethtool_stats *, u64 *);
diff --git a/net/core/ethtool.c b/net/core/ethtool.c
index 74ead9eca126..1c95f0fb8b3a 100644
--- a/net/core/ethtool.c
+++ b/net/core/ethtool.c
@@ -21,6 +21,8 @@
 #include <linux/uaccess.h>
 #include <linux/vmalloc.h>
 #include <linux/slab.h>
+#include <linux/rtnetlink.h>
+#include <linux/sched.h>
 
 /*
  * Some useful ethtool_ops methods that're device independent.
@@ -1618,14 +1620,63 @@ out:
 static int ethtool_phys_id(struct net_device *dev, void __user *useraddr)
 {
         struct ethtool_value id;
+        static bool busy;
+        int rc;
 
-        if (!dev->ethtool_ops->phys_id)
+        if (!dev->ethtool_ops->set_phys_id && !dev->ethtool_ops->phys_id)
                 return -EOPNOTSUPP;
 
+        if (busy)
+                return -EBUSY;
+
         if (copy_from_user(&id, useraddr, sizeof(id)))
                 return -EFAULT;
 
-        return dev->ethtool_ops->phys_id(dev, id.data);
+        if (!dev->ethtool_ops->set_phys_id)
+                /* Do it the old way */
+                return dev->ethtool_ops->phys_id(dev, id.data);
+
+        rc = dev->ethtool_ops->set_phys_id(dev, ETHTOOL_ID_ACTIVE);
+        if (rc && rc != -EINVAL)
+                return rc;
+
+        /* Drop the RTNL lock while waiting, but prevent reentry or
+         * removal of the device.
+         */
+        busy = true;
+        dev_hold(dev);
+        rtnl_unlock();
+
+        if (rc == 0) {
+                /* Driver will handle this itself */
+                schedule_timeout_interruptible(
+                        id.data ? id.data : MAX_SCHEDULE_TIMEOUT);
+        } else {
+                /* Driver expects to be called periodically */
+                do {
+                        rtnl_lock();
+                        rc = dev->ethtool_ops->set_phys_id(dev, ETHTOOL_ID_ON);
+                        rtnl_unlock();
+                        if (rc)
+                                break;
+                        schedule_timeout_interruptible(HZ / 2);
+
+                        rtnl_lock();
+                        rc = dev->ethtool_ops->set_phys_id(dev, ETHTOOL_ID_OFF);
+                        rtnl_unlock();
+                        if (rc)
+                                break;
+                        schedule_timeout_interruptible(HZ / 2);
+                } while (!signal_pending(current) &&
+                         (id.data == 0 || --id.data != 0));
+        }
+
+        rtnl_lock();
+        dev_put(dev);
+        busy = false;
+
+        (void)dev->ethtool_ops->set_phys_id(dev, ETHTOOL_ID_INACTIVE);
+        return rc;
 }
 
 static int ethtool_get_stats(struct net_device *dev, void __user *useraddr)