net/ncsi/ncsi-manage.c

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