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