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