net/ncsi/ncsi-manage.c

   1 /*
   2  * Copyright Gavin Shan, IBM Corporation 2016.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #include <linux/module.h>
  11 #include <linux/kernel.h>
  12 #include <linux/init.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/skbuff.h>
  15
  16 #include <net/ncsi.h>
  17 #include <net/net_namespace.h>
  18 #include <net/sock.h>
  19 #include <net/addrconf.h>
  20 #include <net/ipv6.h>
  21 #include <net/if_inet6.h>
  22
  23 #include "internal.h"
  24 #include "ncsi-pkt.h"
  25 #include "ncsi-netlink.h"
  26
  27 LIST_HEAD(ncsi_dev_list);
  28 DEFINE_SPINLOCK(ncsi_dev_lock);
  29
  30 static void ncsi_report_link(struct ncsi_dev_priv *ndp, bool force_down)
  31 {
  32         struct ncsi_dev *nd = &ndp->ndev;
  33         struct ncsi_package *np;
  34         struct ncsi_channel *nc;
  35         unsigned long flags;
  36
  37         nd->state = ncsi_dev_state_functional;
  38         if (force_down) {
  39                 nd->link_up = 0;
  40                 goto report;
  41         }
  42
  43         nd->link_up = 0;
  44         NCSI_FOR_EACH_PACKAGE(ndp, np) {
  45                 NCSI_FOR_EACH_CHANNEL(np, nc) {
  46                         spin_lock_irqsave(&nc->lock, flags);
  47
  48                         if (!list_empty(&nc->link) ||
  49                             nc->state != NCSI_CHANNEL_ACTIVE) {
  50                                 spin_unlock_irqrestore(&nc->lock, flags);
  51                                 continue;
  52                         }
  53
  54                         if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {
  55                                 spin_unlock_irqrestore(&nc->lock, flags);
  56                                 nd->link_up = 1;
  57                                 goto report;
  58                         }
  59
  60                         spin_unlock_irqrestore(&nc->lock, flags);
  61                 }
  62         }
  63
  64 report:
  65         nd->handler(nd);
  66 }
  67
  68 static void ncsi_channel_monitor(struct timer_list *t)
  69 {
  70         struct ncsi_channel *nc = from_timer(nc, t, monitor.timer);
  71         struct ncsi_package *np = nc->package;
  72         struct ncsi_dev_priv *ndp = np->ndp;
  73         struct ncsi_channel_mode *ncm;
  74         struct ncsi_cmd_arg nca;
  75         bool enabled, chained;
  76         unsigned int monitor_state;
  77         unsigned long flags;
  78         int state, ret;
  79
  80         spin_lock_irqsave(&nc->lock, flags);
  81         state = nc->state;
  82         chained = !list_empty(&nc->link);
  83         enabled = nc->monitor.enabled;
  84         monitor_state = nc->monitor.state;
  85         spin_unlock_irqrestore(&nc->lock, flags);
  86
  87         if (!enabled || chained) {
  88                 ncsi_stop_channel_monitor(nc);
  89                 return;
  90         }
  91         if (state != NCSI_CHANNEL_INACTIVE &&
  92             state != NCSI_CHANNEL_ACTIVE) {
  93                 ncsi_stop_channel_monitor(nc);
  94                 return;
  95         }
  96
  97         switch (monitor_state) {
  98         case NCSI_CHANNEL_MONITOR_START:
  99         case NCSI_CHANNEL_MONITOR_RETRY:
 100                 nca.ndp = ndp;
 101                 nca.package = np->id;
 102                 nca.channel = nc->id;
 103                 nca.type = NCSI_PKT_CMD_GLS;
 104                 nca.req_flags = 0;
 105                 ret = ncsi_xmit_cmd(&nca);
 106                 if (ret)
 107                         netdev_err(ndp->ndev.dev, "Error %d sending GLS\n",
 108                                    ret);
 109                 break;
 110         case NCSI_CHANNEL_MONITOR_WAIT ... NCSI_CHANNEL_MONITOR_WAIT_MAX:
 111                 break;
 112         default:
 113                 netdev_err(ndp->ndev.dev, "NCSI Channel %d timed out!\n",
 114                            nc->id);
 115                 if (!(ndp->flags & NCSI_DEV_HWA)) {
 116                         ncsi_report_link(ndp, true);
 117                         ndp->flags |= NCSI_DEV_RESHUFFLE;
 118                 }
 119
 120                 ncsi_stop_channel_monitor(nc);
 121
 122                 ncm = &nc->modes[NCSI_MODE_LINK];
 123                 spin_lock_irqsave(&nc->lock, flags);
 124                 nc->state = NCSI_CHANNEL_INVISIBLE;
 125                 ncm->data[2] &= ~0x1;
 126                 spin_unlock_irqrestore(&nc->lock, flags);
 127
 128                 spin_lock_irqsave(&ndp->lock, flags);
 129                 nc->state = NCSI_CHANNEL_ACTIVE;
 130                 list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 131                 spin_unlock_irqrestore(&ndp->lock, flags);
 132                 ncsi_process_next_channel(ndp);
 133                 return;
 134         }
 135
 136         spin_lock_irqsave(&nc->lock, flags);
 137         nc->monitor.state++;
 138         spin_unlock_irqrestore(&nc->lock, flags);
 139         mod_timer(&nc->monitor.timer, jiffies + HZ);
 140 }
 141
 142 void ncsi_start_channel_monitor(struct ncsi_channel *nc)
 143 {
 144         unsigned long flags;
 145
 146         spin_lock_irqsave(&nc->lock, flags);
 147         WARN_ON_ONCE(nc->monitor.enabled);
 148         nc->monitor.enabled = true;
 149         nc->monitor.state = NCSI_CHANNEL_MONITOR_START;
 150         spin_unlock_irqrestore(&nc->lock, flags);
 151
 152         mod_timer(&nc->monitor.timer, jiffies + HZ);
 153 }
 154
 155 void ncsi_stop_channel_monitor(struct ncsi_channel *nc)
 156 {
 157         unsigned long flags;
 158
 159         spin_lock_irqsave(&nc->lock, flags);
 160         if (!nc->monitor.enabled) {
 161                 spin_unlock_irqrestore(&nc->lock, flags);
 162                 return;
 163         }
 164         nc->monitor.enabled = false;
 165         spin_unlock_irqrestore(&nc->lock, flags);
 166
 167         del_timer_sync(&nc->monitor.timer);
 168 }
 169
 170 struct ncsi_channel *ncsi_find_channel(struct ncsi_package *np,
 171                                        unsigned char id)
 172 {
 173         struct ncsi_channel *nc;
 174
 175         NCSI_FOR_EACH_CHANNEL(np, nc) {
 176                 if (nc->id == id)
 177                         return nc;
 178         }
 179
 180         return NULL;
 181 }
 182
 183 struct ncsi_channel *ncsi_add_channel(struct ncsi_package *np, unsigned char id)
 184 {
 185         struct ncsi_channel *nc, *tmp;
 186         int index;
 187         unsigned long flags;
 188
 189         nc = kzalloc(sizeof(*nc), GFP_ATOMIC);
 190         if (!nc)
 191                 return NULL;
 192
 193         nc->id = id;
 194         nc->package = np;
 195         nc->state = NCSI_CHANNEL_INACTIVE;
 196         nc->monitor.enabled = false;
 197         timer_setup(&nc->monitor.timer, ncsi_channel_monitor, 0);
 198         spin_lock_init(&nc->lock);
 199         INIT_LIST_HEAD(&nc->link);
 200         for (index = 0; index < NCSI_CAP_MAX; index++)
 201                 nc->caps[index].index = index;
 202         for (index = 0; index < NCSI_MODE_MAX; index++)
 203                 nc->modes[index].index = index;
 204
 205         spin_lock_irqsave(&np->lock, flags);
 206         tmp = ncsi_find_channel(np, id);
 207         if (tmp) {
 208                 spin_unlock_irqrestore(&np->lock, flags);
 209                 kfree(nc);
 210                 return tmp;
 211         }
 212
 213         list_add_tail_rcu(&nc->node, &np->channels);
 214         np->channel_num++;
 215         spin_unlock_irqrestore(&np->lock, flags);
 216
 217         return nc;
 218 }
 219
 220 static void ncsi_remove_channel(struct ncsi_channel *nc)
 221 {
 222         struct ncsi_package *np = nc->package;
 223         unsigned long flags;
 224
 225         spin_lock_irqsave(&nc->lock, flags);
 226
 227         /* Release filters */
 228         kfree(nc->mac_filter.addrs);
 229         kfree(nc->vlan_filter.vids);
 230
 231         nc->state = NCSI_CHANNEL_INACTIVE;
 232         spin_unlock_irqrestore(&nc->lock, flags);
 233         ncsi_stop_channel_monitor(nc);
 234
 235         /* Remove and free channel */
 236         spin_lock_irqsave(&np->lock, flags);
 237         list_del_rcu(&nc->node);
 238         np->channel_num--;
 239         spin_unlock_irqrestore(&np->lock, flags);
 240
 241         kfree(nc);
 242 }
 243
 244 struct ncsi_package *ncsi_find_package(struct ncsi_dev_priv *ndp,
 245                                        unsigned char id)
 246 {
 247         struct ncsi_package *np;
 248
 249         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 250                 if (np->id == id)
 251                         return np;
 252         }
 253
 254         return NULL;
 255 }
 256
 257 struct ncsi_package *ncsi_add_package(struct ncsi_dev_priv *ndp,
 258                                       unsigned char id)
 259 {
 260         struct ncsi_package *np, *tmp;
 261         unsigned long flags;
 262
 263         np = kzalloc(sizeof(*np), GFP_ATOMIC);
 264         if (!np)
 265                 return NULL;
 266
 267         np->id = id;
 268         np->ndp = ndp;
 269         spin_lock_init(&np->lock);
 270         INIT_LIST_HEAD(&np->channels);
 271
 272         spin_lock_irqsave(&ndp->lock, flags);
 273         tmp = ncsi_find_package(ndp, id);
 274         if (tmp) {
 275                 spin_unlock_irqrestore(&ndp->lock, flags);
 276                 kfree(np);
 277                 return tmp;
 278         }
 279
 280         list_add_tail_rcu(&np->node, &ndp->packages);
 281         ndp->package_num++;
 282         spin_unlock_irqrestore(&ndp->lock, flags);
 283
 284         return np;
 285 }
 286
 287 void ncsi_remove_package(struct ncsi_package *np)
 288 {
 289         struct ncsi_dev_priv *ndp = np->ndp;
 290         struct ncsi_channel *nc, *tmp;
 291         unsigned long flags;
 292
 293         /* Release all child channels */
 294         list_for_each_entry_safe(nc, tmp, &np->channels, node)
 295                 ncsi_remove_channel(nc);
 296
 297         /* Remove and free package */
 298         spin_lock_irqsave(&ndp->lock, flags);
 299         list_del_rcu(&np->node);
 300         ndp->package_num--;
 301         spin_unlock_irqrestore(&ndp->lock, flags);
 302
 303         kfree(np);
 304 }
 305
 306 void ncsi_find_package_and_channel(struct ncsi_dev_priv *ndp,
 307                                    unsigned char id,
 308                                    struct ncsi_package **np,
 309                                    struct ncsi_channel **nc)
 310 {
 311         struct ncsi_package *p;
 312         struct ncsi_channel *c;
 313
 314         p = ncsi_find_package(ndp, NCSI_PACKAGE_INDEX(id));
 315         c = p ? ncsi_find_channel(p, NCSI_CHANNEL_INDEX(id)) : NULL;
 316
 317         if (np)
 318                 *np = p;
 319         if (nc)
 320                 *nc = c;
 321 }
 322
 323 /* For two consecutive NCSI commands, the packet IDs shouldn't
 324  * be same. Otherwise, the bogus response might be replied. So
 325  * the available IDs are allocated in round-robin fashion.
 326  */
 327 struct ncsi_request *ncsi_alloc_request(struct ncsi_dev_priv *ndp,
 328                                         unsigned int req_flags)
 329 {
 330         struct ncsi_request *nr = NULL;
 331         int i, limit = ARRAY_SIZE(ndp->requests);
 332         unsigned long flags;
 333
 334         /* Check if there is one available request until the ceiling */
 335         spin_lock_irqsave(&ndp->lock, flags);
 336         for (i = ndp->request_id; i < limit; i++) {
 337                 if (ndp->requests[i].used)
 338                         continue;
 339
 340                 nr = &ndp->requests[i];
 341                 nr->used = true;
 342                 nr->flags = req_flags;
 343                 ndp->request_id = i + 1;
 344                 goto found;
 345         }
 346
 347         /* Fail back to check from the starting cursor */
 348         for (i = NCSI_REQ_START_IDX; i < ndp->request_id; i++) {
 349                 if (ndp->requests[i].used)
 350                         continue;
 351
 352                 nr = &ndp->requests[i];
 353                 nr->used = true;
 354                 nr->flags = req_flags;
 355                 ndp->request_id = i + 1;
 356                 goto found;
 357         }
 358
 359 found:
 360         spin_unlock_irqrestore(&ndp->lock, flags);
 361         return nr;
 362 }
 363
 364 void ncsi_free_request(struct ncsi_request *nr)
 365 {
 366         struct ncsi_dev_priv *ndp = nr->ndp;
 367         struct sk_buff *cmd, *rsp;
 368         unsigned long flags;
 369         bool driven;
 370
 371         if (nr->enabled) {
 372                 nr->enabled = false;
 373                 del_timer_sync(&nr->timer);
 374         }
 375
 376         spin_lock_irqsave(&ndp->lock, flags);
 377         cmd = nr->cmd;
 378         rsp = nr->rsp;
 379         nr->cmd = NULL;
 380         nr->rsp = NULL;
 381         nr->used = false;
 382         driven = !!(nr->flags & NCSI_REQ_FLAG_EVENT_DRIVEN);
 383         spin_unlock_irqrestore(&ndp->lock, flags);
 384
 385         if (driven && cmd && --ndp->pending_req_num == 0)
 386                 schedule_work(&ndp->work);
 387
 388         /* Release command and response */
 389         consume_skb(cmd);
 390         consume_skb(rsp);
 391 }
 392
 393 struct ncsi_dev *ncsi_find_dev(struct net_device *dev)
 394 {
 395         struct ncsi_dev_priv *ndp;
 396
 397         NCSI_FOR_EACH_DEV(ndp) {
 398                 if (ndp->ndev.dev == dev)
 399                         return &ndp->ndev;
 400         }
 401
 402         return NULL;
 403 }
 404
 405 static void ncsi_request_timeout(struct timer_list *t)
 406 {
 407         struct ncsi_request *nr = from_timer(nr, t, timer);
 408         struct ncsi_dev_priv *ndp = nr->ndp;
 409         unsigned long flags;
 410
 411         /* If the request already had associated response,
 412          * let the response handler to release it.
 413          */
 414         spin_lock_irqsave(&ndp->lock, flags);
 415         nr->enabled = false;
 416         if (nr->rsp || !nr->cmd) {
 417                 spin_unlock_irqrestore(&ndp->lock, flags);
 418                 return;
 419         }
 420         spin_unlock_irqrestore(&ndp->lock, flags);
 421
 422         /* Release the request */
 423         ncsi_free_request(nr);
 424 }
 425
 426 static void ncsi_suspend_channel(struct ncsi_dev_priv *ndp)
 427 {
 428         struct ncsi_dev *nd = &ndp->ndev;
 429         struct ncsi_package *np = ndp->active_package;
 430         struct ncsi_channel *nc = ndp->active_channel;
 431         struct ncsi_cmd_arg nca;
 432         unsigned long flags;
 433         int ret;
 434
 435         nca.ndp = ndp;
 436         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 437         switch (nd->state) {
 438         case ncsi_dev_state_suspend:
 439                 nd->state = ncsi_dev_state_suspend_select;
 440                 /* Fall through */
 441         case ncsi_dev_state_suspend_select:
 442                 ndp->pending_req_num = 1;
 443
 444                 nca.type = NCSI_PKT_CMD_SP;
 445                 nca.package = np->id;
 446                 nca.channel = NCSI_RESERVED_CHANNEL;
 447                 if (ndp->flags & NCSI_DEV_HWA)
 448                         nca.bytes[0] = 0;
 449                 else
 450                         nca.bytes[0] = 1;
 451
 452                 /* To retrieve the last link states of channels in current
 453                  * package when current active channel needs fail over to
 454                  * another one. It means we will possibly select another
 455                  * channel as next active one. The link states of channels
 456                  * are most important factor of the selection. So we need
 457                  * accurate link states. Unfortunately, the link states on
 458                  * inactive channels can't be updated with LSC AEN in time.
 459                  */
 460                 if (ndp->flags & NCSI_DEV_RESHUFFLE)
 461                         nd->state = ncsi_dev_state_suspend_gls;
 462                 else
 463                         nd->state = ncsi_dev_state_suspend_dcnt;
 464                 ret = ncsi_xmit_cmd(&nca);
 465                 if (ret)
 466                         goto error;
 467
 468                 break;
 469         case ncsi_dev_state_suspend_gls:
 470                 ndp->pending_req_num = np->channel_num;
 471
 472                 nca.type = NCSI_PKT_CMD_GLS;
 473                 nca.package = np->id;
 474
 475                 nd->state = ncsi_dev_state_suspend_dcnt;
 476                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 477                         nca.channel = nc->id;
 478                         ret = ncsi_xmit_cmd(&nca);
 479                         if (ret)
 480                                 goto error;
 481                 }
 482
 483                 break;
 484         case ncsi_dev_state_suspend_dcnt:
 485                 ndp->pending_req_num = 1;
 486
 487                 nca.type = NCSI_PKT_CMD_DCNT;
 488                 nca.package = np->id;
 489                 nca.channel = nc->id;
 490
 491                 nd->state = ncsi_dev_state_suspend_dc;
 492                 ret = ncsi_xmit_cmd(&nca);
 493                 if (ret)
 494                         goto error;
 495
 496                 break;
 497         case ncsi_dev_state_suspend_dc:
 498                 ndp->pending_req_num = 1;
 499
 500                 nca.type = NCSI_PKT_CMD_DC;
 501                 nca.package = np->id;
 502                 nca.channel = nc->id;
 503                 nca.bytes[0] = 1;
 504
 505                 nd->state = ncsi_dev_state_suspend_deselect;
 506                 ret = ncsi_xmit_cmd(&nca);
 507                 if (ret)
 508                         goto error;
 509
 510                 break;
 511         case ncsi_dev_state_suspend_deselect:
 512                 ndp->pending_req_num = 1;
 513
 514                 nca.type = NCSI_PKT_CMD_DP;
 515                 nca.package = np->id;
 516                 nca.channel = NCSI_RESERVED_CHANNEL;
 517
 518                 nd->state = ncsi_dev_state_suspend_done;
 519                 ret = ncsi_xmit_cmd(&nca);
 520                 if (ret)
 521                         goto error;
 522
 523                 break;
 524         case ncsi_dev_state_suspend_done:
 525                 spin_lock_irqsave(&nc->lock, flags);
 526                 nc->state = NCSI_CHANNEL_INACTIVE;
 527                 spin_unlock_irqrestore(&nc->lock, flags);
 528                 ncsi_process_next_channel(ndp);
 529
 530                 break;
 531         default:
 532                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in suspend\n",
 533                             nd->state);
 534         }
 535
 536         return;
 537 error:
 538         nd->state = ncsi_dev_state_functional;
 539 }
 540
 541 /* Check the VLAN filter bitmap for a set filter, and construct a
 542  * "Set VLAN Filter - Disable" packet if found.
 543  */
 544 static int clear_one_vid(struct ncsi_dev_priv *ndp, struct ncsi_channel *nc,
 545                          struct ncsi_cmd_arg *nca)
 546 {
 547         struct ncsi_channel_vlan_filter *ncf;
 548         unsigned long flags;
 549         void *bitmap;
 550         int index;
 551         u16 vid;
 552
 553         ncf = &nc->vlan_filter;
 554         bitmap = &ncf->bitmap;
 555
 556         spin_lock_irqsave(&nc->lock, flags);
 557         index = find_next_bit(bitmap, ncf->n_vids, 0);
 558         if (index >= ncf->n_vids) {
 559                 spin_unlock_irqrestore(&nc->lock, flags);
 560                 return -1;
 561         }
 562         vid = ncf->vids[index];
 563
 564         clear_bit(index, bitmap);
 565         ncf->vids[index] = 0;
 566         spin_unlock_irqrestore(&nc->lock, flags);
 567
 568         nca->type = NCSI_PKT_CMD_SVF;
 569         nca->words[1] = vid;
 570         /* HW filter index starts at 1 */
 571         nca->bytes[6] = index + 1;
 572         nca->bytes[7] = 0x00;
 573         return 0;
 574 }
 575
 576 /* Find an outstanding VLAN tag and constuct a "Set VLAN Filter - Enable"
 577  * packet.
 578  */
 579 static int set_one_vid(struct ncsi_dev_priv *ndp, struct ncsi_channel *nc,
 580                        struct ncsi_cmd_arg *nca)
 581 {
 582         struct ncsi_channel_vlan_filter *ncf;
 583         struct vlan_vid *vlan = NULL;
 584         unsigned long flags;
 585         int i, index;
 586         void *bitmap;
 587         u16 vid;
 588
 589         if (list_empty(&ndp->vlan_vids))
 590                 return -1;
 591
 592         ncf = &nc->vlan_filter;
 593         bitmap = &ncf->bitmap;
 594
 595         spin_lock_irqsave(&nc->lock, flags);
 596
 597         rcu_read_lock();
 598         list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
 599                 vid = vlan->vid;
 600                 for (i = 0; i < ncf->n_vids; i++)
 601                         if (ncf->vids[i] == vid) {
 602                                 vid = 0;
 603                                 break;
 604                         }
 605                 if (vid)
 606                         break;
 607         }
 608         rcu_read_unlock();
 609
 610         if (!vid) {
 611                 /* No VLAN ID is not set */
 612                 spin_unlock_irqrestore(&nc->lock, flags);
 613                 return -1;
 614         }
 615
 616         index = find_next_zero_bit(bitmap, ncf->n_vids, 0);
 617         if (index < 0 || index >= ncf->n_vids) {
 618                 netdev_err(ndp->ndev.dev,
 619                            "Channel %u already has all VLAN filters set\n",
 620                            nc->id);
 621                 spin_unlock_irqrestore(&nc->lock, flags);
 622                 return -1;
 623         }
 624
 625         ncf->vids[index] = vid;
 626         set_bit(index, bitmap);
 627         spin_unlock_irqrestore(&nc->lock, flags);
 628
 629         nca->type = NCSI_PKT_CMD_SVF;
 630         nca->words[1] = vid;
 631         /* HW filter index starts at 1 */
 632         nca->bytes[6] = index + 1;
 633         nca->bytes[7] = 0x01;
 634
 635         return 0;
 636 }
 637
 638 static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
 639 {
 640         struct ncsi_dev *nd = &ndp->ndev;
 641         struct net_device *dev = nd->dev;
 642         struct ncsi_package *np = ndp->active_package;
 643         struct ncsi_channel *nc = ndp->active_channel;
 644         struct ncsi_channel *hot_nc = NULL;
 645         struct ncsi_cmd_arg nca;
 646         unsigned char index;
 647         unsigned long flags;
 648         int ret;
 649
 650         nca.ndp = ndp;
 651         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 652         switch (nd->state) {
 653         case ncsi_dev_state_config:
 654         case ncsi_dev_state_config_sp:
 655                 ndp->pending_req_num = 1;
 656
 657                 /* Select the specific package */
 658                 nca.type = NCSI_PKT_CMD_SP;
 659                 if (ndp->flags & NCSI_DEV_HWA)
 660                         nca.bytes[0] = 0;
 661                 else
 662                         nca.bytes[0] = 1;
 663                 nca.package = np->id;
 664                 nca.channel = NCSI_RESERVED_CHANNEL;
 665                 ret = ncsi_xmit_cmd(&nca);
 666                 if (ret) {
 667                         netdev_err(ndp->ndev.dev,
 668                                    "NCSI: Failed to transmit CMD_SP\n");
 669                         goto error;
 670                 }
 671
 672                 nd->state = ncsi_dev_state_config_cis;
 673                 break;
 674         case ncsi_dev_state_config_cis:
 675                 ndp->pending_req_num = 1;
 676
 677                 /* Clear initial state */
 678                 nca.type = NCSI_PKT_CMD_CIS;
 679                 nca.package = np->id;
 680                 nca.channel = nc->id;
 681                 ret = ncsi_xmit_cmd(&nca);
 682                 if (ret) {
 683                         netdev_err(ndp->ndev.dev,
 684                                    "NCSI: Failed to transmit CMD_CIS\n");
 685                         goto error;
 686                 }
 687
 688                 nd->state = ncsi_dev_state_config_clear_vids;
 689                 break;
 690         case ncsi_dev_state_config_clear_vids:
 691         case ncsi_dev_state_config_svf:
 692         case ncsi_dev_state_config_ev:
 693         case ncsi_dev_state_config_sma:
 694         case ncsi_dev_state_config_ebf:
 695 #if IS_ENABLED(CONFIG_IPV6)
 696         case ncsi_dev_state_config_egmf:
 697 #endif
 698         case ncsi_dev_state_config_ecnt:
 699         case ncsi_dev_state_config_ec:
 700         case ncsi_dev_state_config_ae:
 701         case ncsi_dev_state_config_gls:
 702                 ndp->pending_req_num = 1;
 703
 704                 nca.package = np->id;
 705                 nca.channel = nc->id;
 706
 707                 /* Clear any active filters on the channel before setting */
 708                 if (nd->state == ncsi_dev_state_config_clear_vids) {
 709                         ret = clear_one_vid(ndp, nc, &nca);
 710                         if (ret) {
 711                                 nd->state = ncsi_dev_state_config_svf;
 712                                 schedule_work(&ndp->work);
 713                                 break;
 714                         }
 715                         /* Repeat */
 716                         nd->state = ncsi_dev_state_config_clear_vids;
 717                 /* Add known VLAN tags to the filter */
 718                 } else if (nd->state == ncsi_dev_state_config_svf) {
 719                         ret = set_one_vid(ndp, nc, &nca);
 720                         if (ret) {
 721                                 nd->state = ncsi_dev_state_config_ev;
 722                                 schedule_work(&ndp->work);
 723                                 break;
 724                         }
 725                         /* Repeat */
 726                         nd->state = ncsi_dev_state_config_svf;
 727                 /* Enable/Disable the VLAN filter */
 728                 } else if (nd->state == ncsi_dev_state_config_ev) {
 729                         if (list_empty(&ndp->vlan_vids)) {
 730                                 nca.type = NCSI_PKT_CMD_DV;
 731                         } else {
 732                                 nca.type = NCSI_PKT_CMD_EV;
 733                                 nca.bytes[3] = NCSI_CAP_VLAN_NO;
 734                         }
 735                         nd->state = ncsi_dev_state_config_sma;
 736                 } else if (nd->state == ncsi_dev_state_config_sma) {
 737                 /* Use first entry in unicast filter table. Note that
 738                  * the MAC filter table starts from entry 1 instead of
 739                  * 0.
 740                  */
 741                         nca.type = NCSI_PKT_CMD_SMA;
 742                         for (index = 0; index < 6; index++)
 743                                 nca.bytes[index] = dev->dev_addr[index];
 744                         nca.bytes[6] = 0x1;
 745                         nca.bytes[7] = 0x1;
 746                         nd->state = ncsi_dev_state_config_ebf;
 747                 } else if (nd->state == ncsi_dev_state_config_ebf) {
 748                         nca.type = NCSI_PKT_CMD_EBF;
 749                         nca.dwords[0] = nc->caps[NCSI_CAP_BC].cap;
 750                         nd->state = ncsi_dev_state_config_ecnt;
 751 #if IS_ENABLED(CONFIG_IPV6)
 752                         if (ndp->inet6_addr_num > 0 &&
 753                             (nc->caps[NCSI_CAP_GENERIC].cap &
 754                              NCSI_CAP_GENERIC_MC))
 755                                 nd->state = ncsi_dev_state_config_egmf;
 756                         else
 757                                 nd->state = ncsi_dev_state_config_ecnt;
 758                 } else if (nd->state == ncsi_dev_state_config_egmf) {
 759                         nca.type = NCSI_PKT_CMD_EGMF;
 760                         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
 761                         nd->state = ncsi_dev_state_config_ecnt;
 762 #endif /* CONFIG_IPV6 */
 763                 } else if (nd->state == ncsi_dev_state_config_ecnt) {
 764                         nca.type = NCSI_PKT_CMD_ECNT;
 765                         nd->state = ncsi_dev_state_config_ec;
 766                 } else if (nd->state == ncsi_dev_state_config_ec) {
 767                         /* Enable AEN if it's supported */
 768                         nca.type = NCSI_PKT_CMD_EC;
 769                         nd->state = ncsi_dev_state_config_ae;
 770                         if (!(nc->caps[NCSI_CAP_AEN].cap & NCSI_CAP_AEN_MASK))
 771                                 nd->state = ncsi_dev_state_config_gls;
 772                 } else if (nd->state == ncsi_dev_state_config_ae) {
 773                         nca.type = NCSI_PKT_CMD_AE;
 774                         nca.bytes[0] = 0;
 775                         nca.dwords[1] = nc->caps[NCSI_CAP_AEN].cap;
 776                         nd->state = ncsi_dev_state_config_gls;
 777                 } else if (nd->state == ncsi_dev_state_config_gls) {
 778                         nca.type = NCSI_PKT_CMD_GLS;
 779                         nd->state = ncsi_dev_state_config_done;
 780                 }
 781
 782                 ret = ncsi_xmit_cmd(&nca);
 783                 if (ret) {
 784                         netdev_err(ndp->ndev.dev,
 785                                    "NCSI: Failed to transmit CMD %x\n",
 786                                    nca.type);
 787                         goto error;
 788                 }
 789                 break;
 790         case ncsi_dev_state_config_done:
 791                 netdev_dbg(ndp->ndev.dev, "NCSI: channel %u config done\n",
 792                            nc->id);
 793                 spin_lock_irqsave(&nc->lock, flags);
 794                 if (nc->reconfigure_needed) {
 795                         /* This channel's configuration has been updated
 796                          * part-way during the config state - start the
 797                          * channel configuration over
 798                          */
 799                         nc->reconfigure_needed = false;
 800                         nc->state = NCSI_CHANNEL_INACTIVE;
 801                         spin_unlock_irqrestore(&nc->lock, flags);
 802
 803                         spin_lock_irqsave(&ndp->lock, flags);
 804                         list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 805                         spin_unlock_irqrestore(&ndp->lock, flags);
 806
 807                         netdev_dbg(dev, "Dirty NCSI channel state reset\n");
 808                         ncsi_process_next_channel(ndp);
 809                         break;
 810                 }
 811
 812                 if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {
 813                         hot_nc = nc;
 814                         nc->state = NCSI_CHANNEL_ACTIVE;
 815                 } else {
 816                         hot_nc = NULL;
 817                         nc->state = NCSI_CHANNEL_INACTIVE;
 818                         netdev_dbg(ndp->ndev.dev,
 819                                    "NCSI: channel %u link down after config\n",
 820                                    nc->id);
 821                 }
 822                 spin_unlock_irqrestore(&nc->lock, flags);
 823
 824                 /* Update the hot channel */
 825                 spin_lock_irqsave(&ndp->lock, flags);
 826                 ndp->hot_channel = hot_nc;
 827                 spin_unlock_irqrestore(&ndp->lock, flags);
 828
 829                 ncsi_start_channel_monitor(nc);
 830                 ncsi_process_next_channel(ndp);
 831                 break;
 832         default:
 833                 netdev_alert(dev, "Wrong NCSI state 0x%x in config\n",
 834                              nd->state);
 835         }
 836
 837         return;
 838
 839 error:
 840         ncsi_report_link(ndp, true);
 841 }
 842
 843 static int ncsi_choose_active_channel(struct ncsi_dev_priv *ndp)
 844 {
 845         struct ncsi_package *np, *force_package;
 846         struct ncsi_channel *nc, *found, *hot_nc, *force_channel;
 847         struct ncsi_channel_mode *ncm;
 848         unsigned long flags;
 849
 850         spin_lock_irqsave(&ndp->lock, flags);
 851         hot_nc = ndp->hot_channel;
 852         force_channel = ndp->force_channel;
 853         force_package = ndp->force_package;
 854         spin_unlock_irqrestore(&ndp->lock, flags);
 855
 856         /* Force a specific channel whether or not it has link if we have been
 857          * configured to do so
 858          */
 859         if (force_package && force_channel) {
 860                 found = force_channel;
 861                 ncm = &found->modes[NCSI_MODE_LINK];
 862                 if (!(ncm->data[2] & 0x1))
 863                         netdev_info(ndp->ndev.dev,
 864                                     "NCSI: Channel %u forced, but it is link down\n",
 865                                     found->id);
 866                 goto out;
 867         }
 868
 869         /* The search is done once an inactive channel with up
 870          * link is found.
 871          */
 872         found = NULL;
 873         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 874                 if (ndp->force_package && np != ndp->force_package)
 875                         continue;
 876                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 877                         spin_lock_irqsave(&nc->lock, flags);
 878
 879                         if (!list_empty(&nc->link) ||
 880                             nc->state != NCSI_CHANNEL_INACTIVE) {
 881                                 spin_unlock_irqrestore(&nc->lock, flags);
 882                                 continue;
 883                         }
 884
 885                         if (!found)
 886                                 found = nc;
 887
 888                         if (nc == hot_nc)
 889                                 found = nc;
 890
 891                         ncm = &nc->modes[NCSI_MODE_LINK];
 892                         if (ncm->data[2] & 0x1) {
 893                                 spin_unlock_irqrestore(&nc->lock, flags);
 894                                 found = nc;
 895                                 goto out;
 896                         }
 897
 898                         spin_unlock_irqrestore(&nc->lock, flags);
 899                 }
 900         }
 901
 902         if (!found) {
 903                 netdev_warn(ndp->ndev.dev,
 904                             "NCSI: No channel found with link\n");
 905                 ncsi_report_link(ndp, true);
 906                 return -ENODEV;
 907         }
 908
 909         ncm = &found->modes[NCSI_MODE_LINK];
 910         netdev_dbg(ndp->ndev.dev,
 911                    "NCSI: Channel %u added to queue (link %s)\n",
 912                    found->id, ncm->data[2] & 0x1 ? "up" : "down");
 913
 914 out:
 915         spin_lock_irqsave(&ndp->lock, flags);
 916         list_add_tail_rcu(&found->link, &ndp->channel_queue);
 917         spin_unlock_irqrestore(&ndp->lock, flags);
 918
 919         return ncsi_process_next_channel(ndp);
 920 }
 921
 922 static bool ncsi_check_hwa(struct ncsi_dev_priv *ndp)
 923 {
 924         struct ncsi_package *np;
 925         struct ncsi_channel *nc;
 926         unsigned int cap;
 927         bool has_channel = false;
 928
 929         /* The hardware arbitration is disabled if any one channel
 930          * doesn't support explicitly.
 931          */
 932         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 933                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 934                         has_channel = true;
 935
 936                         cap = nc->caps[NCSI_CAP_GENERIC].cap;
 937                         if (!(cap & NCSI_CAP_GENERIC_HWA) ||
 938                             (cap & NCSI_CAP_GENERIC_HWA_MASK) !=
 939                             NCSI_CAP_GENERIC_HWA_SUPPORT) {
 940                                 ndp->flags &= ~NCSI_DEV_HWA;
 941                                 return false;
 942                         }
 943                 }
 944         }
 945
 946         if (has_channel) {
 947                 ndp->flags |= NCSI_DEV_HWA;
 948                 return true;
 949         }
 950
 951         ndp->flags &= ~NCSI_DEV_HWA;
 952         return false;
 953 }
 954
 955 static int ncsi_enable_hwa(struct ncsi_dev_priv *ndp)
 956 {
 957         struct ncsi_package *np;
 958         struct ncsi_channel *nc;
 959         unsigned long flags;
 960
 961         /* Move all available channels to processing queue */
 962         spin_lock_irqsave(&ndp->lock, flags);
 963         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 964                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 965                         WARN_ON_ONCE(nc->state != NCSI_CHANNEL_INACTIVE ||
 966                                      !list_empty(&nc->link));
 967                         ncsi_stop_channel_monitor(nc);
 968                         list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 969                 }
 970         }
 971         spin_unlock_irqrestore(&ndp->lock, flags);
 972
 973         /* We can have no channels in extremely case */
 974         if (list_empty(&ndp->channel_queue)) {
 975                 netdev_err(ndp->ndev.dev,
 976                            "NCSI: No available channels for HWA\n");
 977                 ncsi_report_link(ndp, false);
 978                 return -ENOENT;
 979         }
 980
 981         return ncsi_process_next_channel(ndp);
 982 }
 983
 984 static void ncsi_probe_channel(struct ncsi_dev_priv *ndp)
 985 {
 986         struct ncsi_dev *nd = &ndp->ndev;
 987         struct ncsi_package *np;
 988         struct ncsi_channel *nc;
 989         struct ncsi_cmd_arg nca;
 990         unsigned char index;
 991         int ret;
 992
 993         nca.ndp = ndp;
 994         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 995         switch (nd->state) {
 996         case ncsi_dev_state_probe:
 997                 nd->state = ncsi_dev_state_probe_deselect;
 998                 /* Fall through */
 999         case ncsi_dev_state_probe_deselect:
1000                 ndp->pending_req_num = 8;
1001
1002                 /* Deselect all possible packages */
1003                 nca.type = NCSI_PKT_CMD_DP;
1004                 nca.channel = NCSI_RESERVED_CHANNEL;
1005                 for (index = 0; index < 8; index++) {
1006                         nca.package = index;
1007                         ret = ncsi_xmit_cmd(&nca);
1008                         if (ret)
1009                                 goto error;
1010                 }
1011
1012                 nd->state = ncsi_dev_state_probe_package;
1013                 break;
1014         case ncsi_dev_state_probe_package:
1015                 ndp->pending_req_num = 16;
1016
1017                 /* Select all possible packages */
1018                 nca.type = NCSI_PKT_CMD_SP;
1019                 nca.bytes[0] = 1;
1020                 nca.channel = NCSI_RESERVED_CHANNEL;
1021                 for (index = 0; index < 8; index++) {
1022                         nca.package = index;
1023                         ret = ncsi_xmit_cmd(&nca);
1024                         if (ret)
1025                                 goto error;
1026                 }
1027
1028                 /* Disable all possible packages */
1029                 nca.type = NCSI_PKT_CMD_DP;
1030                 for (index = 0; index < 8; index++) {
1031                         nca.package = index;
1032                         ret = ncsi_xmit_cmd(&nca);
1033                         if (ret)
1034                                 goto error;
1035                 }
1036
1037                 nd->state = ncsi_dev_state_probe_channel;
1038                 break;
1039         case ncsi_dev_state_probe_channel:
1040                 if (!ndp->active_package)
1041                         ndp->active_package = list_first_or_null_rcu(
1042                                 &ndp->packages, struct ncsi_package, node);
1043                 else if (list_is_last(&ndp->active_package->node,
1044                                       &ndp->packages))
1045                         ndp->active_package = NULL;
1046                 else
1047                         ndp->active_package = list_next_entry(
1048                                 ndp->active_package, node);
1049
1050                 /* All available packages and channels are enumerated. The
1051                  * enumeration happens for once when the NCSI interface is
1052                  * started. So we need continue to start the interface after
1053                  * the enumeration.
1054                  *
1055                  * We have to choose an active channel before configuring it.
1056                  * Note that we possibly don't have active channel in extreme
1057                  * situation.
1058                  */
1059                 if (!ndp->active_package) {
1060                         ndp->flags |= NCSI_DEV_PROBED;
1061                         if (ncsi_check_hwa(ndp))
1062                                 ncsi_enable_hwa(ndp);
1063                         else
1064                                 ncsi_choose_active_channel(ndp);
1065                         return;
1066                 }
1067
1068                 /* Select the active package */
1069                 ndp->pending_req_num = 1;
1070                 nca.type = NCSI_PKT_CMD_SP;
1071                 nca.bytes[0] = 1;
1072                 nca.package = ndp->active_package->id;
1073                 nca.channel = NCSI_RESERVED_CHANNEL;
1074                 ret = ncsi_xmit_cmd(&nca);
1075                 if (ret)
1076                         goto error;
1077
1078                 nd->state = ncsi_dev_state_probe_cis;
1079                 break;
1080         case ncsi_dev_state_probe_cis:
1081                 ndp->pending_req_num = NCSI_RESERVED_CHANNEL;
1082
1083                 /* Clear initial state */
1084                 nca.type = NCSI_PKT_CMD_CIS;
1085                 nca.package = ndp->active_package->id;
1086                 for (index = 0; index < NCSI_RESERVED_CHANNEL; index++) {
1087                         nca.channel = index;
1088                         ret = ncsi_xmit_cmd(&nca);
1089                         if (ret)
1090                                 goto error;
1091                 }
1092
1093                 nd->state = ncsi_dev_state_probe_gvi;
1094                 break;
1095         case ncsi_dev_state_probe_gvi:
1096         case ncsi_dev_state_probe_gc:
1097         case ncsi_dev_state_probe_gls:
1098                 np = ndp->active_package;
1099                 ndp->pending_req_num = np->channel_num;
1100
1101                 /* Retrieve version, capability or link status */
1102                 if (nd->state == ncsi_dev_state_probe_gvi)
1103                         nca.type = NCSI_PKT_CMD_GVI;
1104                 else if (nd->state == ncsi_dev_state_probe_gc)
1105                         nca.type = NCSI_PKT_CMD_GC;
1106                 else
1107                         nca.type = NCSI_PKT_CMD_GLS;
1108
1109                 nca.package = np->id;
1110                 NCSI_FOR_EACH_CHANNEL(np, nc) {
1111                         nca.channel = nc->id;
1112                         ret = ncsi_xmit_cmd(&nca);
1113                         if (ret)
1114                                 goto error;
1115                 }
1116
1117                 if (nd->state == ncsi_dev_state_probe_gvi)
1118                         nd->state = ncsi_dev_state_probe_gc;
1119                 else if (nd->state == ncsi_dev_state_probe_gc)
1120                         nd->state = ncsi_dev_state_probe_gls;
1121                 else
1122                         nd->state = ncsi_dev_state_probe_dp;
1123                 break;
1124         case ncsi_dev_state_probe_dp:
1125                 ndp->pending_req_num = 1;
1126
1127                 /* Deselect the active package */
1128                 nca.type = NCSI_PKT_CMD_DP;
1129                 nca.package = ndp->active_package->id;
1130                 nca.channel = NCSI_RESERVED_CHANNEL;
1131                 ret = ncsi_xmit_cmd(&nca);
1132                 if (ret)
1133                         goto error;
1134
1135                 /* Scan channels in next package */
1136                 nd->state = ncsi_dev_state_probe_channel;
1137                 break;
1138         default:
1139                 netdev_warn(nd->dev, "Wrong NCSI state 0x%0x in enumeration\n",
1140                             nd->state);
1141         }
1142
1143         return;
1144 error:
1145         netdev_err(ndp->ndev.dev,
1146                    "NCSI: Failed to transmit cmd 0x%x during probe\n",
1147                    nca.type);
1148         ncsi_report_link(ndp, true);
1149 }
1150
1151 static void ncsi_dev_work(struct work_struct *work)
1152 {
1153         struct ncsi_dev_priv *ndp = container_of(work,
1154                         struct ncsi_dev_priv, work);
1155         struct ncsi_dev *nd = &ndp->ndev;
1156
1157         switch (nd->state & ncsi_dev_state_major) {
1158         case ncsi_dev_state_probe:
1159                 ncsi_probe_channel(ndp);
1160                 break;
1161         case ncsi_dev_state_suspend:
1162                 ncsi_suspend_channel(ndp);
1163                 break;
1164         case ncsi_dev_state_config:
1165                 ncsi_configure_channel(ndp);
1166                 break;
1167         default:
1168                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in workqueue\n",
1169                             nd->state);
1170         }
1171 }
1172
1173 int ncsi_process_next_channel(struct ncsi_dev_priv *ndp)
1174 {
1175         struct ncsi_channel *nc;
1176         int old_state;
1177         unsigned long flags;
1178
1179         spin_lock_irqsave(&ndp->lock, flags);
1180         nc = list_first_or_null_rcu(&ndp->channel_queue,
1181                                     struct ncsi_channel, link);
1182         if (!nc) {
1183                 spin_unlock_irqrestore(&ndp->lock, flags);
1184                 goto out;
1185         }
1186
1187         list_del_init(&nc->link);
1188         spin_unlock_irqrestore(&ndp->lock, flags);
1189
1190         spin_lock_irqsave(&nc->lock, flags);
1191         old_state = nc->state;
1192         nc->state = NCSI_CHANNEL_INVISIBLE;
1193         spin_unlock_irqrestore(&nc->lock, flags);
1194
1195         ndp->active_channel = nc;
1196         ndp->active_package = nc->package;
1197
1198         switch (old_state) {
1199         case NCSI_CHANNEL_INACTIVE:
1200                 ndp->ndev.state = ncsi_dev_state_config;
1201                 netdev_dbg(ndp->ndev.dev, "NCSI: configuring channel %u\n",
1202                            nc->id);
1203                 ncsi_configure_channel(ndp);
1204                 break;
1205         case NCSI_CHANNEL_ACTIVE:
1206                 ndp->ndev.state = ncsi_dev_state_suspend;
1207                 netdev_dbg(ndp->ndev.dev, "NCSI: suspending channel %u\n",
1208                            nc->id);
1209                 ncsi_suspend_channel(ndp);
1210                 break;
1211         default:
1212                 netdev_err(ndp->ndev.dev, "Invalid state 0x%x on %d:%d\n",
1213                            old_state, nc->package->id, nc->id);
1214                 ncsi_report_link(ndp, false);
1215                 return -EINVAL;
1216         }
1217
1218         return 0;
1219
1220 out:
1221         ndp->active_channel = NULL;
1222         ndp->active_package = NULL;
1223         if (ndp->flags & NCSI_DEV_RESHUFFLE) {
1224                 ndp->flags &= ~NCSI_DEV_RESHUFFLE;
1225                 return ncsi_choose_active_channel(ndp);
1226         }
1227
1228         ncsi_report_link(ndp, false);
1229         return -ENODEV;
1230 }
1231
1232 #if IS_ENABLED(CONFIG_IPV6)
1233 static int ncsi_inet6addr_event(struct notifier_block *this,
1234                                 unsigned long event, void *data)
1235 {
1236         struct inet6_ifaddr *ifa = data;
1237         struct net_device *dev = ifa->idev->dev;
1238         struct ncsi_dev *nd = ncsi_find_dev(dev);
1239         struct ncsi_dev_priv *ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
1240         struct ncsi_package *np;
1241         struct ncsi_channel *nc;
1242         struct ncsi_cmd_arg nca;
1243         bool action;
1244         int ret;
1245
1246         if (!ndp || (ipv6_addr_type(&ifa->addr) &
1247             (IPV6_ADDR_LINKLOCAL | IPV6_ADDR_LOOPBACK)))
1248                 return NOTIFY_OK;
1249
1250         switch (event) {
1251         case NETDEV_UP:
1252                 action = (++ndp->inet6_addr_num) == 1;
1253                 nca.type = NCSI_PKT_CMD_EGMF;
1254                 break;
1255         case NETDEV_DOWN:
1256                 action = (--ndp->inet6_addr_num == 0);
1257                 nca.type = NCSI_PKT_CMD_DGMF;
1258                 break;
1259         default:
1260                 return NOTIFY_OK;
1261         }
1262
1263         /* We might not have active channel or packages. The IPv6
1264          * required multicast will be enabled when active channel
1265          * or packages are chosen.
1266          */
1267         np = ndp->active_package;
1268         nc = ndp->active_channel;
1269         if (!action || !np || !nc)
1270                 return NOTIFY_OK;
1271
1272         /* We needn't enable or disable it if the function isn't supported */
1273         if (!(nc->caps[NCSI_CAP_GENERIC].cap & NCSI_CAP_GENERIC_MC))
1274                 return NOTIFY_OK;
1275
1276         nca.ndp = ndp;
1277         nca.req_flags = 0;
1278         nca.package = np->id;
1279         nca.channel = nc->id;
1280         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
1281         ret = ncsi_xmit_cmd(&nca);
1282         if (ret) {
1283                 netdev_warn(dev, "Fail to %s global multicast filter (%d)\n",
1284                             (event == NETDEV_UP) ? "enable" : "disable", ret);
1285                 return NOTIFY_DONE;
1286         }
1287
1288         return NOTIFY_OK;
1289 }
1290
1291 static struct notifier_block ncsi_inet6addr_notifier = {
1292         .notifier_call = ncsi_inet6addr_event,
1293 };
1294 #endif /* CONFIG_IPV6 */
1295
1296 static int ncsi_kick_channels(struct ncsi_dev_priv *ndp)
1297 {
1298         struct ncsi_dev *nd = &ndp->ndev;
1299         struct ncsi_channel *nc;
1300         struct ncsi_package *np;
1301         unsigned long flags;
1302         unsigned int n = 0;
1303
1304         NCSI_FOR_EACH_PACKAGE(ndp, np) {
1305                 NCSI_FOR_EACH_CHANNEL(np, nc) {
1306                         spin_lock_irqsave(&nc->lock, flags);
1307
1308                         /* Channels may be busy, mark dirty instead of
1309                          * kicking if;
1310                          * a) not ACTIVE (configured)
1311                          * b) in the channel_queue (to be configured)
1312                          * c) it's ndev is in the config state
1313                          */
1314                         if (nc->state != NCSI_CHANNEL_ACTIVE) {
1315                                 if ((ndp->ndev.state & 0xff00) ==
1316                                                 ncsi_dev_state_config ||
1317                                                 !list_empty(&nc->link)) {
1318                                         netdev_dbg(nd->dev,
1319                                                    "NCSI: channel %p marked dirty\n",
1320                                                    nc);
1321                                         nc->reconfigure_needed = true;
1322                                 }
1323                                 spin_unlock_irqrestore(&nc->lock, flags);
1324                                 continue;
1325                         }
1326
1327                         spin_unlock_irqrestore(&nc->lock, flags);
1328
1329                         ncsi_stop_channel_monitor(nc);
1330                         spin_lock_irqsave(&nc->lock, flags);
1331                         nc->state = NCSI_CHANNEL_INACTIVE;
1332                         spin_unlock_irqrestore(&nc->lock, flags);
1333
1334                         spin_lock_irqsave(&ndp->lock, flags);
1335                         list_add_tail_rcu(&nc->link, &ndp->channel_queue);
1336                         spin_unlock_irqrestore(&ndp->lock, flags);
1337
1338                         netdev_dbg(nd->dev, "NCSI: kicked channel %p\n", nc);
1339                         n++;
1340                 }
1341         }
1342
1343         return n;
1344 }
1345
1346 int ncsi_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
1347 {
1348         struct ncsi_dev_priv *ndp;
1349         unsigned int n_vids = 0;
1350         struct vlan_vid *vlan;
1351         struct ncsi_dev *nd;
1352         bool found = false;
1353
1354         if (vid == 0)
1355                 return 0;
1356
1357         nd = ncsi_find_dev(dev);
1358         if (!nd) {
1359                 netdev_warn(dev, "NCSI: No net_device?\n");
1360                 return 0;
1361         }
1362
1363         ndp = TO_NCSI_DEV_PRIV(nd);
1364
1365         /* Add the VLAN id to our internal list */
1366         list_for_each_entry_rcu(vlan, &ndp->vlan_vids, list) {
1367                 n_vids++;
1368                 if (vlan->vid == vid) {
1369                         netdev_dbg(dev, "NCSI: vid %u already registered\n",
1370                                    vid);
1371                         return 0;
1372                 }
1373         }
1374         if (n_vids >= NCSI_MAX_VLAN_VIDS) {
1375                 netdev_warn(dev,
1376                             "tried to add vlan id %u but NCSI max already registered (%u)\n",
1377                             vid, NCSI_MAX_VLAN_VIDS);
1378                 return -ENOSPC;
1379         }
1380
1381         vlan = kzalloc(sizeof(*vlan), GFP_KERNEL);
1382         if (!vlan)
1383                 return -ENOMEM;
1384
1385         vlan->proto = proto;
1386         vlan->vid = vid;
1387         list_add_rcu(&vlan->list, &ndp->vlan_vids);
1388
1389         netdev_dbg(dev, "NCSI: Added new vid %u\n", vid);
1390
1391         found = ncsi_kick_channels(ndp) != 0;
1392
1393         return found ? ncsi_process_next_channel(ndp) : 0;
1394 }
1395 EXPORT_SYMBOL_GPL(ncsi_vlan_rx_add_vid);
1396
1397 int ncsi_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
1398 {
1399         struct vlan_vid *vlan, *tmp;
1400         struct ncsi_dev_priv *ndp;
1401         struct ncsi_dev *nd;
1402         bool found = false;
1403
1404         if (vid == 0)
1405                 return 0;
1406
1407         nd = ncsi_find_dev(dev);
1408         if (!nd) {
1409                 netdev_warn(dev, "NCSI: no net_device?\n");
1410                 return 0;
1411         }
1412
1413         ndp = TO_NCSI_DEV_PRIV(nd);
1414
1415         /* Remove the VLAN id from our internal list */
1416         list_for_each_entry_safe(vlan, tmp, &ndp->vlan_vids, list)
1417                 if (vlan->vid == vid) {
1418                         netdev_dbg(dev, "NCSI: vid %u found, removing\n", vid);
1419                         list_del_rcu(&vlan->list);
1420                         found = true;
1421                         kfree(vlan);
1422                 }
1423
1424         if (!found) {
1425                 netdev_err(dev, "NCSI: vid %u wasn't registered!\n", vid);
1426                 return -EINVAL;
1427         }
1428
1429         found = ncsi_kick_channels(ndp) != 0;
1430
1431         return found ? ncsi_process_next_channel(ndp) : 0;
1432 }
1433 EXPORT_SYMBOL_GPL(ncsi_vlan_rx_kill_vid);
1434
1435 struct ncsi_dev *ncsi_register_dev(struct net_device *dev,
1436                                    void (*handler)(struct ncsi_dev *ndev))
1437 {
1438         struct ncsi_dev_priv *ndp;
1439         struct ncsi_dev *nd;
1440         unsigned long flags;
1441         int i;
1442
1443         /* Check if the device has been registered or not */
1444         nd = ncsi_find_dev(dev);
1445         if (nd)
1446                 return nd;
1447
1448         /* Create NCSI device */
1449         ndp = kzalloc(sizeof(*ndp), GFP_ATOMIC);
1450         if (!ndp)
1451                 return NULL;
1452
1453         nd = &ndp->ndev;
1454         nd->state = ncsi_dev_state_registered;
1455         nd->dev = dev;
1456         nd->handler = handler;
1457         ndp->pending_req_num = 0;
1458         INIT_LIST_HEAD(&ndp->channel_queue);
1459         INIT_LIST_HEAD(&ndp->vlan_vids);
1460         INIT_WORK(&ndp->work, ncsi_dev_work);
1461
1462         /* Initialize private NCSI device */
1463         spin_lock_init(&ndp->lock);
1464         INIT_LIST_HEAD(&ndp->packages);
1465         ndp->request_id = NCSI_REQ_START_IDX;
1466         for (i = 0; i < ARRAY_SIZE(ndp->requests); i++) {
1467                 ndp->requests[i].id = i;
1468                 ndp->requests[i].ndp = ndp;
1469                 timer_setup(&ndp->requests[i].timer, ncsi_request_timeout, 0);
1470         }
1471
1472         spin_lock_irqsave(&ncsi_dev_lock, flags);
1473 #if IS_ENABLED(CONFIG_IPV6)
1474         ndp->inet6_addr_num = 0;
1475         if (list_empty(&ncsi_dev_list))
1476                 register_inet6addr_notifier(&ncsi_inet6addr_notifier);
1477 #endif
1478         list_add_tail_rcu(&ndp->node, &ncsi_dev_list);
1479         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1480
1481         /* Register NCSI packet Rx handler */
1482         ndp->ptype.type = cpu_to_be16(ETH_P_NCSI);
1483         ndp->ptype.func = ncsi_rcv_rsp;
1484         ndp->ptype.dev = dev;
1485         dev_add_pack(&ndp->ptype);
1486
1487         /* Set up generic netlink interface */
1488         ncsi_init_netlink(dev);
1489
1490         return nd;
1491 }
1492 EXPORT_SYMBOL_GPL(ncsi_register_dev);
1493
1494 int ncsi_start_dev(struct ncsi_dev *nd)
1495 {
1496         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1497         int ret;
1498
1499         if (nd->state != ncsi_dev_state_registered &&
1500             nd->state != ncsi_dev_state_functional)
1501                 return -ENOTTY;
1502
1503         if (!(ndp->flags & NCSI_DEV_PROBED)) {
1504                 nd->state = ncsi_dev_state_probe;
1505                 schedule_work(&ndp->work);
1506                 return 0;
1507         }
1508
1509         if (ndp->flags & NCSI_DEV_HWA) {
1510                 netdev_info(ndp->ndev.dev, "NCSI: Enabling HWA mode\n");
1511                 ret = ncsi_enable_hwa(ndp);
1512         } else {
1513                 ret = ncsi_choose_active_channel(ndp);
1514         }
1515
1516         return ret;
1517 }
1518 EXPORT_SYMBOL_GPL(ncsi_start_dev);
1519
1520 void ncsi_stop_dev(struct ncsi_dev *nd)
1521 {
1522         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1523         struct ncsi_package *np;
1524         struct ncsi_channel *nc;
1525         bool chained;
1526         int old_state;
1527         unsigned long flags;
1528
1529         /* Stop the channel monitor and reset channel's state */
1530         NCSI_FOR_EACH_PACKAGE(ndp, np) {
1531                 NCSI_FOR_EACH_CHANNEL(np, nc) {
1532                         ncsi_stop_channel_monitor(nc);
1533
1534                         spin_lock_irqsave(&nc->lock, flags);
1535                         chained = !list_empty(&nc->link);
1536                         old_state = nc->state;
1537                         nc->state = NCSI_CHANNEL_INACTIVE;
1538                         spin_unlock_irqrestore(&nc->lock, flags);
1539
1540                         WARN_ON_ONCE(chained ||
1541                                      old_state == NCSI_CHANNEL_INVISIBLE);
1542                 }
1543         }
1544
1545         netdev_dbg(ndp->ndev.dev, "NCSI: Stopping device\n");
1546         ncsi_report_link(ndp, true);
1547 }
1548 EXPORT_SYMBOL_GPL(ncsi_stop_dev);
1549
1550 void ncsi_unregister_dev(struct ncsi_dev *nd)
1551 {
1552         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1553         struct ncsi_package *np, *tmp;
1554         unsigned long flags;
1555
1556         dev_remove_pack(&ndp->ptype);
1557
1558         list_for_each_entry_safe(np, tmp, &ndp->packages, node)
1559                 ncsi_remove_package(np);
1560
1561         spin_lock_irqsave(&ncsi_dev_lock, flags);
1562         list_del_rcu(&ndp->node);
1563 #if IS_ENABLED(CONFIG_IPV6)
1564         if (list_empty(&ncsi_dev_list))
1565                 unregister_inet6addr_notifier(&ncsi_inet6addr_notifier);
1566 #endif
1567         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1568
1569         ncsi_unregister_netlink(nd->dev);
1570
1571         kfree(ndp);
1572 }
1573 EXPORT_SYMBOL_GPL(ncsi_unregister_dev);