net/ethtool: support set coalesce per queue

[mirror_ubuntu-zesty-kernel.git] / net / core / ethtool.c

diff --git a/net/core/ethtool.c b/net/core/ethtool.c
index 2a6c3a26f63f7e10cffa3fe4cb51dda33215caa8..2406101002b1bd00f57abdf013d55a4ce11d07d7 100644 (file)
--- a/net/core/ethtool.c
+++ b/net/core/ethtool.c
@@ -1920,6 +1920,65 @@ static int ethtool_get_per_queue_coalesce(struct net_device *dev,
        return 0;
 }
 
+static int ethtool_set_per_queue_coalesce(struct net_device *dev,
+                                         void __user *useraddr,
+                                         struct ethtool_per_queue_op *per_queue_opt)
+{
+       u32 bit;
+       int i, ret = 0;
+       int n_queue;
+       struct ethtool_coalesce *backup = NULL, *tmp = NULL;
+       DECLARE_BITMAP(queue_mask, MAX_NUM_QUEUE);
+
+       if ((!dev->ethtool_ops->set_per_queue_coalesce) ||
+           (!dev->ethtool_ops->get_per_queue_coalesce))
+               return -EOPNOTSUPP;
+
+       useraddr += sizeof(*per_queue_opt);
+
+       bitmap_from_u32array(queue_mask,
+                            MAX_NUM_QUEUE,
+                            per_queue_opt->queue_mask,
+                            DIV_ROUND_UP(MAX_NUM_QUEUE, 32));
+       n_queue = bitmap_weight(queue_mask, MAX_NUM_QUEUE);
+       tmp = backup = kmalloc_array(n_queue, sizeof(*backup), GFP_KERNEL);
+       if (!backup)
+               return -ENOMEM;
+
+       for_each_set_bit(bit, queue_mask, MAX_NUM_QUEUE) {
+               struct ethtool_coalesce coalesce;
+
+               ret = dev->ethtool_ops->get_per_queue_coalesce(dev, bit, tmp);
+               if (ret != 0)
+                       goto roll_back;
+
+               tmp++;
+
+               if (copy_from_user(&coalesce, useraddr, sizeof(coalesce))) {
+                       ret = -EFAULT;
+                       goto roll_back;
+               }
+
+               ret = dev->ethtool_ops->set_per_queue_coalesce(dev, bit, &coalesce);
+               if (ret != 0)
+                       goto roll_back;
+
+               useraddr += sizeof(coalesce);
+       }
+
+roll_back:
+       if (ret != 0) {
+               tmp = backup;
+               for_each_set_bit(i, queue_mask, bit) {
+                       dev->ethtool_ops->set_per_queue_coalesce(dev, i, tmp);
+                       tmp++;
+               }
+       }
+       kfree(backup);
+
+       return ret;
+}
+
 static int ethtool_set_per_queue(struct net_device *dev, void __user *useraddr)
 {
        struct ethtool_per_queue_op per_queue_opt;
@@ -1930,6 +1989,8 @@ static int ethtool_set_per_queue(struct net_device *dev, void __user *useraddr)
        switch (per_queue_opt.sub_command) {
        case ETHTOOL_GCOALESCE:
                return ethtool_get_per_queue_coalesce(dev, useraddr, &per_queue_opt);
+       case ETHTOOL_SCOALESCE:
+               return ethtool_set_per_queue_coalesce(dev, useraddr, &per_queue_opt);
        default:
                return -EOPNOTSUPP;
        };