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
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
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;
79 static struct most_aim aim;
80
skb_to_mamac(const struct sk_buff * skb,struct mbo * mbo)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
skb_to_mep(const struct sk_buff * skb,struct mbo * mbo)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
most_nd_set_mac_address(struct net_device * dev,void * p)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
most_nd_open(struct net_device * dev)181 static int most_nd_open(struct net_device *dev)
182 {
183 struct net_dev_context *nd = dev->ml_priv;
184
185 netdev_info(dev, "open net device\n");
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
194 if (most_start_channel(nd->iface, nd->rx.ch_id, &aim)) {
195 netdev_err(dev, "most_start_channel() failed\n");
196 return -EBUSY;
197 }
198
199 if (most_start_channel(nd->iface, nd->tx.ch_id, &aim)) {
200 netdev_err(dev, "most_start_channel() failed\n");
201 most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
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
most_nd_stop(struct net_device * dev)217 static int most_nd_stop(struct net_device *dev)
218 {
219 struct net_dev_context *nd = dev->ml_priv;
220
221 netdev_info(dev, "stop net device\n");
222
223 BUG_ON(nd->dev != dev);
224 netif_stop_queue(dev);
225
226 if (nd->channels_opened) {
227 most_stop_channel(nd->iface, nd->rx.ch_id, &aim);
228 most_stop_channel(nd->iface, nd->tx.ch_id, &aim);
229 nd->channels_opened = false;
230 }
231
232 return 0;
233 }
234
most_nd_start_xmit(struct sk_buff * skb,struct net_device * dev)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
244 mbo = most_get_mbo(nd->iface, nd->tx.ch_id, &aim);
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
most_nd_setup(struct net_device * dev)278 static void most_nd_setup(struct net_device *dev)
279 {
280 netdev_info(dev, "setup net device\n");
281 ether_setup(dev);
282 dev->netdev_ops = &most_nd_ops;
283 }
284
most_net_rm_netdev_safe(struct net_dev_context * nd)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);
294 nd->dev = NULL;
295 }
296
get_net_dev_context(struct most_interface * iface)297 static struct net_dev_context *get_net_dev_context(
298 struct most_interface *iface)
299 {
300 struct net_dev_context *nd, *tmp;
301
302 spin_lock(&list_lock);
303 list_for_each_entry_safe(nd, tmp, &net_devices, list) {
304 if (nd->iface == iface) {
305 spin_unlock(&list_lock);
306 return nd;
307 }
308 }
309 spin_unlock(&list_lock);
310 return NULL;
311 }
312
aim_probe_channel(struct most_interface * iface,int channel_idx,struct most_channel_config * ccfg,struct kobject * parent,char * name)313 static int aim_probe_channel(struct most_interface *iface, int channel_idx,
314 struct most_channel_config *ccfg,
315 struct kobject *parent, char *name)
316 {
317 struct net_dev_context *nd;
318 struct net_dev_channel *ch;
319
320 if (!iface)
321 return -EINVAL;
322
323 if (ccfg->data_type != MOST_CH_ASYNC)
324 return -EINVAL;
325
326 nd = get_net_dev_context(iface);
327
328 if (!nd) {
329 nd = kzalloc(sizeof(*nd), GFP_KERNEL);
330 if (!nd)
331 return -ENOMEM;
332
333 nd->iface = iface;
334
335 spin_lock(&list_lock);
336 list_add(&nd->list, &net_devices);
337 spin_unlock(&list_lock);
338 }
339
340 ch = ccfg->direction == MOST_CH_TX ? &nd->tx : &nd->rx;
341 if (ch->linked) {
342 pr_err("only one channel per instance & direction allowed\n");
343 return -EINVAL;
344 }
345
346 if (nd->tx.linked || nd->rx.linked) {
347 struct net_device *dev =
348 alloc_netdev(0, "meth%d", NET_NAME_UNKNOWN,
349 most_nd_setup);
350
351 if (!dev) {
352 pr_err("no memory for net_device\n");
353 return -ENOMEM;
354 }
355
356 nd->dev = dev;
357 ch->ch_id = channel_idx;
358 ch->linked = true;
359
360 dev->ml_priv = nd;
361 if (register_netdev(dev)) {
362 pr_err("registering net device failed\n");
363 ch->linked = false;
364 free_netdev(dev);
365 return -EINVAL;
366 }
367 }
368
369 ch->ch_id = channel_idx;
370 ch->linked = true;
371
372 return 0;
373 }
374
aim_disconnect_channel(struct most_interface * iface,int channel_idx)375 static int aim_disconnect_channel(struct most_interface *iface,
376 int channel_idx)
377 {
378 struct net_dev_context *nd;
379 struct net_dev_channel *ch;
380
381 nd = get_net_dev_context(iface);
382 if (!nd)
383 return -EINVAL;
384
385 if (nd->rx.linked && channel_idx == nd->rx.ch_id)
386 ch = &nd->rx;
387 else if (nd->tx.linked && channel_idx == nd->tx.ch_id)
388 ch = &nd->tx;
389 else
390 return -EINVAL;
391
392 ch->linked = false;
393
394 /*
395 * do not call most_stop_channel() here, because channels are
396 * going to be closed in ndo_stop() after unregister_netdev()
397 */
398 most_net_rm_netdev_safe(nd);
399
400 if (!nd->rx.linked && !nd->tx.linked) {
401 spin_lock(&list_lock);
402 list_del(&nd->list);
403 spin_unlock(&list_lock);
404 kfree(nd);
405 }
406
407 return 0;
408 }
409
aim_resume_tx_channel(struct most_interface * iface,int channel_idx)410 static int aim_resume_tx_channel(struct most_interface *iface,
411 int channel_idx)
412 {
413 struct net_dev_context *nd;
414
415 nd = get_net_dev_context(iface);
416 if (!nd || !nd->channels_opened || nd->tx.ch_id != channel_idx)
417 return 0;
418
419 if (!nd->dev)
420 return 0;
421
422 netif_wake_queue(nd->dev);
423 return 0;
424 }
425
aim_rx_data(struct mbo * mbo)426 static int aim_rx_data(struct mbo *mbo)
427 {
428 const u32 zero = 0;
429 struct net_dev_context *nd;
430 char *buf = mbo->virt_address;
431 u32 len = mbo->processed_length;
432 struct sk_buff *skb;
433 struct net_device *dev;
434
435 nd = get_net_dev_context(mbo->ifp);
436 if (!nd || !nd->channels_opened || nd->rx.ch_id != mbo->hdm_channel_id)
437 return -EIO;
438
439 dev = nd->dev;
440 if (!dev) {
441 pr_err_once("drop packet: missing net_device\n");
442 return -EIO;
443 }
444
445 if (nd->is_mamac) {
446 if (!PMS_IS_MAMAC(buf, len))
447 return -EIO;
448
449 skb = dev_alloc_skb(len - MDP_HDR_LEN + 2 * ETH_ALEN + 2);
450 } else {
451 if (!PMS_IS_MEP(buf, len))
452 return -EIO;
453
454 skb = dev_alloc_skb(len - MEP_HDR_LEN);
455 }
456
457 if (!skb) {
458 dev->stats.rx_dropped++;
459 pr_err_once("drop packet: no memory for skb\n");
460 goto out;
461 }
462
463 skb->dev = dev;
464
465 if (nd->is_mamac) {
466 /* dest */
467 ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
468
469 /* src */
470 memcpy(skb_put(skb, 4), &zero, 4);
471 memcpy(skb_put(skb, 2), buf + 5, 2);
472
473 /* eth type */
474 memcpy(skb_put(skb, 2), buf + 10, 2);
475
476 buf += MDP_HDR_LEN;
477 len -= MDP_HDR_LEN;
478 } else {
479 buf += MEP_HDR_LEN;
480 len -= MEP_HDR_LEN;
481 }
482
483 memcpy(skb_put(skb, len), buf, len);
484 skb->protocol = eth_type_trans(skb, dev);
485 dev->stats.rx_packets++;
486 dev->stats.rx_bytes += skb->len;
487 netif_rx(skb);
488
489 out:
490 most_put_mbo(mbo);
491 return 0;
492 }
493
494 static struct most_aim aim = {
495 .name = "networking",
496 .probe_channel = aim_probe_channel,
497 .disconnect_channel = aim_disconnect_channel,
498 .tx_completion = aim_resume_tx_channel,
499 .rx_completion = aim_rx_data,
500 };
501
most_net_init(void)502 static int __init most_net_init(void)
503 {
504 pr_info("most_net_init()\n");
505 spin_lock_init(&list_lock);
506 return most_register_aim(&aim);
507 }
508
most_net_exit(void)509 static void __exit most_net_exit(void)
510 {
511 struct net_dev_context *nd, *tmp;
512
513 spin_lock(&list_lock);
514 list_for_each_entry_safe(nd, tmp, &net_devices, list) {
515 list_del(&nd->list);
516 spin_unlock(&list_lock);
517 /*
518 * do not call most_stop_channel() here, because channels are
519 * going to be closed in ndo_stop() after unregister_netdev()
520 */
521 most_net_rm_netdev_safe(nd);
522 kfree(nd);
523 spin_lock(&list_lock);
524 }
525 spin_unlock(&list_lock);
526
527 most_deregister_aim(&aim);
528 pr_info("most_net_exit()\n");
529 }
530
531 /**
532 * most_deliver_netinfo - callback for HDM to be informed about HW's MAC
533 * @param iface - most interface instance
534 * @param link_stat - link status
535 * @param mac_addr - MAC address
536 */
most_deliver_netinfo(struct most_interface * iface,unsigned char link_stat,unsigned char * mac_addr)537 void most_deliver_netinfo(struct most_interface *iface,
538 unsigned char link_stat, unsigned char *mac_addr)
539 {
540 struct net_dev_context *nd;
541 struct net_device *dev;
542
543 pr_info("Received netinfo from %s\n", iface->description);
544
545 nd = get_net_dev_context(iface);
546 if (!nd)
547 return;
548
549 dev = nd->dev;
550 if (!dev)
551 return;
552
553 if (mac_addr)
554 ether_addr_copy(dev->dev_addr, mac_addr);
555
556 if (nd->link_stat != link_stat) {
557 nd->link_stat = link_stat;
558 if (nd->link_stat)
559 netif_wake_queue(dev);
560 else
561 netif_stop_queue(dev);
562 }
563 }
564 EXPORT_SYMBOL(most_deliver_netinfo);
565
566 module_init(most_net_init);
567 module_exit(most_net_exit);
568 MODULE_LICENSE("GPL");
569 MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
570 MODULE_DESCRIPTION("Networking Application Interface Module for MostCore");
571