[PATCH v2 4/4] net: ntb_netdev: Support ethtool channels for multi-queue

Support dynamic queue pair addition/removal via ethtool channels.
Use the combined channel count to control the number of netdev TX/RX
queues, each corresponding to a ntb_transport queue pair.

When the number of queues is reduced, tear down and free the removed
ntb_transport queue pairs (not just deactivate them) so other
ntb_transport clients can reuse the freed resources.

When the number of queues is increased, create additional queue pairs up
to NTB_NETDEV_MAX_QUEUES (=64). The effective limit is determined by the
underlying ntb_transport implementation and NTB hardware resources (the
number of MWs), so set_channels may return -ENOSPC if no more QPs can be
allocated.

Keep the default at one queue pair to preserve the previous behavior.

Signed-off-by: Koichiro Den <[email protected]>
---
 drivers/net/ntb_netdev.c | 147 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 147 insertions(+)

diff --git a/drivers/net/ntb_netdev.c b/drivers/net/ntb_netdev.c
index 6aa59316569c..c2b1886775bf 100644
--- a/drivers/net/ntb_netdev.c
+++ b/drivers/net/ntb_netdev.c
@@ -113,6 +113,24 @@ static void ntb_netdev_update_carrier(struct ntb_netdev *dev)
 		netif_carrier_off(ndev);
 }
 
+static void ntb_netdev_sync_subqueues(struct ntb_netdev *dev)
+{
+	struct net_device *ndev = dev->ndev;
+	unsigned int q;
+
+	if (!netif_running(ndev))
+		return;
+
+	for (q = 0; q < dev->num_queues; q++) {
+		struct ntb_netdev_queue *queue = &dev->queues[q];
+
+		if (ntb_transport_link_query(queue->qp))
+			netif_wake_subqueue(ndev, queue->qid);
+		else
+			netif_stop_subqueue(ndev, queue->qid);
+	}
+}
+
 static void ntb_netdev_queue_rx_drain(struct ntb_netdev_queue *queue)
 {
 	struct sk_buff *skb;
@@ -464,6 +482,8 @@ static const struct net_device_ops ntb_netdev_ops = {
 	.ndo_set_mac_address = eth_mac_addr,
 };
 
+static const struct ntb_queue_handlers ntb_netdev_handlers;
+
 static void ntb_get_drvinfo(struct net_device *ndev,
 			    struct ethtool_drvinfo *info)
 {
@@ -491,10 +511,137 @@ static int ntb_get_link_ksettings(struct net_device *dev,
 	return 0;
 }
 
+static void ntb_get_channels(struct net_device *ndev,
+			     struct ethtool_channels *channels)
+{
+	struct ntb_netdev *dev = netdev_priv(ndev);
+
+	channels->combined_count = dev->num_queues;
+	channels->max_combined = ndev->num_tx_queues;
+}
+
+static int ntb_set_channels(struct net_device *ndev,
+			    struct ethtool_channels *channels)
+{
+	struct ntb_netdev *dev = netdev_priv(ndev);
+	unsigned int new = channels->combined_count;
+	unsigned int old = dev->num_queues;
+	bool running = netif_running(ndev);
+	struct ntb_netdev_queue *queue;
+	unsigned int q, created;
+	int rc = 0;
+
+	if (channels->rx_count || channels->tx_count || channels->other_count)
+		return -EINVAL;
+
+	if (!new || new > ndev->num_tx_queues)
+		return -ERANGE;
+
+	if (new == old)
+		return 0;
+
+	if (new < old) {
+		if (running)
+			for (q = new; q < old; q++)
+				netif_stop_subqueue(ndev, q);
+
+		rc = netif_set_real_num_queues(ndev, new, new);
+		if (rc)
+			goto out_restore;
+
+		/* Publish new queue count before invalidating QP pointers */
+		dev->num_queues = new;
+
+		for (q = new; q < old; q++) {
+			queue = &dev->queues[q];
+
+			if (running) {
+				ntb_transport_link_down(queue->qp);
+				ntb_netdev_queue_rx_drain(queue);
+				timer_delete_sync(&queue->tx_timer);
+			}
+
+			ntb_transport_free_queue(queue->qp);
+			queue->qp = NULL;
+		}
+
+		goto out_restore;
+	}
+
+	created = old;
+	for (q = old; q < new; q++) {
+		queue = &dev->queues[q];
+
+		queue->ntdev = dev;
+		queue->qid = q;
+		queue->qp = ntb_transport_create_queue(queue, dev->client_dev,
+						       &ntb_netdev_handlers);
+		if (!queue->qp) {
+			rc = -ENOSPC;
+			goto err_new;
+		}
+		created++;
+
+		if (!running)
+			continue;
+
+		timer_setup(&queue->tx_timer, ntb_netdev_tx_timer, 0);
+
+		rc = ntb_netdev_queue_rx_fill(ndev, queue);
+		if (rc)
+			goto err_new;
+
+		/*
+		 * Carrier may already be on due to other QPs. Keep the new
+		 * subqueue stopped until we get a Link Up event for this QP.
+		 */
+		netif_stop_subqueue(ndev, q);
+	}
+
+	rc = netif_set_real_num_queues(ndev, new, new);
+	if (rc)
+		goto err_new;
+
+	dev->num_queues = new;
+
+	if (running)
+		for (q = old; q < new; q++)
+			ntb_transport_link_up(dev->queues[q].qp);
+
+	return 0;
+
+err_new:
+	if (running) {
+		unsigned int rollback = created;
+
+		while (rollback-- > old) {
+			queue = &dev->queues[rollback];
+			ntb_transport_link_down(queue->qp);
+			ntb_netdev_queue_rx_drain(queue);
+			timer_delete_sync(&queue->tx_timer);
+		}
+	}
+
+	while (created-- > old) {
+		queue = &dev->queues[created];
+		ntb_transport_free_queue(queue->qp);
+		queue->qp = NULL;
+	}
+
+out_restore:
+	if (running) {
+		ntb_netdev_sync_subqueues(dev);
+		ntb_netdev_update_carrier(dev);
+	}
+	return rc;
+}
+
 static const struct ethtool_ops ntb_ethtool_ops = {
 	.get_drvinfo = ntb_get_drvinfo,
 	.get_link = ethtool_op_get_link,
 	.get_link_ksettings = ntb_get_link_ksettings,
+	.get_channels = ntb_get_channels,
+	.set_channels = ntb_set_channels,
 };
 
 static const struct ntb_queue_handlers ntb_netdev_handlers = {
-- 
2.51.0