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