drivers/staging/most/net/net.c

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