nvethernet: add support for 802.1Qav (CBS)

Code added for
1) Customized application to configure avb setting
2) ndo_select_queue support to identify queue based on
   user priority.

Bug 200512771

Change-Id: I6468aa838567e50885931f10d49126870f1e25c4
Signed-off-by: Rakesh Goyal <rgoyal@nvidia.com>
Reviewed-on: https://git-master.nvidia.com/r/2108339
Reviewed-by: mobile promotions <svcmobile_promotions@nvidia.com>
Tested-by: mobile promotions <svcmobile_promotions@nvidia.com>

drivers/net/ethernet/nvidia/nvethernet/Makefile

@@ -20,6 +20,7 @@ nvethernet-objs:= ether_linux.o \
 		  osd.o \
 		  ethtool.o \
 		  sysfs.o \
+		  ioctl.o \
 		  $(OSI)/osi_core.o \
 		  $(OSI)/osi_common.o \
 		  $(OSI)/osi_dma.o \

drivers/net/ethernet/nvidia/nvethernet/ether_linux.c

@@ -1048,6 +1048,45 @@ static int ether_tx_swcx_alloc(struct device *dev,
 	return cnt;
 }
 
+/**
+ *	ether_select_queue - Select queue based on user priority
+ *	@dev: Network device pointer
+ *	@skb: sk_buff pointer, buffer data to send
+ *	@accel_priv: private data used for L2 forwarding offload
+ *	@fallback: fallback function pointer
+ *
+ *	Algorithm:
+ *	1) Select the correct queue index based which has priority of queue
+ *	same as skb-<priority
+ *	2) default select queue index 0
+ *
+ *	Dependencies: None.
+ *
+ *	Protection: None.
+ *
+ *	Return: tx queu index - success, -1 - failure.
+ */
+static unsigned short ether_select_queue(struct net_device *dev,
+					 struct sk_buff *skb,
+					 void *accel_priv,
+					 select_queue_fallback_t fallback)
+{
+	struct ether_priv_data *pdata = netdev_priv(dev);
+	struct osi_dma_priv_data *osi_dma = pdata->osi_dma;
+	unsigned short txqueue_select = 0;
+	unsigned int i, chan;
+
+	for (i = 0; i < OSI_EQOS_MAX_NUM_CHANS; i++) {
+		chan = osi_dma->dma_chans[i];
+		if (pdata->q_prio[chan] == skb->priority) {
+			txqueue_select = (unsigned short)chan;
+			break;
+		}
+	}
+
+	return txqueue_select;
+}
+
 /**
  *	ether_start_xmit - Network layer hook for data transmission.
  *	@skb: SKB data structure.
@@ -1095,6 +1134,7 @@ static int ether_start_xmit(struct sk_buff *skb, struct net_device *ndev)
  *
  *	Algorithm:
  *	1) Invokes MII API for phy read/write based on IOCTL command
+ *	2) SIOCDEVPRIVATE for private ioctl
  *
  *	Dependencies: Ethernet interface need to be up.
  *
@@ -1120,6 +1160,10 @@ static int ether_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 		ret = phy_mii_ioctl(dev->phydev, rq, cmd);
 		break;
 
+	case SIOCDEVPRIVATE:
+		ret = ether_handle_priv_ioctl(dev, rq);
+		break;
+
 	default:
 		break;
 	}
@@ -1202,6 +1246,7 @@ static const struct net_device_ops ether_netdev_ops = {
 	.ndo_do_ioctl = ether_ioctl,
 	.ndo_set_mac_address = ether_set_mac_addr,
 	.ndo_change_mtu = ether_change_mtu,
+	.ndo_select_queue = ether_select_queue,
 };
 
 /**
@@ -1921,6 +1966,56 @@ static int ether_parse_phy_dt(struct ether_priv_data *pdata,
 	return 0;
 }
 
+/**
+ *	ether_parse_queue_prio - Parse queue priority DT.
+ *	@pdata: OS dependent private data structure.
+ *	@pdt_prop: name of property
+ *	@pval: structure pointer where value will be filed
+ *	@val_def: default value if DT entry not reset
+ *	@num_entries: number of entries to be read form DT
+ *
+ *	Algorithm: Reads queue priority form DT. Updates
+ *	data either by DT values or by default value.
+ *
+ *	Dependencies: None
+ *
+ *	Protection: None
+ *
+ *	Return: void
+ */
+static void ether_parse_queue_prio(struct ether_priv_data *pdata,
+				   const char *pdt_prop,
+				   unsigned int *pval, unsigned int val_def,
+				   unsigned int val_max,
+				   unsigned int num_entries)
+{
+	struct device_node *pnode = pdata->dev->of_node;
+	unsigned int i, pmask = 0x0U;
+	int ret = 0;
+
+	ret = of_property_read_u32_array(pnode, pdt_prop, pval, num_entries);
+	if (ret < 0) {
+		dev_err(pdata->dev, "%s(): \"%s\" read failed %d. Using default\n",
+			__func__, pdt_prop, ret);
+		for (i = 0; i < num_entries; i++) {
+			pval[i] = val_def;
+		}
+
+		return;
+	}
+	/* If Some priority is alreay give to queue or priority in DT more than
+	 * MAX priority, assig default priority to queue with error message
+	 */
+	for (i = 0; i < num_entries; i++) {
+		if ((pval[i] > val_max) || ((pmask & (1U << pval[i])) != 0U)) {
+			dev_err(pdata->dev, "%s():Wrong or duplicate priority in DT entry for Q(%d)\n",
+				__func__, i);
+			pval[i] = val_def;
+		}
+		pmask |= 1U << pval[i];
+	}
+}
+
 /**
  *	ether_parse_dt - Parse MAC and PHY DT.
  *	@pdata: OS dependent private data structure.
@@ -1987,6 +2082,10 @@ static int ether_parse_dt(struct ether_priv_data *pdata)
 		return ret;
 	}
 
+	ether_parse_queue_prio(pdata, "nvidia,queue_prio", pdata->q_prio,
+			       ETHER_QUEUE_PRIO_DEFAULT, ETHER_QUEUE_PRIO_MAX,
+			       osi_core->num_mtl_queues);
+
 	ret = ether_parse_phy_dt(pdata, np);
 	if (ret < 0) {
 		dev_err(dev, "failed to parse PHY DT\n");
@@ -2021,7 +2120,8 @@ static void ether_get_num_dma_chan_mtl_q(struct platform_device *pdev,
 					 unsigned int *num_mtl_queues)
 {
 	struct device_node *np = pdev->dev.of_node;
-	unsigned int max_chans;
+	/* intializing with 1 channel */
+	unsigned int max_chans = 1;
 	int ret = 0;
 
 	ret = of_device_is_compatible(np, "nvidia,nveqos");

drivers/net/ethernet/nvidia/nvethernet/ether_linux.h

@@ -33,10 +33,13 @@
 
 #include <osi_core.h>
 #include <osi_dma.h>
+#include "ioctl.h"
 
-#define ETHER_MAX_IRQS		4
-#define ETHER_IRQ_MAX_IDX	8
-#define ETHER_IRQ_NAME_SZ	32
+#define ETHER_MAX_IRQS			4
+#define ETHER_IRQ_MAX_IDX		8
+#define ETHER_IRQ_NAME_SZ		32
+#define ETHER_QUEUE_PRIO_DEFAULT	0U
+#define ETHER_QUEUE_PRIO_MAX		7U
 
 /**
  *	struct ether_tx_napi - DMA Transmit Channel NAPI
@@ -94,7 +97,9 @@ struct ether_rx_napi {
  *	@common_irq:	Common IRQ number for MAC
  *	@tx_irqs:	Array of DMA Transmit channel IRQ numbers
  *	@rx_irqs:	Array of DMA Receive channel IRQ numbers
- *	dma_mask:	memory allocation mask
+ *	@dma_mask:	memory allocation mask
+ *	@mac_loopback_mode:	MAC loopback mode
+ *	@q_prio:	Array of MTL queue TX priority
  */
 struct ether_priv_data {
 	struct osi_core_priv_data *osi_core;
@@ -136,6 +141,7 @@ struct ether_priv_data {
 
 	/* for MAC loopback */
 	unsigned int mac_loopback_mode;
+	unsigned int q_prio[OSI_EQOS_MAX_NUM_CHANS];
 };
 
 void ether_set_ethtool_ops(struct net_device *ndev);

drivers/net/ethernet/nvidia/nvethernet/ioctl.c

@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2019, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ether_linux.h"
+
+/**
+ *	ether_set_avb_algo - function to handle private ioctl
+ *	EQOS_AVB_ALGORITHM
+ *	@ndev: network device structure
+ *	@ifdata: interface private data structure
+ *
+ *	Algorithm:
+ *	- Call osi_set_avb with user passed data
+ *
+ *	Dependencies: Ethernet interface need to be up.
+ *
+ *	Protection: None.
+ *
+ *	Return: 0 - success, negative value - failure.
+ */
+int ether_set_avb_algo(struct net_device *ndev,
+		       struct ether_ifr_data *ifdata)
+{
+	struct ether_priv_data *pdata = netdev_priv(ndev);
+	struct osi_core_priv_data *osi_core = pdata->osi_core;
+	struct osi_core_avb_algorithm l_avb_struct;
+	int ret = -1;
+
+	if (ifdata->ptr == NULL) {
+		dev_err(pdata->dev, "%s: Invalid data for priv ioctl %d\n",
+			__func__, ifdata->ifcmd);
+		return ret;
+	}
+
+	if (copy_from_user(&l_avb_struct,
+			   (struct osi_core_avb_algorithm *)ifdata->ptr,
+			   sizeof(struct osi_core_avb_algorithm)) != 0U) {
+		dev_err(pdata->dev,
+			"Failed to fetch AVB Struct info from user\n");
+		return ret;
+	}
+
+	return osi_set_avb(osi_core, &l_avb_struct);
+}
+
+/**
+ *	ether_priv_ioctl - Handle private IOCTLs
+ *	@ndev: network device structure
+ *	@ifr: Interface request structure used for socket ioctl's.
+ *
+ *	Algorithm:
+ *	1) Copy the priv command data from user space.
+ *	2) Check the priv command cmd and invoke handler func.
+ *	if it is supported.
+ *	3) Copy result back to user space.
+ *
+ *	Dependencies: Interface should be running (enforced by caller).
+ *
+ *	Protection: None.
+ *
+ *	Return: 0 - success, negative value - failure.
+ */
+int ether_handle_priv_ioctl(struct net_device *ndev,
+			    struct ifreq *ifr)
+{
+	struct ether_priv_data *pdata = netdev_priv(ndev);
+	struct ether_ifr_data ifdata;
+	int ret = -EOPNOTSUPP;
+
+	if (copy_from_user(&ifdata, ifr->ifr_data, sizeof(ifdata)) != 0U) {
+		dev_err(pdata->dev, "%s(): copy_from_user failed %d\n"
+			, __func__, __LINE__);
+		return -EFAULT;
+	}
+
+	switch (ifdata.ifcmd) {
+	case ETHER_AVB_ALGORITHM:
+		ret = ether_set_avb_algo(ndev, &ifdata);
+		break;
+	default:
+		break;
+	}
+
+	ifdata.command_error = ret;
+	if (copy_to_user(ifr->ifr_data, &ifdata, sizeof(ifdata)) != 0U) {
+		dev_err(pdata->dev, "%s: copy_to_user failed\n", __func__);
+		return -EFAULT;
+	}
+
+	return ret;
+}

drivers/net/ethernet/nvidia/nvethernet/ioctl.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2018-2019, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IOCTL_H
+#define IOCTL_H
+
+/* Remote wakeup filter */
+#define EQOS_RWK_FILTER_LENGTH	8
+/* private ioctl number*/
+#define ETHER_AVB_ALGORITHM	27
+
+/**
+ *	struct ether_ifr_data - Private data of struct ifreq
+ *	@flags: Flags used for specific ioctl - like enable/disable
+ *	@qinx: Queue index to be used for certain ioctls.
+ *		Not in use, keep for app compatibility.
+ *		Some applications already use this same struct
+ *	@cmd: The private ioctl command number.
+ *	@context_setup: Used to indicate if context descriptor needs
+ *		to be setup to handle ioctl.
+ *		Not in use, keep for app compatibility.
+ *	@connected_speed: Used to query the connected link speed.
+ *		Not in use, keep for app compatibility.
+ *	@rwk_filter_values: Used to set Remote wakeup filters.
+ *		Not in use, keep for app compatibility.
+ *	@rwk_filter_length: Number of remote wakeup filters to use.
+ *		Not in use, keep for app compatibility.
+ *	@command_error: The return value of IOCTL handler func.
+ *		This is passed back to user space application.
+ *	@test_done: Not in use, keep for app compatibility.
+ *	@ptr: IOCTL cmd specific structure pointer.
+ */
+struct ether_ifr_data {
+	unsigned int if_flags;
+	unsigned int qinx;
+	unsigned int ifcmd;
+	unsigned int context_setup;
+	unsigned int connected_speed;
+	unsigned int rwk_filter_values[EQOS_RWK_FILTER_LENGTH];
+	unsigned int rwk_filter_length;
+	int command_error;
+	int test_done;
+	void *ptr;
+};
+
+int ether_set_avb_algo(struct net_device *ndev,
+		       struct ether_ifr_data *ifdata);
+/* Private ioctl handler function */
+int ether_handle_priv_ioctl(struct net_device *ndev,
+			    struct ifreq *ifr);
+#endif
+