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