drivers/staging/most/aim-network/networking.c

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