1
2
3
4
5
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 struct list_head net_devices = LIST_HEAD_INIT(net_devices);
71static DEFINE_MUTEX(probe_disc_mt);
72static DEFINE_SPINLOCK(list_lock);
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
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
176
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
393
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
476 ether_addr_copy(skb_put(skb, ETH_ALEN), dev->dev_addr);
477
478
479 skb_put_data(skb, &zero, 4);
480 skb_put_data(skb, buf + 5, 2);
481
482
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
542
543
544
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 ether_addr_copy(dev->dev_addr, 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
