net/ncsi/ncsi-manage.c

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