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