Openwrt/target/linux/layerscape/patches-5.4/701-net-0401-fsl_qbman-Framework-for-enabling-Link-status-notific.patch

From 136d46d2fa27815cc4cc3a57d5e3d54523028768 Mon Sep 17 00:00:00 2001
From: Sachin Saxena <sachin.saxena@nxp.com>
Date: Thu, 19 Dec 2019 12:57:35 +0530
Subject: [PATCH] fsl_qbman: Framework for enabling Link status notification

 -  This will enable link update event notification for
    user space USDPAA application.

Signed-off-by: Sachin Saxena <sachin.saxena@nxp.com>
DPDK-2128
(cherry picked from commit fb53c813a779cc3fc28c8ed3fc8bc0dde24db0ea)
---
 drivers/staging/fsl_qbman/Makefile     |   4 +
 drivers/staging/fsl_qbman/fsl_usdpaa.c | 278 ++++++++++++++++++++++++++++++++-
 include/linux/fsl_usdpaa.h             |  32 ++++
 3 files changed, 313 insertions(+), 1 deletion(-)

--- a/drivers/staging/fsl_qbman/Makefile
+++ b/drivers/staging/fsl_qbman/Makefile
@@ -1,5 +1,9 @@
 subdir-ccflags-y := -Werror
 
+# Include netcomm SW specific definitions
+include $(srctree)/drivers/net/ethernet/freescale/sdk_fman/ncsw_config.mk
+ccflags-y += -I$(NET_DPA)
+
 # Common
 obj-$(CONFIG_FSL_SDK_DPA)		+= dpa_alloc.o
 obj-$(CONFIG_FSL_SDK_DPA)	+= qbman_driver.o
--- a/drivers/staging/fsl_qbman/fsl_usdpaa.c
+++ b/drivers/staging/fsl_qbman/fsl_usdpaa.c
@@ -18,6 +18,8 @@
 #include <linux/slab.h>
 #include <linux/mman.h>
 #include <linux/of_reserved_mem.h>
+#include <linux/eventfd.h>
+#include <linux/fdtable.h>
 
 #if !(defined(CONFIG_ARM) || defined(CONFIG_ARM64))
 #include <mm/mmu_decl.h>
@@ -27,6 +29,26 @@
 #include <linux/fsl_usdpaa.h>
 #include "bman_low.h"
 #include "qman_low.h"
+/* Headers requires for
+ * Link status support
+ */
+#include <linux/device.h>
+#include <linux/of_mdio.h>
+#include "mac.h"
+#include "dpaa_eth_common.h"
+
+/* Private data for Proxy Interface */
+struct dpa_proxy_priv_s {
+	struct mac_device	*mac_dev;
+	struct eventfd_ctx	*efd_ctx;
+};
+/* Interface Helpers */
+static inline struct device *get_dev_ptr(char *if_name);
+static void phy_link_updates(struct net_device *net_dev);
+/* IOCTL handlers */
+static inline int ioctl_usdpaa_get_link_status(char *if_name);
+static int ioctl_en_if_link_status(struct usdpaa_ioctl_link_status *args);
+static int ioctl_disable_if_link_status(char *if_name);
 
 /* Physical address range of the memory reservation, exported for mm/mem.c */
 static u64 phys_start;
@@ -556,7 +578,6 @@ static bool check_portal_channel(void *c
 
 
 
-
 static int usdpaa_release(struct inode *inode, struct file *filp)
 {
 	int err = 0;
@@ -1656,6 +1677,220 @@ found:
 	return 0;
 }
 
+
+static inline struct device *get_dev_ptr(char *if_name)
+{
+	struct device *dev;
+	char node[NODE_NAME_LEN];
+
+	sprintf(node, "soc:fsl,dpaa:%s",if_name);
+	dev = bus_find_device_by_name(&platform_bus_type, NULL, node);
+	if (dev == NULL) {
+		pr_err(KBUILD_MODNAME "IF %s not found\n", if_name);
+		return NULL;
+	}
+	pr_debug("%s: found dev 0x%lX  for If %s ,dev->platform_data %p\n",
+			  __func__, (unsigned long)dev,
+			  if_name, dev->platform_data);
+
+	return dev;
+}
+
+/* This function will return Current link status of the device
+ * '1' if Link is UP, '0' otherwise.
+ *
+ * Input parameter:
+ * if_name: Interface node name
+ *
+ */
+static inline int ioctl_usdpaa_get_link_status(char *if_name)
+{
+	struct net_device *net_dev = NULL;
+	struct device *dev;
+
+	dev = get_dev_ptr(if_name);
+	if (dev == NULL)
+		return -ENODEV;
+	net_dev = dev->platform_data;
+	if (net_dev == NULL)
+		return -ENODEV;
+
+	if (test_bit(__LINK_STATE_NOCARRIER, &net_dev->state))
+		return 0; /* Link is DOWN */
+	else
+		return 1; /* Link is UP */
+}
+
+
+/* Link Status Callback Function
+ * This function will be resgitered to PHY framework to get
+ * Link update notifications and should be responsible for waking up
+ * user space task when there is a link update notification.
+ */
+static void phy_link_updates(struct net_device *net_dev)
+{
+	struct dpa_proxy_priv_s *priv = NULL;
+
+	pr_debug("%s: Link '%s': Speed '%d-Mbps': Autoneg '%d': Duplex '%d'\n",
+		net_dev->name,
+		ioctl_usdpaa_get_link_status(net_dev->name)?"UP":"DOWN",
+		net_dev->phydev->speed,
+		net_dev->phydev->autoneg,
+		net_dev->phydev->duplex);
+
+	/* Wake up the user space context to notify PHY update */
+	priv = netdev_priv(net_dev);
+	eventfd_signal(priv->efd_ctx, 1);
+}
+
+
+/* IOCTL handler for enabling Link status request for a given interface
+ * Input parameters:
+ * args->if_name:	This the network interface node name as defind in
+ *			device tree file. Currently, it has format of
+ *			"ethernet@x" type for each interface.
+ * args->efd:		The eventfd value which should be waked up when
+ *			there is any link update received.
+ */
+static int ioctl_en_if_link_status(struct usdpaa_ioctl_link_status *args)
+{
+	struct net_device *net_dev = NULL;
+	struct dpa_proxy_priv_s *priv = NULL;
+	struct device *dev;
+	struct mac_device *mac_dev;
+	struct proxy_device *proxy_dev;
+	struct task_struct *userspace_task = NULL;
+	struct file *efd_file = NULL;
+
+	dev = get_dev_ptr(args->if_name);
+	if (dev == NULL)
+		return -ENODEV;
+	/* Utilize dev->platform_data to save netdevice
+	   pointer as it will not be registered */
+	if (dev->platform_data) {
+		pr_debug("%s: IF %s already initialized\n",
+			__func__, args->if_name);
+		/* This will happen when application is not able to initiate
+		 * cleanup in last run. We still need to save the new
+		 * eventfd context.
+		 */
+		net_dev = dev->platform_data;
+		priv = netdev_priv(net_dev);
+
+		/* Get current task context from which IOCTL was called */
+		userspace_task = current;
+
+		rcu_read_lock();
+		efd_file = fcheck_files(userspace_task->files, args->efd);
+		rcu_read_unlock();
+
+		priv->efd_ctx = eventfd_ctx_fileget(efd_file);
+		if (!priv->efd_ctx) {
+			pr_err(KBUILD_MODNAME "get eventfd context failed\n");
+			/* Free the allocated memory for net device */
+			dev->platform_data = NULL;
+			free_netdev(net_dev);
+			return -EINVAL;
+		}
+		/* Since there will be NO PHY update as link is already setup,
+		 * wake user context once so that current PHY status can
+		 * be fetched.
+		 */
+		phy_link_updates(net_dev);
+		return 0;
+	}
+
+	proxy_dev = dev_get_drvdata(dev);
+	mac_dev =  proxy_dev->mac_dev;
+	/* Allocate an dummy net device for proxy interface */
+	net_dev = alloc_etherdev(sizeof(*priv));
+	if (!net_dev) {
+		pr_err(KBUILD_MODNAME "alloc_etherdev failed\n");
+		return -ENOMEM;
+	} else {
+		SET_NETDEV_DEV(net_dev, dev);
+		priv = netdev_priv(net_dev);
+		priv->mac_dev = mac_dev;
+		/* Get current task context from which IOCTL was called */
+		userspace_task = current;
+
+		rcu_read_lock();
+		efd_file = fcheck_files(userspace_task->files, args->efd);
+		rcu_read_unlock();
+
+		priv->efd_ctx = eventfd_ctx_fileget(efd_file);
+
+		if (!priv->efd_ctx) {
+			pr_err(KBUILD_MODNAME "get eventfd context failed\n");
+			/* Free the allocated memory for net device */
+			free_netdev(net_dev);
+			return -EINVAL;
+		}
+		strncpy(net_dev->name, args->if_name, IF_NAME_MAX_LEN);
+		dev->platform_data = net_dev;
+	}
+
+	pr_debug("%s: mac_dev %p cell_index %d\n",
+		__func__, mac_dev, mac_dev->cell_index);
+	mac_dev->phy_dev = of_phy_connect(net_dev, mac_dev->phy_node,
+	                         phy_link_updates, 0, mac_dev->phy_if);
+	if (unlikely(mac_dev->phy_dev == NULL) || IS_ERR(mac_dev->phy_dev)) {
+		pr_err("%s: --------Error in PHY Connect\n", __func__);
+		/* Free the allocated memory for net device */
+		free_netdev(net_dev);
+		return -ENODEV;
+	}
+	net_dev->phydev = mac_dev->phy_dev;
+	mac_dev->start(mac_dev);
+	pr_debug("%s: --- PHY connected for %s\n", __func__, args->if_name);
+
+	return 0;
+}
+
+/* IOCTL handler for disabling Link status for a given interface
+ * Input parameters:
+ * if_name:	This the network interface node name as defind in
+ *		device tree file. Currently, it has format of
+ *		"ethernet@x" type for each interface.
+ */
+static int ioctl_disable_if_link_status(char *if_name)
+{
+	struct net_device *net_dev = NULL;
+	struct device *dev;
+	struct mac_device *mac_dev;
+	struct proxy_device *proxy_dev;
+	struct dpa_proxy_priv_s *priv = NULL;
+
+	dev = get_dev_ptr(if_name);
+	if (dev == NULL)
+		return -ENODEV;
+	/* Utilize dev->platform_data to save netdevice
+	   pointer as it will not be registered */
+	if (!dev->platform_data) {
+		pr_debug("%s: IF %s already Disabled for Link status\n",
+			__func__, if_name);
+		return 0;
+	}
+
+	net_dev = dev->platform_data;
+	proxy_dev = dev_get_drvdata(dev);
+	mac_dev =  proxy_dev->mac_dev;
+	mac_dev->stop(mac_dev);
+
+	priv = netdev_priv(net_dev);
+	eventfd_ctx_put(priv->efd_ctx);
+
+	/* This will also deregister the call back */
+	phy_disconnect(mac_dev->phy_dev);
+	phy_resume(mac_dev->phy_dev);
+
+	free_netdev(net_dev);
+	dev->platform_data = NULL;
+
+	pr_debug("%s: Link status Disabled for %s\n", __func__, if_name);
+	return 0;
+}
+
 static long usdpaa_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
 {
 	struct ctx *ctx = fp->private_data;
@@ -1722,6 +1957,47 @@ static long usdpaa_ioctl(struct file *fp
 			return -EFAULT;
 		return ioctl_free_raw_portal(fp, ctx, &input);
 	}
+	case USDPAA_IOCTL_ENABLE_LINK_STATUS_INTERRUPT:
+	{
+		struct usdpaa_ioctl_link_status input;
+		int ret;
+
+		if (copy_from_user(&input, a, sizeof(input)))
+			return -EFAULT;
+		ret = ioctl_en_if_link_status(&input);
+		if (ret)
+			pr_err("Error(%d) enable link interrupt:IF: %s\n",
+				ret, input.if_name);
+		return ret;
+	}
+	case USDPAA_IOCTL_DISABLE_LINK_STATUS_INTERRUPT:
+	{
+		char *input;
+		int ret;
+
+		if (copy_from_user(&input, a, sizeof(input)))
+			return -EFAULT;
+		ret = ioctl_disable_if_link_status(input);
+		if (ret)
+			pr_err("Error(%d) Disabling link interrupt:IF: %s\n",
+				ret, input);
+		return ret;
+	}
+	case USDPAA_IOCTL_GET_LINK_STATUS:
+	{
+		struct usdpaa_ioctl_link_status_args input;
+
+		if (copy_from_user(&input, a, sizeof(input)))
+			return -EFAULT;
+
+		input.link_status = ioctl_usdpaa_get_link_status(input.if_name);
+		if (input.link_status < 0)
+			return input.link_status;
+		if (copy_to_user(a, &input, sizeof(input)))
+			return -EFAULT;
+
+		return 0;
+	}
 	}
 	return -EINVAL;
 }
--- a/include/linux/fsl_usdpaa.h
+++ b/include/linux/fsl_usdpaa.h
@@ -365,6 +365,38 @@ int dpa_alloc_pop(struct dpa_alloc *allo
 int dpa_alloc_check(struct dpa_alloc *list, u32 id);
 #endif /* __KERNEL__ */
 
+
+/************************************
+ * Link Status support for user space
+ * interface
+ ************************************/
+#define IF_NAME_MAX_LEN 16
+#define NODE_NAME_LEN	32
+
+struct usdpaa_ioctl_link_status {
+	/* network device node name */
+	char		if_name[IF_NAME_MAX_LEN];
+	/* Eventfd value */
+	uint32_t	efd;
+};
+
+#define USDPAA_IOCTL_ENABLE_LINK_STATUS_INTERRUPT \
+	_IOW(USDPAA_IOCTL_MAGIC, 0x0E, struct usdpaa_ioctl_link_status)
+
+#define USDPAA_IOCTL_DISABLE_LINK_STATUS_INTERRUPT \
+	_IOW(USDPAA_IOCTL_MAGIC, 0x0F, char *)
+
+struct usdpaa_ioctl_link_status_args {
+	/* network device node name */
+	char    if_name[IF_NAME_MAX_LEN];
+	/* link status(UP/DOWN) */
+	int     link_status;
+};
+
+#define USDPAA_IOCTL_GET_LINK_STATUS \
+	_IOWR(USDPAA_IOCTL_MAGIC, 0x10, struct usdpaa_ioctl_link_status_args)
+
+
 #ifdef __cplusplus
 }
 #endif