1 files changed, 562 insertions, 0 deletions

diff --git a/drivers/staging/most/net/net.c b/drivers/staging/most/net/net.c
new file mode 100644
index 000000000000..30d816b7e165
--- /dev/null
+++ b/drivers/staging/most/net/net.c
@@ -0,0 +1,562 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * net.c - Networking component for Mostcore
+ *
+ * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/wait.h>
+#include <linux/kobject.h>
+#include "most/core.h"
+
+#define MEP_HDR_LEN 8
+#define MDP_HDR_LEN 16
+#define MAMAC_DATA_LEN (1024 - MDP_HDR_LEN)
+
+#define PMHL 5
+
+#define PMS_TELID_UNSEGM_MAMAC	0x0A
+#define PMS_FIFONO_MDP		0x01
+#define PMS_FIFONO_MEP		0x04
+#define PMS_MSGTYPE_DATA	0x04
+#define PMS_DEF_PRIO		0
+#define MEP_DEF_RETRY		15
+
+#define PMS_FIFONO_MASK		0x07
+#define PMS_FIFONO_SHIFT	3
+#define PMS_RETRY_SHIFT		4
+#define PMS_TELID_MASK		0x0F
+#define PMS_TELID_SHIFT		4
+
+#define HB(value)		((u8)((u16)(value) >> 8))
+#define LB(value)		((u8)(value))
+
+#define EXTRACT_BIT_SET(bitset_name, value) \
+	(((value) >> bitset_name##_SHIFT) & bitset_name##_MASK)
+
+#define PMS_IS_MEP(buf, len) \
+	((len) > MEP_HDR_LEN && \
+	 EXTRACT_BIT_SET(PMS_FIFONO, (buf)[3]) == PMS_FIFONO_MEP)
+
+static inline bool pms_is_mamac(char *buf, u32 len)
+{
+	return (len > MDP_HDR_LEN &&
+		EXTRACT_BIT_SET(PMS_FIFONO, buf[3]) == PMS_FIFONO_MDP &&
+		EXTRACT_BIT_SET(PMS_TELID, buf[14]) == PMS_TELID_UNSEGM_MAMAC);
+}
+
+struct net_dev_channel {
+	bool linked;
+	int ch_id;
+};
+
+struct net_dev_context {
+	struct most_interface *iface;
+	bool is_mamac;
+	struct net_device *dev;
+	struct net_dev_channel rx;
+	struct net_dev_channel tx;
+	struct list_head list;
+};
+
+static struct list_head net_devices = LIST_HEAD_INIT(net_devices);
+static struct mutex probe_disc_mt; /* ch->linked = true, most_nd_open */
+static struct spinlock list_lock; /* list_head, ch->linked = false, dev_hold */
+static struct core_component comp;
+
+static int skb_to_mamac(const struct sk_buff *skb, struct mbo *mbo)
+{
+	u8 *buff = mbo->virt_address;
+	const u8 broadcast[] = { 0x03, 0xFF };
+	const u8 *dest_addr = skb->data + 4;
+	const u8 *eth_type = skb->data + 12;
+	unsigned int payload_len = skb->len - ETH_HLEN;
+	unsigned int mdp_len = payload_len + MDP_HDR_LEN;
+
+	if (mbo->buffer_length < mdp_len) {
+		pr_err("drop: too small buffer! (%d for %d)\n",
+		       mbo->buffer_length, mdp_len);
+		return -EINVAL;
+	}
+
+	if (skb->len < ETH_HLEN) {
+		pr_err("drop: too small packet! (%d)\n", skb->len);
+		return -EINVAL;
+	}
+
+	if (dest_addr[0] == 0xFF && dest_addr[1] == 0xFF)
+		dest_addr = broadcast;
+
+	*buff++ = HB(mdp_len - 2);
+	*buff++ = LB(mdp_len - 2);
+
+	*buff++ = PMHL;
+	*buff++ = (PMS_FIFONO_MDP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
+	*buff++ = PMS_DEF_PRIO;
+	*buff++ = dest_addr[0];
+	*buff++ = dest_addr[1];
+	*buff++ = 0x00;
+
+	*buff++ = HB(payload_len + 6);
+	*buff++ = LB(payload_len + 6);
+
+	/* end of FPH here */
+
+	*buff++ = eth_type[0];
+	*buff++ = eth_type[1];
+	*buff++ = 0;
+	*buff++ = 0;
+
+	*buff++ = PMS_TELID_UNSEGM_MAMAC << 4 | HB(payload_len);
+	*buff++ = LB(payload_len);
+
+	memcpy(buff, skb->data + ETH_HLEN, payload_len);
+	mbo->buffer_length = mdp_len;
+	return 0;
+}
+
+static int skb_to_mep(const struct sk_buff *skb, struct mbo *mbo)
+{
+	u8 *buff = mbo->virt_address;
+	unsigned int mep_len = skb->len + MEP_HDR_LEN;
+
+	if (mbo->buffer_length < mep_len) {
+		pr_err("drop: too small buffer! (%d for %d)\n",
+		       mbo->buffer_length, mep_len);
+		return -EINVAL;
+	}
+
+	*buff++ = HB(mep_len - 2);
+	*buff++ = LB(mep_len - 2);
+
+	*buff++ = PMHL;
+	*buff++ = (PMS_FIFONO_MEP << PMS_FIFONO_SHIFT) | PMS_MSGTYPE_DATA;
+	*buff++ = (MEP_DEF_RETRY << PMS_RETRY_SHIFT) | PMS_DEF_PRIO;
+	*buff++ = 0;
+	*buff++ = 0;
+	*buff++ = 0;
+
+	memcpy(buff, skb->data, skb->len);
+	mbo->buffer_length = mep_len;
+	return 0;
+}
+
+static int most_nd_set_mac_address(struct net_device *dev, void *p)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	int err = eth_mac_addr(dev, p);
+
+	if (err)
+		return err;
+
+	nd->is_mamac =
+		(dev->dev_addr[0] == 0 && dev->dev_addr[1] == 0 &&
+		 dev->dev_addr[2] == 0 && dev->dev_addr[3] == 0);
+
+	/*
+	 * Set default MTU for the given packet type.
+	 * It is still possible to change MTU using ip tools afterwards.
+	 */
+	dev->mtu = nd->is_mamac ? MAMAC_DATA_LEN : ETH_DATA_LEN;
+
+	return 0;
+}
+
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr);
+
+static int most_nd_open(struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	int ret = 0;
+
+	mutex_lock(&probe_disc_mt);
+
+	if (most_start_channel(nd->iface, nd->rx.ch_id, &comp)) {
+		netdev_err(dev, "most_start_channel() failed\n");
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	if (most_start_channel(nd->iface, nd->tx.ch_id, &comp)) {
+		netdev_err(dev, "most_start_channel() failed\n");
+		most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	netif_carrier_off(dev);
+	if (is_valid_ether_addr(dev->dev_addr))
+		netif_dormant_off(dev);
+	else
+		netif_dormant_on(dev);
+	netif_wake_queue(dev);
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, on_netinfo);
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int most_nd_stop(struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	if (nd->iface->request_netinfo)
+		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id, NULL);
+	most_stop_channel(nd->iface, nd->rx.ch_id, &comp);
+	most_stop_channel(nd->iface, nd->tx.ch_id, &comp);
+
+	return 0;
+}
+
+static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
+				      struct net_device *dev)
+{
+	struct net_dev_context *nd = netdev_priv(dev);
+	struct mbo *mbo;
+	int ret;
+
+	mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &comp);
+
+	if (!mbo) {
+		netif_stop_queue(dev);
+		dev->stats.tx_fifo_errors++;
+		return NETDEV_TX_BUSY;
+	}
+
+	if (nd->is_mamac)
+		ret = skb_to_mamac(skb, mbo);
+	else
+		ret = skb_to_mep(skb, mbo);
+
+	if (ret) {
+		most_put_mbo(mbo);
+		dev->stats.tx_dropped++;
+		kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	most_submit_mbo(mbo);
+	dev->stats.tx_packets++;
+	dev->stats.tx_bytes += skb->len;
+	kfree_skb(skb);
+	return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops most_nd_ops = {
+	.ndo_open = most_nd_open,
+	.ndo_stop = most_nd_stop,
+	.ndo_start_xmit = most_nd_start_xmit,
+	.ndo_set_mac_address = most_nd_set_mac_address,
+};
+
+static void most_nd_setup(struct net_device *dev)
+{
+	ether_setup(dev);
+	dev->netdev_ops = &most_nd_ops;
+}
+
+static struct net_dev_context *get_net_dev(struct most_interface *iface)
+{
+	struct net_dev_context *nd;
+
+	list_for_each_entry(nd, &net_devices, list)
+		if (nd->iface == iface)
+			return nd;
+	return NULL;
+}
+
+static struct net_dev_context *get_net_dev_hold(struct most_interface *iface)
+{
+	struct net_dev_context *nd;
+	unsigned long flags;
+
+	spin_lock_irqsave(&list_lock, flags);
+	nd = get_net_dev(iface);
+	if (nd && nd->rx.linked && nd->tx.linked)
+		dev_hold(nd->dev);
+	else
+		nd = NULL;
+	spin_unlock_irqrestore(&list_lock, flags);
+	return nd;
+}
+
+static int comp_probe_channel(struct most_interface *iface, int channel_idx,
+			      struct most_channel_config *ccfg, char *name)
+{
+	struct net_dev_context *nd;
+	struct net_dev_channel *ch;
+	struct net_device *dev;
+	unsigned long flags;
+	int ret = 0;
+
+	if (!iface)
+		return -EINVAL;
+
+	if (ccfg->data_type != MOST_CH_ASYNC)
+		return -EINVAL;
+
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
+	if (!nd) {
+		dev = alloc_netdev(sizeof(struct net_dev_context), "meth%d",
+				   NET_NAME_UNKNOWN, most_nd_setup);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+
+		nd = netdev_priv(dev);
+		nd->iface = iface;
+		nd->dev = dev;
+
+		spin_lock_irqsave(&list_lock, flags);
+		list_add(&nd->list, &net_devices);
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+	} else {
+		ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
+		if (ch->linked) {
+			pr_err("direction is allocated\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+
+		if (register_netdev(nd->dev)) {
+			pr_err("register_netdev() failed\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+	ch->ch_id = channel_idx;
+	ch->linked = true;
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int comp_disconnect_channel(struct most_interface *iface,
+				   int channel_idx)
+{
+	struct net_dev_context *nd;
+	struct net_dev_channel *ch;
+	unsigned long flags;
+	int ret = 0;
+
+	mutex_lock(&probe_disc_mt);
+	nd = get_net_dev(iface);
+	if (!nd) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (nd->rx.linked && channel_idx == nd->rx.ch_id) {
+		ch = &nd->rx;
+	} else if (nd->tx.linked && channel_idx == nd->tx.ch_id) {
+		ch = &nd->tx;
+	} else {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (nd->rx.linked && nd->tx.linked) {
+		spin_lock_irqsave(&list_lock, flags);
+		ch->linked = false;
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		/*
+		 * do not call most_stop_channel() here, because channels are
+		 * going to be closed in ndo_stop() after unregister_netdev()
+		 */
+		unregister_netdev(nd->dev);
+	} else {
+		spin_lock_irqsave(&list_lock, flags);
+		list_del(&nd->list);
+		spin_unlock_irqrestore(&list_lock, flags);
+
+		free_netdev(nd->dev);
+	}
+
+unlock:
+	mutex_unlock(&probe_disc_mt);
+	return ret;
+}
+
+static int comp_resume_tx_channel(struct most_interface *iface,
+				  int channel_idx)
+{
+	struct net_dev_context *nd;
+
+	nd = get_net_dev_hold(iface);
+	if (!nd)
+		return 0;
+
+	if (nd->tx.ch_id != channel_idx)
+		goto put_nd;
+
+	netif_wake_queue(nd->dev);
+
+put_nd:
+	dev_put(nd->dev);
+	return 0;
+}
+
+static int comp_rx_data(struct mbo *mbo)
+{
+	const u32 zero = 0;
+	struct net_dev_context *nd;
+	char *buf = mbo->virt_address;
+	u32 len = mbo->processed_length;
+	struct sk_buff *skb;
+	struct net_device *dev;
+	unsigned int skb_len;
+	int ret = 0;
+
+	nd = get_net_dev_hold(mbo->ifp);
+	if (!nd)
+		return -EIO;
+
+	if (nd->rx.ch_id != mbo->hdm_channel_id) {
+		ret = -EIO;
+		goto put_nd;
+	}
+
+	dev = nd->dev;
+
+	if (nd->is_mamac) {
+		if (!pms_is_mamac(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
+
+		skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
+	} else {
+		if (!PMS_IS_MEP(buf, len)) {
+			ret = -EIO;
+			goto put_nd;
+		}
+
+		skb = dev_alloc_skb(len - MEP_HDR_LEN);
+	}
+
+	if (!skb) {
+		dev->stats.rx_dropped++;
+		pr_err_once("drop packet: no memory for skb\n");
+		goto out;
+	}
+
+	skb->dev = dev;
+
+	if (nd->is_mamac) {
+		/* dest */
+		ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
+
+		/* src */
+		skb_put_data(skb, &zero, 4);
+		skb_put_data(skb, buf + 5, 2);
+
+		/* eth type */
+		skb_put_data(skb, buf + 10, 2);
+
+		buf += MDP_HDR_LEN;
+		len -= MDP_HDR_LEN;
+	} else {
+		buf += MEP_HDR_LEN;
+		len -= MEP_HDR_LEN;
+	}
+
+	skb_put_data(skb, buf, len);
+	skb->protocol = eth_type_trans(skb, dev);
+	skb_len = skb->len;
+	if (netif_rx(skb) == NET_RX_SUCCESS) {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += skb_len;
+	} else {
+		dev->stats.rx_dropped++;
+	}
+
+out:
+	most_put_mbo(mbo);
+
+put_nd:
+	dev_put(nd->dev);
+	return ret;
+}
+
+static struct core_component comp = {
+	.name = "net",
+	.probe_channel = comp_probe_channel,
+	.disconnect_channel = comp_disconnect_channel,
+	.tx_completion = comp_resume_tx_channel,
+	.rx_completion = comp_rx_data,
+};
+
+static int __init most_net_init(void)
+{
+	spin_lock_init(&list_lock);
+	mutex_init(&probe_disc_mt);
+	return most_register_component(&comp);
+}
+
+static void __exit most_net_exit(void)
+{
+	most_deregister_component(&comp);
+}
+
+/**
+ * on_netinfo - callback for HDM to be informed about HW's MAC
+ * @param iface - most interface instance
+ * @param link_stat - link status
+ * @param mac_addr - MAC address
+ */
+static void on_netinfo(struct most_interface *iface,
+		       unsigned char link_stat, unsigned char *mac_addr)
+{
+	struct net_dev_context *nd;
+	struct net_device *dev;
+	const u8 *m = mac_addr;
+
+	nd = get_net_dev_hold(iface);
+	if (!nd)
+		return;
+
+	dev = nd->dev;
+
+	if (link_stat)
+		netif_carrier_on(dev);
+	else
+		netif_carrier_off(dev);
+
+	if (m && is_valid_ether_addr(m)) {
+		if (!is_valid_ether_addr(dev->dev_addr)) {
+			netdev_info(dev, "set mac %02x-%02x-%02x-%02x-%02x-%02x\n",
+				    m[0], m[1], m[2], m[3], m[4], m[5]);
+			ether_addr_copy(dev->dev_addr, m);
+			netif_dormant_off(dev);
+		} else if (!ether_addr_equal(dev->dev_addr, m)) {
+			netdev_warn(dev, "reject mac %02x-%02x-%02x-%02x-%02x-%02x\n",
+				    m[0], m[1], m[2], m[3], m[4], m[5]);
+		}
+	}
+
+	dev_put(nd->dev);
+}
+
+module_init(most_net_init);
+module_exit(most_net_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
+MODULE_DESCRIPTION("Networking Component Module for Mostcore");