linux/net/caif/caif_dev.c
<<
>>
Prefs
   1/*
   2 * CAIF Interface registration.
   3 * Copyright (C) ST-Ericsson AB 2010
   4 * Author:      Sjur Brendeland/sjur.brandeland@stericsson.com
   5 * License terms: GNU General Public License (GPL) version 2
   6 *
   7 * Borrowed heavily from file: pn_dev.c. Thanks to Remi Denis-Courmont
   8 *  and Sakari Ailus <sakari.ailus@nokia.com>
   9 */
  10
  11#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  12
  13#include <linux/kernel.h>
  14#include <linux/if_arp.h>
  15#include <linux/net.h>
  16#include <linux/netdevice.h>
  17#include <linux/mutex.h>
  18#include <linux/module.h>
  19#include <linux/spinlock.h>
  20#include <net/netns/generic.h>
  21#include <net/net_namespace.h>
  22#include <net/pkt_sched.h>
  23#include <net/caif/caif_device.h>
  24#include <net/caif/caif_layer.h>
  25#include <net/caif/cfpkt.h>
  26#include <net/caif/cfcnfg.h>
  27#include <net/caif/cfserl.h>
  28
  29MODULE_LICENSE("GPL");
  30
  31/* Used for local tracking of the CAIF net devices */
  32struct caif_device_entry {
  33        struct cflayer layer;
  34        struct list_head list;
  35        struct net_device *netdev;
  36        int __percpu *pcpu_refcnt;
  37        spinlock_t flow_lock;
  38        struct sk_buff *xoff_skb;
  39        void (*xoff_skb_dtor)(struct sk_buff *skb);
  40        bool xoff;
  41};
  42
  43struct caif_device_entry_list {
  44        struct list_head list;
  45        /* Protects simulanous deletes in list */
  46        struct mutex lock;
  47};
  48
  49struct caif_net {
  50        struct cfcnfg *cfg;
  51        struct caif_device_entry_list caifdevs;
  52};
  53
  54static int caif_net_id;
  55static int q_high = 50; /* Percent */
  56
  57struct cfcnfg *get_cfcnfg(struct net *net)
  58{
  59        struct caif_net *caifn;
  60        caifn = net_generic(net, caif_net_id);
  61        return caifn->cfg;
  62}
  63EXPORT_SYMBOL(get_cfcnfg);
  64
  65static struct caif_device_entry_list *caif_device_list(struct net *net)
  66{
  67        struct caif_net *caifn;
  68        caifn = net_generic(net, caif_net_id);
  69        return &caifn->caifdevs;
  70}
  71
  72static void caifd_put(struct caif_device_entry *e)
  73{
  74        this_cpu_dec(*e->pcpu_refcnt);
  75}
  76
  77static void caifd_hold(struct caif_device_entry *e)
  78{
  79        this_cpu_inc(*e->pcpu_refcnt);
  80}
  81
  82static int caifd_refcnt_read(struct caif_device_entry *e)
  83{
  84        int i, refcnt = 0;
  85        for_each_possible_cpu(i)
  86                refcnt += *per_cpu_ptr(e->pcpu_refcnt, i);
  87        return refcnt;
  88}
  89
  90/* Allocate new CAIF device. */
  91static struct caif_device_entry *caif_device_alloc(struct net_device *dev)
  92{
  93        struct caif_device_entry *caifd;
  94
  95        caifd = kzalloc(sizeof(*caifd), GFP_KERNEL);
  96        if (!caifd)
  97                return NULL;
  98        caifd->pcpu_refcnt = alloc_percpu(int);
  99        if (!caifd->pcpu_refcnt) {
 100                kfree(caifd);
 101                return NULL;
 102        }
 103        caifd->netdev = dev;
 104        dev_hold(dev);
 105        return caifd;
 106}
 107
 108static struct caif_device_entry *caif_get(struct net_device *dev)
 109{
 110        struct caif_device_entry_list *caifdevs =
 111            caif_device_list(dev_net(dev));
 112        struct caif_device_entry *caifd;
 113
 114        list_for_each_entry_rcu(caifd, &caifdevs->list, list) {
 115                if (caifd->netdev == dev)
 116                        return caifd;
 117        }
 118        return NULL;
 119}
 120
 121void caif_flow_cb(struct sk_buff *skb)
 122{
 123        struct caif_device_entry *caifd;
 124        void (*dtor)(struct sk_buff *skb) = NULL;
 125        bool send_xoff;
 126
 127        WARN_ON(skb->dev == NULL);
 128
 129        rcu_read_lock();
 130        caifd = caif_get(skb->dev);
 131
 132        WARN_ON(caifd == NULL);
 133        if (caifd == NULL)
 134                return;
 135
 136        caifd_hold(caifd);
 137        rcu_read_unlock();
 138
 139        spin_lock_bh(&caifd->flow_lock);
 140        send_xoff = caifd->xoff;
 141        caifd->xoff = 0;
 142        dtor = caifd->xoff_skb_dtor;
 143
 144        if (WARN_ON(caifd->xoff_skb != skb))
 145                skb = NULL;
 146
 147        caifd->xoff_skb = NULL;
 148        caifd->xoff_skb_dtor = NULL;
 149
 150        spin_unlock_bh(&caifd->flow_lock);
 151
 152        if (dtor && skb)
 153                dtor(skb);
 154
 155        if (send_xoff)
 156                caifd->layer.up->
 157                        ctrlcmd(caifd->layer.up,
 158                                _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND,
 159                                caifd->layer.id);
 160        caifd_put(caifd);
 161}
 162
 163static int transmit(struct cflayer *layer, struct cfpkt *pkt)
 164{
 165        int err, high = 0, qlen = 0;
 166        struct caif_device_entry *caifd =
 167            container_of(layer, struct caif_device_entry, layer);
 168        struct sk_buff *skb;
 169        struct netdev_queue *txq;
 170
 171        rcu_read_lock_bh();
 172
 173        skb = cfpkt_tonative(pkt);
 174        skb->dev = caifd->netdev;
 175        skb_reset_network_header(skb);
 176        skb->protocol = htons(ETH_P_CAIF);
 177
 178        /* Check if we need to handle xoff */
 179        if (likely(caifd->netdev->tx_queue_len == 0))
 180                goto noxoff;
 181
 182        if (unlikely(caifd->xoff))
 183                goto noxoff;
 184
 185        if (likely(!netif_queue_stopped(caifd->netdev))) {
 186                /* If we run with a TX queue, check if the queue is too long*/
 187                txq = netdev_get_tx_queue(skb->dev, 0);
 188                qlen = qdisc_qlen(rcu_dereference_bh(txq->qdisc));
 189
 190                if (likely(qlen == 0))
 191                        goto noxoff;
 192
 193                high = (caifd->netdev->tx_queue_len * q_high) / 100;
 194                if (likely(qlen < high))
 195                        goto noxoff;
 196        }
 197
 198        /* Hold lock while accessing xoff */
 199        spin_lock_bh(&caifd->flow_lock);
 200        if (caifd->xoff) {
 201                spin_unlock_bh(&caifd->flow_lock);
 202                goto noxoff;
 203        }
 204
 205        /*
 206         * Handle flow off, we do this by temporary hi-jacking this
 207         * skb's destructor function, and replace it with our own
 208         * flow-on callback. The callback will set flow-on and call
 209         * the original destructor.
 210         */
 211
 212        pr_debug("queue has stopped(%d) or is full (%d > %d)\n",
 213                        netif_queue_stopped(caifd->netdev),
 214                        qlen, high);
 215        caifd->xoff = 1;
 216        caifd->xoff_skb = skb;
 217        caifd->xoff_skb_dtor = skb->destructor;
 218        skb->destructor = caif_flow_cb;
 219        spin_unlock_bh(&caifd->flow_lock);
 220
 221        caifd->layer.up->ctrlcmd(caifd->layer.up,
 222                                        _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND,
 223                                        caifd->layer.id);
 224noxoff:
 225        rcu_read_unlock_bh();
 226
 227        err = dev_queue_xmit(skb);
 228        if (err > 0)
 229                err = -EIO;
 230
 231        return err;
 232}
 233
 234/*
 235 * Stuff received packets into the CAIF stack.
 236 * On error, returns non-zero and releases the skb.
 237 */
 238static int receive(struct sk_buff *skb, struct net_device *dev,
 239                   struct packet_type *pkttype, struct net_device *orig_dev)
 240{
 241        struct cfpkt *pkt;
 242        struct caif_device_entry *caifd;
 243        int err;
 244
 245        pkt = cfpkt_fromnative(CAIF_DIR_IN, skb);
 246
 247        rcu_read_lock();
 248        caifd = caif_get(dev);
 249
 250        if (!caifd || !caifd->layer.up || !caifd->layer.up->receive ||
 251                        !netif_oper_up(caifd->netdev)) {
 252                rcu_read_unlock();
 253                kfree_skb(skb);
 254                return NET_RX_DROP;
 255        }
 256
 257        /* Hold reference to netdevice while using CAIF stack */
 258        caifd_hold(caifd);
 259        rcu_read_unlock();
 260
 261        err = caifd->layer.up->receive(caifd->layer.up, pkt);
 262
 263        /* For -EILSEQ the packet is not freed so so it now */
 264        if (err == -EILSEQ)
 265                cfpkt_destroy(pkt);
 266
 267        /* Release reference to stack upwards */
 268        caifd_put(caifd);
 269
 270        if (err != 0)
 271                err = NET_RX_DROP;
 272        return err;
 273}
 274
 275static struct packet_type caif_packet_type __read_mostly = {
 276        .type = cpu_to_be16(ETH_P_CAIF),
 277        .func = receive,
 278};
 279
 280static void dev_flowctrl(struct net_device *dev, int on)
 281{
 282        struct caif_device_entry *caifd;
 283
 284        rcu_read_lock();
 285
 286        caifd = caif_get(dev);
 287        if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) {
 288                rcu_read_unlock();
 289                return;
 290        }
 291
 292        caifd_hold(caifd);
 293        rcu_read_unlock();
 294
 295        caifd->layer.up->ctrlcmd(caifd->layer.up,
 296                                 on ?
 297                                 _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND :
 298                                 _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND,
 299                                 caifd->layer.id);
 300        caifd_put(caifd);
 301}
 302
 303void caif_enroll_dev(struct net_device *dev, struct caif_dev_common *caifdev,
 304                        struct cflayer *link_support, int head_room,
 305                        struct cflayer **layer, int (**rcv_func)(
 306                                struct sk_buff *, struct net_device *,
 307                                struct packet_type *, struct net_device *))
 308{
 309        struct caif_device_entry *caifd;
 310        enum cfcnfg_phy_preference pref;
 311        struct cfcnfg *cfg = get_cfcnfg(dev_net(dev));
 312        struct caif_device_entry_list *caifdevs;
 313
 314        caifdevs = caif_device_list(dev_net(dev));
 315        caifd = caif_device_alloc(dev);
 316        if (!caifd)
 317                return;
 318        *layer = &caifd->layer;
 319        spin_lock_init(&caifd->flow_lock);
 320
 321        switch (caifdev->link_select) {
 322        case CAIF_LINK_HIGH_BANDW:
 323                pref = CFPHYPREF_HIGH_BW;
 324                break;
 325        case CAIF_LINK_LOW_LATENCY:
 326                pref = CFPHYPREF_LOW_LAT;
 327                break;
 328        default:
 329                pref = CFPHYPREF_HIGH_BW;
 330                break;
 331        }
 332        mutex_lock(&caifdevs->lock);
 333        list_add_rcu(&caifd->list, &caifdevs->list);
 334
 335        strncpy(caifd->layer.name, dev->name,
 336                sizeof(caifd->layer.name) - 1);
 337        caifd->layer.name[sizeof(caifd->layer.name) - 1] = 0;
 338        caifd->layer.transmit = transmit;
 339        cfcnfg_add_phy_layer(cfg,
 340                                dev,
 341                                &caifd->layer,
 342                                pref,
 343                                link_support,
 344                                caifdev->use_fcs,
 345                                head_room);
 346        mutex_unlock(&caifdevs->lock);
 347        if (rcv_func)
 348                *rcv_func = receive;
 349}
 350EXPORT_SYMBOL(caif_enroll_dev);
 351
 352/* notify Caif of device events */
 353static int caif_device_notify(struct notifier_block *me, unsigned long what,
 354                              void *arg)
 355{
 356        struct net_device *dev = arg;
 357        struct caif_device_entry *caifd = NULL;
 358        struct caif_dev_common *caifdev;
 359        struct cfcnfg *cfg;
 360        struct cflayer *layer, *link_support;
 361        int head_room = 0;
 362        struct caif_device_entry_list *caifdevs;
 363
 364        cfg = get_cfcnfg(dev_net(dev));
 365        caifdevs = caif_device_list(dev_net(dev));
 366
 367        caifd = caif_get(dev);
 368        if (caifd == NULL && dev->type != ARPHRD_CAIF)
 369                return 0;
 370
 371        switch (what) {
 372        case NETDEV_REGISTER:
 373                if (caifd != NULL)
 374                        break;
 375
 376                caifdev = netdev_priv(dev);
 377
 378                link_support = NULL;
 379                if (caifdev->use_frag) {
 380                        head_room = 1;
 381                        link_support = cfserl_create(dev->ifindex,
 382                                                        caifdev->use_stx);
 383                        if (!link_support) {
 384                                pr_warn("Out of memory\n");
 385                                break;
 386                        }
 387                }
 388                caif_enroll_dev(dev, caifdev, link_support, head_room,
 389                                &layer, NULL);
 390                caifdev->flowctrl = dev_flowctrl;
 391                break;
 392
 393        case NETDEV_UP:
 394                rcu_read_lock();
 395
 396                caifd = caif_get(dev);
 397                if (caifd == NULL) {
 398                        rcu_read_unlock();
 399                        break;
 400                }
 401
 402                caifd->xoff = 0;
 403                cfcnfg_set_phy_state(cfg, &caifd->layer, true);
 404                rcu_read_unlock();
 405
 406                break;
 407
 408        case NETDEV_DOWN:
 409                rcu_read_lock();
 410
 411                caifd = caif_get(dev);
 412                if (!caifd || !caifd->layer.up || !caifd->layer.up->ctrlcmd) {
 413                        rcu_read_unlock();
 414                        return -EINVAL;
 415                }
 416
 417                cfcnfg_set_phy_state(cfg, &caifd->layer, false);
 418                caifd_hold(caifd);
 419                rcu_read_unlock();
 420
 421                caifd->layer.up->ctrlcmd(caifd->layer.up,
 422                                         _CAIF_CTRLCMD_PHYIF_DOWN_IND,
 423                                         caifd->layer.id);
 424
 425                spin_lock_bh(&caifd->flow_lock);
 426
 427                /*
 428                 * Replace our xoff-destructor with original destructor.
 429                 * We trust that skb->destructor *always* is called before
 430                 * the skb reference is invalid. The hijacked SKB destructor
 431                 * takes the flow_lock so manipulating the skb->destructor here
 432                 * should be safe.
 433                */
 434                if (caifd->xoff_skb_dtor != NULL && caifd->xoff_skb != NULL)
 435                        caifd->xoff_skb->destructor = caifd->xoff_skb_dtor;
 436
 437                caifd->xoff = 0;
 438                caifd->xoff_skb_dtor = NULL;
 439                caifd->xoff_skb = NULL;
 440
 441                spin_unlock_bh(&caifd->flow_lock);
 442                caifd_put(caifd);
 443                break;
 444
 445        case NETDEV_UNREGISTER:
 446                mutex_lock(&caifdevs->lock);
 447
 448                caifd = caif_get(dev);
 449                if (caifd == NULL) {
 450                        mutex_unlock(&caifdevs->lock);
 451                        break;
 452                }
 453                list_del_rcu(&caifd->list);
 454
 455                /*
 456                 * NETDEV_UNREGISTER is called repeatedly until all reference
 457                 * counts for the net-device are released. If references to
 458                 * caifd is taken, simply ignore NETDEV_UNREGISTER and wait for
 459                 * the next call to NETDEV_UNREGISTER.
 460                 *
 461                 * If any packets are in flight down the CAIF Stack,
 462                 * cfcnfg_del_phy_layer will return nonzero.
 463                 * If no packets are in flight, the CAIF Stack associated
 464                 * with the net-device un-registering is freed.
 465                 */
 466
 467                if (caifd_refcnt_read(caifd) != 0 ||
 468                        cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0) {
 469
 470                        pr_info("Wait for device inuse\n");
 471                        /* Enrole device if CAIF Stack is still in use */
 472                        list_add_rcu(&caifd->list, &caifdevs->list);
 473                        mutex_unlock(&caifdevs->lock);
 474                        break;
 475                }
 476
 477                synchronize_rcu();
 478                dev_put(caifd->netdev);
 479                free_percpu(caifd->pcpu_refcnt);
 480                kfree(caifd);
 481
 482                mutex_unlock(&caifdevs->lock);
 483                break;
 484        }
 485        return 0;
 486}
 487
 488static struct notifier_block caif_device_notifier = {
 489        .notifier_call = caif_device_notify,
 490        .priority = 0,
 491};
 492
 493/* Per-namespace Caif devices handling */
 494static int caif_init_net(struct net *net)
 495{
 496        struct caif_net *caifn = net_generic(net, caif_net_id);
 497        INIT_LIST_HEAD(&caifn->caifdevs.list);
 498        mutex_init(&caifn->caifdevs.lock);
 499
 500        caifn->cfg = cfcnfg_create();
 501        if (!caifn->cfg)
 502                return -ENOMEM;
 503
 504        return 0;
 505}
 506
 507static void caif_exit_net(struct net *net)
 508{
 509        struct caif_device_entry *caifd, *tmp;
 510        struct caif_device_entry_list *caifdevs =
 511            caif_device_list(net);
 512        struct cfcnfg *cfg =  get_cfcnfg(net);
 513
 514        rtnl_lock();
 515        mutex_lock(&caifdevs->lock);
 516
 517        list_for_each_entry_safe(caifd, tmp, &caifdevs->list, list) {
 518                int i = 0;
 519                list_del_rcu(&caifd->list);
 520                cfcnfg_set_phy_state(cfg, &caifd->layer, false);
 521
 522                while (i < 10 &&
 523                        (caifd_refcnt_read(caifd) != 0 ||
 524                        cfcnfg_del_phy_layer(cfg, &caifd->layer) != 0)) {
 525
 526                        pr_info("Wait for device inuse\n");
 527                        msleep(250);
 528                        i++;
 529                }
 530                synchronize_rcu();
 531                dev_put(caifd->netdev);
 532                free_percpu(caifd->pcpu_refcnt);
 533                kfree(caifd);
 534        }
 535        cfcnfg_remove(cfg);
 536
 537        mutex_unlock(&caifdevs->lock);
 538        rtnl_unlock();
 539}
 540
 541static struct pernet_operations caif_net_ops = {
 542        .init = caif_init_net,
 543        .exit = caif_exit_net,
 544        .id   = &caif_net_id,
 545        .size = sizeof(struct caif_net),
 546};
 547
 548/* Initialize Caif devices list */
 549static int __init caif_device_init(void)
 550{
 551        int result;
 552
 553        result = register_pernet_subsys(&caif_net_ops);
 554
 555        if (result)
 556                return result;
 557
 558        register_netdevice_notifier(&caif_device_notifier);
 559        dev_add_pack(&caif_packet_type);
 560
 561        return result;
 562}
 563
 564static void __exit caif_device_exit(void)
 565{
 566        unregister_netdevice_notifier(&caif_device_notifier);
 567        dev_remove_pack(&caif_packet_type);
 568        unregister_pernet_subsys(&caif_net_ops);
 569}
 570
 571module_init(caif_device_init);
 572module_exit(caif_device_exit);
 573