[deliverable/linux.git] / drivers / staging / most / aim-network / networking.c

/*
 * Networking AIM - Networking Application Interface Module for MostCore
 *
 * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
 *
 * This program is distributed in the hope that 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.
 *
 * This file is licensed under GPLv2.
 */

#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 "mostcore.h"
#include "networking.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)

#define PMS_IS_MAMAC(buf, len) \
	((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 channels_opened;
	bool is_mamac;
	unsigned char link_stat;
	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 spinlock list_lock;
static struct most_aim aim;

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 = dev->ml_priv;
	int err = eth_mac_addr(dev, p);

	if (err)
		return err;

	BUG_ON(nd->dev != dev);

	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 int most_nd_open(struct net_device *dev)
{
	struct net_dev_context *nd = dev->ml_priv;

	netdev_info(dev, "open net device\n");

	BUG_ON(nd->dev != dev);

	if (nd->channels_opened)
		return -EFAULT;

	BUG_ON(!nd->tx.linked || !nd->rx.linked);

	if (most_start_channel(nd->iface, nd->rx.ch_id, &aim)) {
		netdev_err(dev, "most_start_channel() failed\n");
		return -EBUSY;
	}

	if (most_start_channel(nd->iface, nd->tx.ch_id, &aim)) {
		netdev_err(dev, "most_start_channel() failed\n");
		most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
		return -EBUSY;
	}

	nd->channels_opened = true;

	if (nd->is_mamac) {
		nd->link_stat = 1;
		netif_wake_queue(dev);
	} else {
		nd->iface->request_netinfo(nd->iface, nd->tx.ch_id);
	}

	return 0;
}

static int most_nd_stop(struct net_device *dev)
{
	struct net_dev_context *nd = dev->ml_priv;

	netdev_info(dev, "stop net device\n");

	BUG_ON(nd->dev != dev);
	netif_stop_queue(dev);

	if (nd->channels_opened) {
		most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
		most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
		nd->channels_opened = false;
	}

	return 0;
}

static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
				      struct net_device *dev)
{
	struct net_dev_context *nd = dev->ml_priv;
	struct mbo *mbo;
	int ret;

	BUG_ON(nd->dev != dev);

	mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &aim);

	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)
{
	netdev_info(dev, "setup net device\n");
	ether_setup(dev);
	dev->netdev_ops = &most_nd_ops;
}

static void most_net_rm_netdev_safe(struct net_dev_context *nd)
{
	if (!nd->dev)
		return;

	pr_info("remove net device %p\n", nd->dev);

	unregister_netdev(nd->dev);
	free_netdev(nd->dev);
	nd->dev = NULL;
}

static struct net_dev_context *get_net_dev_context(
	struct most_interface *iface)
{
	struct net_dev_context *nd, *tmp;
	unsigned long flags;

	spin_lock_irqsave(&list_lock, flags);
	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
		if (nd->iface == iface) {
			spin_unlock_irqrestore(&list_lock, flags);
			return nd;
		}
	}
	spin_unlock_irqrestore(&list_lock, flags);
	return NULL;
}

static int aim_probe_channel(struct most_interface *iface, int channel_idx,
			     struct most_channel_config *ccfg,
			     struct kobject *parent, char *name)
{
	struct net_dev_context *nd;
	struct net_dev_channel *ch;
	unsigned long flags;

	if (!iface)
		return -EINVAL;

	if (ccfg->data_type != MOST_CH_ASYNC)
		return -EINVAL;

	nd = get_net_dev_context(iface);

	if (!nd) {
		nd = kzalloc(sizeof(*nd), GFP_KERNEL);
		if (!nd)
			return -ENOMEM;

		nd->iface = iface;

		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;
	if (ch->linked) {
		pr_err("only one channel per instance & direction allowed\n");
		return -EINVAL;
	}

	if (nd->tx.linked || nd->rx.linked) {
		struct net_device *dev =
			alloc_netdev(0, "meth%d", NET_NAME_UNKNOWN,
				     most_nd_setup);

		if (!dev) {
			pr_err("no memory for net_device\n");
			return -ENOMEM;
		}

		nd->dev = dev;
		ch->ch_id = channel_idx;
		ch->linked = true;

		dev->ml_priv = nd;
		if (register_netdev(dev)) {
			pr_err("registering net device failed\n");
			ch->linked = false;
			free_netdev(dev);
			return -EINVAL;
		}
	}

	ch->ch_id = channel_idx;
	ch->linked = true;

	return 0;
}

static int aim_disconnect_channel(struct most_interface *iface,
				  int channel_idx)
{
	struct net_dev_context *nd;
	struct net_dev_channel *ch;
	unsigned long flags;

	nd = get_net_dev_context(iface);
	if (!nd)
		return -EINVAL;

	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
		return -EINVAL;

	ch->linked = false;

	/*
	 * do not call most_stop_channel() here, because channels are
	 * going to be closed in ndo_stop() after unregister_netdev()
	 */
	most_net_rm_netdev_safe(nd);

	if (!nd->rx.linked && !nd->tx.linked) {
		spin_lock_irqsave(&list_lock, flags);
		list_del(&nd->list);
		spin_unlock_irqrestore(&list_lock, flags);
		kfree(nd);
	}

	return 0;
}

static int aim_resume_tx_channel(struct most_interface *iface,
				 int channel_idx)
{
	struct net_dev_context *nd;

	nd = get_net_dev_context(iface);
	if (!nd || !nd->channels_opened || nd->tx.ch_id != channel_idx)
		return 0;

	if (!nd->dev)
		return 0;

	netif_wake_queue(nd->dev);
	return 0;
}

static int aim_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;

	nd = get_net_dev_context(mbo->ifp);
	if (!nd || !nd->channels_opened || nd->rx.ch_id != mbo->hdm_channel_id)
		return -EIO;

	dev = nd->dev;
	if (!dev) {
		pr_err_once("drop packet: missing net_device\n");
		return -EIO;
	}

	if (nd->is_mamac) {
		if (!PMS_IS_MAMAC(buf, len))
			return -EIO;

		skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
	} else {
		if (!PMS_IS_MEP(buf, len))
			return -EIO;

		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 */
		memcpy(skb_put(skb, 4), &zero, 4);
		memcpy(skb_put(skb, 2), buf + 5, 2);

		/* eth type */
		memcpy(skb_put(skb, 2), buf + 10, 2);

		buf += MDP_HDR_LEN;
		len -= MDP_HDR_LEN;
	} else {
		buf += MEP_HDR_LEN;
		len -= MEP_HDR_LEN;
	}

	memcpy(skb_put(skb, len), 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);
	return 0;
}

static struct most_aim aim = {
	.name = "networking",
	.probe_channel = aim_probe_channel,
	.disconnect_channel = aim_disconnect_channel,
	.tx_completion = aim_resume_tx_channel,
	.rx_completion = aim_rx_data,
};

static int __init most_net_init(void)
{
	pr_info("most_net_init()\n");
	spin_lock_init(&list_lock);
	return most_register_aim(&aim);
}

static void __exit most_net_exit(void)
{
	struct net_dev_context *nd, *tmp;
	unsigned long flags;

	spin_lock_irqsave(&list_lock, flags);
	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
		list_del(&nd->list);
		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()
		 */
		most_net_rm_netdev_safe(nd);
		kfree(nd);
		spin_lock_irqsave(&list_lock, flags);
	}
	spin_unlock_irqrestore(&list_lock, flags);

	most_deregister_aim(&aim);
	pr_info("most_net_exit()\n");
}

/**
 * most_deliver_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
 */
void most_deliver_netinfo(struct most_interface *iface,
			  unsigned char link_stat, unsigned char *mac_addr)
{
	struct net_dev_context *nd;
	struct net_device *dev;

	pr_info("Received netinfo from %s\n", iface->description);

	nd = get_net_dev_context(iface);
	if (!nd)
		return;

	dev = nd->dev;
	if (!dev)
		return;

	if (mac_addr)
		ether_addr_copy(dev->dev_addr, mac_addr);

	if (nd->link_stat != link_stat) {
		nd->link_stat = link_stat;
		if (nd->link_stat)
			netif_wake_queue(dev);
		else
			netif_stop_queue(dev);
	}
}
EXPORT_SYMBOL(most_deliver_netinfo);

module_init(most_net_init);
module_exit(most_net_exit);
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
MODULE_DESCRIPTION("Networking Application Interface Module for MostCore");
Commit	Line	Data
d4a8ce7f CG	1	/*
	2	* Networking AIM - Networking Application Interface Module for MostCore
	3	*
	4	* Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
	5	*
	6	* This program is distributed in the hope that it will be useful,
	7	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	8	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	9	* GNU General Public License for more details.
	10	*
	11	* This file is licensed under GPLv2.
	12	*/
	13
	14	#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
	15
	16	#include <linux/module.h>
	17	#include <linux/netdevice.h>
	18	#include <linux/etherdevice.h>
	19	#include <linux/slab.h>
	20	#include <linux/init.h>
	21	#include <linux/list.h>
	22	#include <linux/wait.h>
	23	#include <linux/kobject.h>
	24	#include "mostcore.h"
	25	#include "networking.h"
	26
d4a8ce7f CG	27	#define MEP_HDR_LEN 8
	28	#define MDP_HDR_LEN 16
	29	#define MAMAC_DATA_LEN (1024 - MDP_HDR_LEN)
	30
	31	#define PMHL 5
	32
	33	#define PMS_TELID_UNSEGM_MAMAC 0x0A
	34	#define PMS_FIFONO_MDP 0x01
	35	#define PMS_FIFONO_MEP 0x04
	36	#define PMS_MSGTYPE_DATA 0x04
	37	#define PMS_DEF_PRIO 0
	38	#define MEP_DEF_RETRY 15
	39
	40	#define PMS_FIFONO_MASK 0x07
	41	#define PMS_FIFONO_SHIFT 3
	42	#define PMS_RETRY_SHIFT 4
	43	#define PMS_TELID_MASK 0x0F
	44	#define PMS_TELID_SHIFT 4
	45
	46	#define HB(value) ((u8)((u16)(value) >> 8))
	47	#define LB(value) ((u8)(value))
	48
d4a8ce7f CG	49	#define EXTRACT_BIT_SET(bitset_name, value) \
	50	(((value) >> bitset_name##_SHIFT) & bitset_name##_MASK)
	51
	52	#define PMS_IS_MEP(buf, len) \
	53	((len) > MEP_HDR_LEN && \
	54	EXTRACT_BIT_SET(PMS_FIFONO, (buf)[3]) == PMS_FIFONO_MEP)
	55
	56	#define PMS_IS_MAMAC(buf, len) \
	57	((len) > MDP_HDR_LEN && \
	58	EXTRACT_BIT_SET(PMS_FIFONO, (buf)[3]) == PMS_FIFONO_MDP && \
	59	EXTRACT_BIT_SET(PMS_TELID, (buf)[14]) == PMS_TELID_UNSEGM_MAMAC)
	60
	61	struct net_dev_channel {
	62	bool linked;
	63	int ch_id;
	64	};
	65
	66	struct net_dev_context {
	67	struct most_interface *iface;
	68	bool channels_opened;
	69	bool is_mamac;
	70	unsigned char link_stat;
	71	struct net_device *dev;
	72	struct net_dev_channel rx;
	73	struct net_dev_channel tx;
	74	struct list_head list;
	75	};
	76
	77	static struct list_head net_devices = LIST_HEAD_INIT(net_devices);
	78	static struct spinlock list_lock;
f13f6981	79	static struct most_aim aim;
d4a8ce7f	80
d4a8ce7f CG	81	static int skb_to_mamac(const struct sk_buff skb, struct mbo mbo)
	82	{
	83	u8 *buff = mbo->virt_address;
	84	const u8 broadcast[] = { 0x03, 0xFF };
	85	const u8 *dest_addr = skb->data + 4;
	86	const u8 *eth_type = skb->data + 12;
	87	unsigned int payload_len = skb->len - ETH_HLEN;
	88	unsigned int mdp_len = payload_len + MDP_HDR_LEN;
	89
	90	if (mbo->buffer_length < mdp_len) {
	91	pr_err("drop: too small buffer! (%d for %d)\n",
	92	mbo->buffer_length, mdp_len);
	93	return -EINVAL;
	94	}
	95
	96	if (skb->len < ETH_HLEN) {
	97	pr_err("drop: too small packet! (%d)\n", skb->len);
	98	return -EINVAL;
	99	}
	100
	101	if (dest_addr[0] == 0xFF && dest_addr[1] == 0xFF)
	102	dest_addr = broadcast;
	103
	104	*buff++ = HB(mdp_len - 2);
	105	*buff++ = LB(mdp_len - 2);
	106
	107	*buff++ = PMHL;
	108	*buff++ = (PMS_FIFONO_MDP << PMS_FIFONO_SHIFT) \| PMS_MSGTYPE_DATA;
	109	*buff++ = PMS_DEF_PRIO;
	110	*buff++ = dest_addr[0];
	111	*buff++ = dest_addr[1];
	112	*buff++ = 0x00;
	113
	114	*buff++ = HB(payload_len + 6);
	115	*buff++ = LB(payload_len + 6);
	116
	117	/* end of FPH here */
	118
	119	*buff++ = eth_type[0];
	120	*buff++ = eth_type[1];
	121	*buff++ = 0;
	122	*buff++ = 0;
	123
	124	*buff++ = PMS_TELID_UNSEGM_MAMAC << 4 \| HB(payload_len);
	125	*buff++ = LB(payload_len);
	126
	127	memcpy(buff, skb->data + ETH_HLEN, payload_len);
	128	mbo->buffer_length = mdp_len;
	129	return 0;
	130	}
	131
	132	static int skb_to_mep(const struct sk_buff skb, struct mbo mbo)
	133	{
	134	u8 *buff = mbo->virt_address;
	135	unsigned int mep_len = skb->len + MEP_HDR_LEN;
	136
	137	if (mbo->buffer_length < mep_len) {
	138	pr_err("drop: too small buffer! (%d for %d)\n",
	139	mbo->buffer_length, mep_len);
	140	return -EINVAL;
	141	}
	142
	143	*buff++ = HB(mep_len - 2);
	144	*buff++ = LB(mep_len - 2);
145
146	*buff++ = PMHL;
147	*buff++ = (PMS_FIFONO_MEP << PMS_FIFONO_SHIFT) \| PMS_MSGTYPE_DATA;
148	*buff++ = (MEP_DEF_RETRY << PMS_RETRY_SHIFT) \| PMS_DEF_PRIO;
149	*buff++ = 0;
150	*buff++ = 0;
151	*buff++ = 0;
152
153	memcpy(buff, skb->data, skb->len);
154	mbo->buffer_length = mep_len;
155	return 0;
156	}
157
158	static int most_nd_set_mac_address(struct net_device dev, void p)
159	{
160	struct net_dev_context *nd = dev->ml_priv;
161	int err = eth_mac_addr(dev, p);
162
163	if (err)
164	return err;
165
166	BUG_ON(nd->dev != dev);
167
168	nd->is_mamac =
169	(dev->dev_addr[0] == 0 && dev->dev_addr[1] == 0 &&
170	dev->dev_addr[2] == 0 && dev->dev_addr[3] == 0);
171
172	/*
173	* Set default MTU for the given packet type.
174	* It is still possible to change MTU using ip tools afterwards.
175	*/
176	dev->mtu = nd->is_mamac ? MAMAC_DATA_LEN : ETH_DATA_LEN;
177
178	return 0;
179	}
180
181	static int most_nd_open(struct net_device *dev)
182	{
183	struct net_dev_context *nd = dev->ml_priv;
184
efc0cfa1	185	netdev_info(dev, "open net device\n");
d4a8ce7f CG	186
	187	BUG_ON(nd->dev != dev);
	188
	189	if (nd->channels_opened)
	190	return -EFAULT;
	191
	192	BUG_ON(!nd->tx.linked \|\| !nd->rx.linked);
	193
f13f6981	194	if (most_start_channel(nd->iface, nd->rx.ch_id, &aim)) {
efc0cfa1	195	netdev_err(dev, "most_start_channel() failed\n");
d4a8ce7f CG	196	return -EBUSY;
	197	}
	198
f13f6981	199	if (most_start_channel(nd->iface, nd->tx.ch_id, &aim)) {
efc0cfa1	200	netdev_err(dev, "most_start_channel() failed\n");
f13f6981	201	most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
d4a8ce7f CG	202	return -EBUSY;
	203	}
	204
	205	nd->channels_opened = true;
	206
	207	if (nd->is_mamac) {
	208	nd->link_stat = 1;
	209	netif_wake_queue(dev);
	210	} else {
	211	nd->iface->request_netinfo(nd->iface, nd->tx.ch_id);
	212	}
	213
	214	return 0;
	215	}
	216
	217	static int most_nd_stop(struct net_device *dev)
	218	{
	219	struct net_dev_context *nd = dev->ml_priv;
	220
efc0cfa1	221	netdev_info(dev, "stop net device\n");
d4a8ce7f CG	222
	223	BUG_ON(nd->dev != dev);
	224	netif_stop_queue(dev);
	225
	226	if (nd->channels_opened) {
f13f6981 CG	227	most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
f13f6981 CG	228	most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
d4a8ce7f CG	229	nd->channels_opened = false;
	230	}
	231
	232	return 0;
	233	}
	234
	235	static netdev_tx_t most_nd_start_xmit(struct sk_buff *skb,
	236	struct net_device *dev)
	237	{
	238	struct net_dev_context *nd = dev->ml_priv;
	239	struct mbo *mbo;
	240	int ret;
	241
	242	BUG_ON(nd->dev != dev);
	243
71457d48	244	mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &aim);
d4a8ce7f CG	245
	246	if (!mbo) {
	247	netif_stop_queue(dev);
	248	dev->stats.tx_fifo_errors++;
	249	return NETDEV_TX_BUSY;
	250	}
	251
	252	if (nd->is_mamac)
	253	ret = skb_to_mamac(skb, mbo);
	254	else
	255	ret = skb_to_mep(skb, mbo);
	256
	257	if (ret) {
	258	most_put_mbo(mbo);
	259	dev->stats.tx_dropped++;
	260	kfree_skb(skb);
	261	return NETDEV_TX_OK;
	262	}
	263
	264	most_submit_mbo(mbo);
	265	dev->stats.tx_packets++;
	266	dev->stats.tx_bytes += skb->len;
	267	kfree_skb(skb);
	268	return NETDEV_TX_OK;
	269	}
	270
	271	static const struct net_device_ops most_nd_ops = {
	272	.ndo_open = most_nd_open,
	273	.ndo_stop = most_nd_stop,
	274	.ndo_start_xmit = most_nd_start_xmit,
	275	.ndo_set_mac_address = most_nd_set_mac_address,
	276	};
	277
	278	static void most_nd_setup(struct net_device *dev)
	279	{
efc0cfa1	280	netdev_info(dev, "setup net device\n");
d4a8ce7f CG	281	ether_setup(dev);
	282	dev->netdev_ops = &most_nd_ops;
	283	}
	284
	285	static void most_net_rm_netdev_safe(struct net_dev_context *nd)
	286	{
	287	if (!nd->dev)
	288	return;
	289
	290	pr_info("remove net device %p\n", nd->dev);
	291
	292	unregister_netdev(nd->dev);
	293	free_netdev(nd->dev);
1865e4ea	294	nd->dev = NULL;
d4a8ce7f CG	295	}
	296
	297	static struct net_dev_context *get_net_dev_context(
	298	struct most_interface *iface)
	299	{
	300	struct net_dev_context nd, tmp;
a75c0312	301	unsigned long flags;
d4a8ce7f	302
a75c0312	303	spin_lock_irqsave(&list_lock, flags);
d4a8ce7f CG	304	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
d4a8ce7f CG	305	if (nd->iface == iface) {
a75c0312	306	spin_unlock_irqrestore(&list_lock, flags);
d4a8ce7f CG	307	return nd;
	308	}
	309	}
a75c0312	310	spin_unlock_irqrestore(&list_lock, flags);
b3c1a617	311	return NULL;
d4a8ce7f CG	312	}
	313
	314	static int aim_probe_channel(struct most_interface *iface, int channel_idx,
	315	struct most_channel_config *ccfg,
	316	struct kobject parent, char name)
	317	{
	318	struct net_dev_context *nd;
	319	struct net_dev_channel *ch;
a75c0312	320	unsigned long flags;
d4a8ce7f CG	321
	322	if (!iface)
	323	return -EINVAL;
	324
	325	if (ccfg->data_type != MOST_CH_ASYNC)
	326	return -EINVAL;
	327
	328	nd = get_net_dev_context(iface);
	329
	330	if (!nd) {
	331	nd = kzalloc(sizeof(*nd), GFP_KERNEL);
	332	if (!nd)
	333	return -ENOMEM;
	334
	335	nd->iface = iface;
	336
a75c0312	337	spin_lock_irqsave(&list_lock, flags);
d4a8ce7f	338	list_add(&nd->list, &net_devices);
a75c0312	339	spin_unlock_irqrestore(&list_lock, flags);
d4a8ce7f CG	340	}
	341
	342	ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
	343	if (ch->linked) {
	344	pr_err("only one channel per instance & direction allowed\n");
	345	return -EINVAL;
	346	}
	347
	348	if (nd->tx.linked \|\| nd->rx.linked) {
	349	struct net_device *dev =
1446ff09 CG	350	alloc_netdev(0, "meth%d", NET_NAME_UNKNOWN,
1446ff09 CG	351	most_nd_setup);
d4a8ce7f CG	352
	353	if (!dev) {
	354	pr_err("no memory for net_device\n");
	355	return -ENOMEM;
	356	}
	357
	358	nd->dev = dev;
25ef42f3 CG	359	ch->ch_id = channel_idx;
25ef42f3 CG	360	ch->linked = true;
d4a8ce7f CG	361
	362	dev->ml_priv = nd;
	363	if (register_netdev(dev)) {
	364	pr_err("registering net device failed\n");
25ef42f3	365	ch->linked = false;
d4a8ce7f CG	366	free_netdev(dev);
	367	return -EINVAL;
	368	}
	369	}
	370
	371	ch->ch_id = channel_idx;
	372	ch->linked = true;
	373
	374	return 0;
	375	}
	376
	377	static int aim_disconnect_channel(struct most_interface *iface,
	378	int channel_idx)
	379	{
	380	struct net_dev_context *nd;
	381	struct net_dev_channel *ch;
a75c0312	382	unsigned long flags;
d4a8ce7f CG	383
	384	nd = get_net_dev_context(iface);
	385	if (!nd)
	386	return -EINVAL;
	387
	388	if (nd->rx.linked && channel_idx == nd->rx.ch_id)
	389	ch = &nd->rx;
	390	else if (nd->tx.linked && channel_idx == nd->tx.ch_id)
	391	ch = &nd->tx;
	392	else
	393	return -EINVAL;
	394
	395	ch->linked = false;
	396
	397	/*
	398	* do not call most_stop_channel() here, because channels are
	399	* going to be closed in ndo_stop() after unregister_netdev()
	400	*/
	401	most_net_rm_netdev_safe(nd);
	402
	403	if (!nd->rx.linked && !nd->tx.linked) {
a75c0312	404	spin_lock_irqsave(&list_lock, flags);
d4a8ce7f	405	list_del(&nd->list);
a75c0312	406	spin_unlock_irqrestore(&list_lock, flags);
d4a8ce7f CG	407	kfree(nd);
	408	}
	409
	410	return 0;
	411	}
	412
	413	static int aim_resume_tx_channel(struct most_interface *iface,
	414	int channel_idx)
	415	{
	416	struct net_dev_context *nd;
	417
	418	nd = get_net_dev_context(iface);
	419	if (!nd \|\| !nd->channels_opened \|\| nd->tx.ch_id != channel_idx)
	420	return 0;
	421
	422	if (!nd->dev)
	423	return 0;
	424
	425	netif_wake_queue(nd->dev);
	426	return 0;
	427	}
	428
	429	static int aim_rx_data(struct mbo *mbo)
	430	{
	431	const u32 zero = 0;
	432	struct net_dev_context *nd;
	433	char *buf = mbo->virt_address;
2aa9b96f	434	u32 len = mbo->processed_length;
d4a8ce7f CG	435	struct sk_buff *skb;
d4a8ce7f CG	436	struct net_device *dev;
1c55d30b	437	unsigned int skb_len;
d4a8ce7f CG	438
	439	nd = get_net_dev_context(mbo->ifp);
	440	if (!nd \|\| !nd->channels_opened \|\| nd->rx.ch_id != mbo->hdm_channel_id)
	441	return -EIO;
	442
	443	dev = nd->dev;
	444	if (!dev) {
	445	pr_err_once("drop packet: missing net_device\n");
	446	return -EIO;
	447	}
	448
	449	if (nd->is_mamac) {
	450	if (!PMS_IS_MAMAC(buf, len))
	451	return -EIO;
	452
	453	skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
	454	} else {
	455	if (!PMS_IS_MEP(buf, len))
	456	return -EIO;
	457
	458	skb = dev_alloc_skb(len - MEP_HDR_LEN);
	459	}
	460
	461	if (!skb) {
	462	dev->stats.rx_dropped++;
	463	pr_err_once("drop packet: no memory for skb\n");
	464	goto out;
	465	}
	466
	467	skb->dev = dev;
	468
	469	if (nd->is_mamac) {
	470	/* dest */
d58b0ee3	471	ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
d4a8ce7f CG	472
	473	/* src */
	474	memcpy(skb_put(skb, 4), &zero, 4);
	475	memcpy(skb_put(skb, 2), buf + 5, 2);
	476
	477	/* eth type */
	478	memcpy(skb_put(skb, 2), buf + 10, 2);
	479
	480	buf += MDP_HDR_LEN;
	481	len -= MDP_HDR_LEN;
	482	} else {
	483	buf += MEP_HDR_LEN;
	484	len -= MEP_HDR_LEN;
	485	}
	486
	487	memcpy(skb_put(skb, len), buf, len);
	488	skb->protocol = eth_type_trans(skb, dev);
1c55d30b CG	489	skb_len = skb->len;
	490	if (netif_rx(skb) == NET_RX_SUCCESS) {
	491	dev->stats.rx_packets++;
	492	dev->stats.rx_bytes += skb_len;
	493	} else {
	494	dev->stats.rx_dropped++;
	495	}
d4a8ce7f CG	496
	497	out:
	498	most_put_mbo(mbo);
	499	return 0;
	500	}
	501
ec5c00af CG	502	static struct most_aim aim = {
	503	.name = "networking",
	504	.probe_channel = aim_probe_channel,
	505	.disconnect_channel = aim_disconnect_channel,
	506	.tx_completion = aim_resume_tx_channel,
	507	.rx_completion = aim_rx_data,
	508	};
	509
d4a8ce7f CG	510	static int __init most_net_init(void)
	511	{
	512	pr_info("most_net_init()\n");
	513	spin_lock_init(&list_lock);
d4a8ce7f CG	514	return most_register_aim(&aim);
	515	}
	516
	517	static void __exit most_net_exit(void)
	518	{
	519	struct net_dev_context nd, tmp;
a75c0312	520	unsigned long flags;
d4a8ce7f	521
a75c0312	522	spin_lock_irqsave(&list_lock, flags);
d4a8ce7f CG	523	list_for_each_entry_safe(nd, tmp, &net_devices, list) {
d4a8ce7f CG	524	list_del(&nd->list);
a75c0312	525	spin_unlock_irqrestore(&list_lock, flags);
d4a8ce7f CG	526	/*
	527	* do not call most_stop_channel() here, because channels are
	528	* going to be closed in ndo_stop() after unregister_netdev()
	529	*/
	530	most_net_rm_netdev_safe(nd);
	531	kfree(nd);
a75c0312	532	spin_lock_irqsave(&list_lock, flags);
d4a8ce7f	533	}
a75c0312	534	spin_unlock_irqrestore(&list_lock, flags);
d4a8ce7f CG	535
	536	most_deregister_aim(&aim);
	537	pr_info("most_net_exit()\n");
	538	}
	539
	540	/**
	541	* most_deliver_netinfo - callback for HDM to be informed about HW's MAC
	542	* @param iface - most interface instance
	543	* @param link_stat - link status
	544	* @param mac_addr - MAC address
	545	*/
	546	void most_deliver_netinfo(struct most_interface *iface,
	547	unsigned char link_stat, unsigned char *mac_addr)
	548	{
	549	struct net_dev_context *nd;
	550	struct net_device *dev;
	551
	552	pr_info("Received netinfo from %s\n", iface->description);
	553
	554	nd = get_net_dev_context(iface);
	555	if (!nd)
	556	return;
	557
	558	dev = nd->dev;
	559	if (!dev)
	560	return;
	561
	562	if (mac_addr)
d58b0ee3	563	ether_addr_copy(dev->dev_addr, mac_addr);
d4a8ce7f CG	564
	565	if (nd->link_stat != link_stat) {
	566	nd->link_stat = link_stat;
	567	if (nd->link_stat)
	568	netif_wake_queue(dev);
	569	else
	570	netif_stop_queue(dev);
	571	}
	572	}
	573	EXPORT_SYMBOL(most_deliver_netinfo);
	574
	575	module_init(most_net_init);
	576	module_exit(most_net_exit);
	577	MODULE_LICENSE("GPL");
	578	MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
	579	MODULE_DESCRIPTION("Networking Application Interface Module for MostCore");