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