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