drivers/staging/most/net/net.c

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