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 int ncsi_find_filter(struct ncsi_channel *nc, int table, void *data)
  42 {
  43         struct ncsi_channel_filter *ncf;
  44         void *bitmap;
  45         int index, size;
  46         unsigned long flags;
  47
  48         ncf = nc->filters[table];
  49         if (!ncf)
  50                 return -ENXIO;
  51
  52         size = ncsi_filter_size(table);
  53         if (size < 0)
  54                 return size;
  55
  56         spin_lock_irqsave(&nc->lock, flags);
  57         bitmap = (void *)&ncf->bitmap;
  58         index = -1;
  59         while ((index = find_next_bit(bitmap, ncf->total, index + 1))
  60                < ncf->total) {
  61                 if (!memcmp(ncf->data + size * index, data, size)) {
  62                         spin_unlock_irqrestore(&nc->lock, flags);
  63                         return index;
  64                 }
  65         }
  66         spin_unlock_irqrestore(&nc->lock, flags);
  67
  68         return -ENOENT;
  69 }
  70
  71 int ncsi_add_filter(struct ncsi_channel *nc, int table, void *data)
  72 {
  73         struct ncsi_channel_filter *ncf;
  74         int index, size;
  75         void *bitmap;
  76         unsigned long flags;
  77
  78         size = ncsi_filter_size(table);
  79         if (size < 0)
  80                 return size;
  81
  82         index = ncsi_find_filter(nc, table, data);
  83         if (index >= 0)
  84                 return index;
  85
  86         ncf = nc->filters[table];
  87         if (!ncf)
  88                 return -ENODEV;
  89
  90         spin_lock_irqsave(&nc->lock, flags);
  91         bitmap = (void *)&ncf->bitmap;
  92         do {
  93                 index = find_next_zero_bit(bitmap, ncf->total, 0);
  94                 if (index >= ncf->total) {
  95                         spin_unlock_irqrestore(&nc->lock, flags);
  96                         return -ENOSPC;
  97                 }
  98         } while (test_and_set_bit(index, bitmap));
  99
 100         memcpy(ncf->data + size * index, data, size);
 101         spin_unlock_irqrestore(&nc->lock, flags);
 102
 103         return index;
 104 }
 105
 106 int ncsi_remove_filter(struct ncsi_channel *nc, int table, int index)
 107 {
 108         struct ncsi_channel_filter *ncf;
 109         int size;
 110         void *bitmap;
 111         unsigned long flags;
 112
 113         size = ncsi_filter_size(table);
 114         if (size < 0)
 115                 return size;
 116
 117         ncf = nc->filters[table];
 118         if (!ncf || index >= ncf->total)
 119                 return -ENODEV;
 120
 121         spin_lock_irqsave(&nc->lock, flags);
 122         bitmap = (void *)&ncf->bitmap;
 123         if (test_and_clear_bit(index, bitmap))
 124                 memset(ncf->data + size * index, 0, size);
 125         spin_unlock_irqrestore(&nc->lock, flags);
 126
 127         return 0;
 128 }
 129
 130 static void ncsi_report_link(struct ncsi_dev_priv *ndp, bool force_down)
 131 {
 132         struct ncsi_dev *nd = &ndp->ndev;
 133         struct ncsi_package *np;
 134         struct ncsi_channel *nc;
 135         unsigned long flags;
 136
 137         nd->state = ncsi_dev_state_functional;
 138         if (force_down) {
 139                 nd->link_up = 0;
 140                 goto report;
 141         }
 142
 143         nd->link_up = 0;
 144         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 145                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 146                         spin_lock_irqsave(&nc->lock, flags);
 147
 148                         if (!list_empty(&nc->link) ||
 149                             nc->state != NCSI_CHANNEL_ACTIVE) {
 150                                 spin_unlock_irqrestore(&nc->lock, flags);
 151                                 continue;
 152                         }
 153
 154                         if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1) {
 155                                 spin_unlock_irqrestore(&nc->lock, flags);
 156                                 nd->link_up = 1;
 157                                 goto report;
 158                         }
 159
 160                         spin_unlock_irqrestore(&nc->lock, flags);
 161                 }
 162         }
 163
 164 report:
 165         nd->handler(nd);
 166 }
 167
 168 static void ncsi_channel_monitor(unsigned long data)
 169 {
 170         struct ncsi_channel *nc = (struct ncsi_channel *)data;
 171         struct ncsi_package *np = nc->package;
 172         struct ncsi_dev_priv *ndp = np->ndp;
 173         struct ncsi_cmd_arg nca;
 174         bool enabled, chained;
 175         unsigned int timeout;
 176         unsigned long flags;
 177         int state, ret;
 178
 179         spin_lock_irqsave(&nc->lock, flags);
 180         state = nc->state;
 181         chained = !list_empty(&nc->link);
 182         timeout = nc->timeout;
 183         enabled = nc->enabled;
 184         spin_unlock_irqrestore(&nc->lock, flags);
 185
 186         if (!enabled || chained)
 187                 return;
 188         if (state != NCSI_CHANNEL_INACTIVE &&
 189             state != NCSI_CHANNEL_ACTIVE)
 190                 return;
 191
 192         if (!(timeout % 2)) {
 193                 nca.ndp = ndp;
 194                 nca.package = np->id;
 195                 nca.channel = nc->id;
 196                 nca.type = NCSI_PKT_CMD_GLS;
 197                 nca.driven = false;
 198                 ret = ncsi_xmit_cmd(&nca);
 199                 if (ret) {
 200                         netdev_err(ndp->ndev.dev, "Error %d sending GLS\n",
 201                                    ret);
 202                         return;
 203                 }
 204         }
 205
 206         if (timeout + 1 >= 3) {
 207                 if (!(ndp->flags & NCSI_DEV_HWA) &&
 208                     state == NCSI_CHANNEL_ACTIVE)
 209                         ncsi_report_link(ndp, true);
 210
 211                 spin_lock_irqsave(&nc->lock, flags);
 212                 nc->state = NCSI_CHANNEL_INVISIBLE;
 213                 spin_unlock_irqrestore(&nc->lock, flags);
 214
 215                 spin_lock_irqsave(&ndp->lock, flags);
 216                 nc->state = NCSI_CHANNEL_INACTIVE;
 217                 list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 218                 spin_unlock_irqrestore(&ndp->lock, flags);
 219                 ncsi_process_next_channel(ndp);
 220                 return;
 221         }
 222
 223         spin_lock_irqsave(&nc->lock, flags);
 224         nc->timeout = timeout + 1;
 225         nc->enabled = true;
 226         spin_unlock_irqrestore(&nc->lock, flags);
 227         mod_timer(&nc->timer, jiffies + HZ * (1 << (nc->timeout / 2)));
 228 }
 229
 230 void ncsi_start_channel_monitor(struct ncsi_channel *nc)
 231 {
 232         unsigned long flags;
 233
 234         spin_lock_irqsave(&nc->lock, flags);
 235         WARN_ON_ONCE(nc->enabled);
 236         nc->timeout = 0;
 237         nc->enabled = true;
 238         spin_unlock_irqrestore(&nc->lock, flags);
 239
 240         mod_timer(&nc->timer, jiffies + HZ * (1 << (nc->timeout / 2)));
 241 }
 242
 243 void ncsi_stop_channel_monitor(struct ncsi_channel *nc)
 244 {
 245         unsigned long flags;
 246
 247         spin_lock_irqsave(&nc->lock, flags);
 248         if (!nc->enabled) {
 249                 spin_unlock_irqrestore(&nc->lock, flags);
 250                 return;
 251         }
 252         nc->enabled = false;
 253         spin_unlock_irqrestore(&nc->lock, flags);
 254
 255         del_timer_sync(&nc->timer);
 256 }
 257
 258 struct ncsi_channel *ncsi_find_channel(struct ncsi_package *np,
 259                                        unsigned char id)
 260 {
 261         struct ncsi_channel *nc;
 262
 263         NCSI_FOR_EACH_CHANNEL(np, nc) {
 264                 if (nc->id == id)
 265                         return nc;
 266         }
 267
 268         return NULL;
 269 }
 270
 271 struct ncsi_channel *ncsi_add_channel(struct ncsi_package *np, unsigned char id)
 272 {
 273         struct ncsi_channel *nc, *tmp;
 274         int index;
 275         unsigned long flags;
 276
 277         nc = kzalloc(sizeof(*nc), GFP_ATOMIC);
 278         if (!nc)
 279                 return NULL;
 280
 281         nc->id = id;
 282         nc->package = np;
 283         nc->state = NCSI_CHANNEL_INACTIVE;
 284         nc->enabled = false;
 285         setup_timer(&nc->timer, ncsi_channel_monitor, (unsigned long)nc);
 286         spin_lock_init(&nc->lock);
 287         INIT_LIST_HEAD(&nc->link);
 288         for (index = 0; index < NCSI_CAP_MAX; index++)
 289                 nc->caps[index].index = index;
 290         for (index = 0; index < NCSI_MODE_MAX; index++)
 291                 nc->modes[index].index = index;
 292
 293         spin_lock_irqsave(&np->lock, flags);
 294         tmp = ncsi_find_channel(np, id);
 295         if (tmp) {
 296                 spin_unlock_irqrestore(&np->lock, flags);
 297                 kfree(nc);
 298                 return tmp;
 299         }
 300
 301         list_add_tail_rcu(&nc->node, &np->channels);
 302         np->channel_num++;
 303         spin_unlock_irqrestore(&np->lock, flags);
 304
 305         return nc;
 306 }
 307
 308 static void ncsi_remove_channel(struct ncsi_channel *nc)
 309 {
 310         struct ncsi_package *np = nc->package;
 311         struct ncsi_channel_filter *ncf;
 312         unsigned long flags;
 313         int i;
 314
 315         /* Release filters */
 316         spin_lock_irqsave(&nc->lock, flags);
 317         for (i = 0; i < NCSI_FILTER_MAX; i++) {
 318                 ncf = nc->filters[i];
 319                 if (!ncf)
 320                         continue;
 321
 322                 nc->filters[i] = NULL;
 323                 kfree(ncf);
 324         }
 325
 326         nc->state = NCSI_CHANNEL_INACTIVE;
 327         spin_unlock_irqrestore(&nc->lock, flags);
 328         ncsi_stop_channel_monitor(nc);
 329
 330         /* Remove and free channel */
 331         spin_lock_irqsave(&np->lock, flags);
 332         list_del_rcu(&nc->node);
 333         np->channel_num--;
 334         spin_unlock_irqrestore(&np->lock, flags);
 335
 336         kfree(nc);
 337 }
 338
 339 struct ncsi_package *ncsi_find_package(struct ncsi_dev_priv *ndp,
 340                                        unsigned char id)
 341 {
 342         struct ncsi_package *np;
 343
 344         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 345                 if (np->id == id)
 346                         return np;
 347         }
 348
 349         return NULL;
 350 }
 351
 352 struct ncsi_package *ncsi_add_package(struct ncsi_dev_priv *ndp,
 353                                       unsigned char id)
 354 {
 355         struct ncsi_package *np, *tmp;
 356         unsigned long flags;
 357
 358         np = kzalloc(sizeof(*np), GFP_ATOMIC);
 359         if (!np)
 360                 return NULL;
 361
 362         np->id = id;
 363         np->ndp = ndp;
 364         spin_lock_init(&np->lock);
 365         INIT_LIST_HEAD(&np->channels);
 366
 367         spin_lock_irqsave(&ndp->lock, flags);
 368         tmp = ncsi_find_package(ndp, id);
 369         if (tmp) {
 370                 spin_unlock_irqrestore(&ndp->lock, flags);
 371                 kfree(np);
 372                 return tmp;
 373         }
 374
 375         list_add_tail_rcu(&np->node, &ndp->packages);
 376         ndp->package_num++;
 377         spin_unlock_irqrestore(&ndp->lock, flags);
 378
 379         return np;
 380 }
 381
 382 void ncsi_remove_package(struct ncsi_package *np)
 383 {
 384         struct ncsi_dev_priv *ndp = np->ndp;
 385         struct ncsi_channel *nc, *tmp;
 386         unsigned long flags;
 387
 388         /* Release all child channels */
 389         list_for_each_entry_safe(nc, tmp, &np->channels, node)
 390                 ncsi_remove_channel(nc);
 391
 392         /* Remove and free package */
 393         spin_lock_irqsave(&ndp->lock, flags);
 394         list_del_rcu(&np->node);
 395         ndp->package_num--;
 396         spin_unlock_irqrestore(&ndp->lock, flags);
 397
 398         kfree(np);
 399 }
 400
 401 void ncsi_find_package_and_channel(struct ncsi_dev_priv *ndp,
 402                                    unsigned char id,
 403                                    struct ncsi_package **np,
 404                                    struct ncsi_channel **nc)
 405 {
 406         struct ncsi_package *p;
 407         struct ncsi_channel *c;
 408
 409         p = ncsi_find_package(ndp, NCSI_PACKAGE_INDEX(id));
 410         c = p ? ncsi_find_channel(p, NCSI_CHANNEL_INDEX(id)) : NULL;
 411
 412         if (np)
 413                 *np = p;
 414         if (nc)
 415                 *nc = c;
 416 }
 417
 418 /* For two consecutive NCSI commands, the packet IDs shouldn't
 419  * be same. Otherwise, the bogus response might be replied. So
 420  * the available IDs are allocated in round-robin fashion.
 421  */
 422 struct ncsi_request *ncsi_alloc_request(struct ncsi_dev_priv *ndp, bool driven)
 423 {
 424         struct ncsi_request *nr = NULL;
 425         int i, limit = ARRAY_SIZE(ndp->requests);
 426         unsigned long flags;
 427
 428         /* Check if there is one available request until the ceiling */
 429         spin_lock_irqsave(&ndp->lock, flags);
 430         for (i = ndp->request_id; !nr && i < limit; i++) {
 431                 if (ndp->requests[i].used)
 432                         continue;
 433
 434                 nr = &ndp->requests[i];
 435                 nr->used = true;
 436                 nr->driven = driven;
 437                 if (++ndp->request_id >= limit)
 438                         ndp->request_id = 0;
 439         }
 440
 441         /* Fail back to check from the starting cursor */
 442         for (i = 0; !nr && i < ndp->request_id; i++) {
 443                 if (ndp->requests[i].used)
 444                         continue;
 445
 446                 nr = &ndp->requests[i];
 447                 nr->used = true;
 448                 nr->driven = driven;
 449                 if (++ndp->request_id >= limit)
 450                         ndp->request_id = 0;
 451         }
 452         spin_unlock_irqrestore(&ndp->lock, flags);
 453
 454         return nr;
 455 }
 456
 457 void ncsi_free_request(struct ncsi_request *nr)
 458 {
 459         struct ncsi_dev_priv *ndp = nr->ndp;
 460         struct sk_buff *cmd, *rsp;
 461         unsigned long flags;
 462         bool driven;
 463
 464         if (nr->enabled) {
 465                 nr->enabled = false;
 466                 del_timer_sync(&nr->timer);
 467         }
 468
 469         spin_lock_irqsave(&ndp->lock, flags);
 470         cmd = nr->cmd;
 471         rsp = nr->rsp;
 472         nr->cmd = NULL;
 473         nr->rsp = NULL;
 474         nr->used = false;
 475         driven = nr->driven;
 476         spin_unlock_irqrestore(&ndp->lock, flags);
 477
 478         if (driven && cmd && --ndp->pending_req_num == 0)
 479                 schedule_work(&ndp->work);
 480
 481         /* Release command and response */
 482         consume_skb(cmd);
 483         consume_skb(rsp);
 484 }
 485
 486 struct ncsi_dev *ncsi_find_dev(struct net_device *dev)
 487 {
 488         struct ncsi_dev_priv *ndp;
 489
 490         NCSI_FOR_EACH_DEV(ndp) {
 491                 if (ndp->ndev.dev == dev)
 492                         return &ndp->ndev;
 493         }
 494
 495         return NULL;
 496 }
 497
 498 static void ncsi_request_timeout(unsigned long data)
 499 {
 500         struct ncsi_request *nr = (struct ncsi_request *)data;
 501         struct ncsi_dev_priv *ndp = nr->ndp;
 502         unsigned long flags;
 503
 504         /* If the request already had associated response,
 505          * let the response handler to release it.
 506          */
 507         spin_lock_irqsave(&ndp->lock, flags);
 508         nr->enabled = false;
 509         if (nr->rsp || !nr->cmd) {
 510                 spin_unlock_irqrestore(&ndp->lock, flags);
 511                 return;
 512         }
 513         spin_unlock_irqrestore(&ndp->lock, flags);
 514
 515         /* Release the request */
 516         ncsi_free_request(nr);
 517 }
 518
 519 static void ncsi_suspend_channel(struct ncsi_dev_priv *ndp)
 520 {
 521         struct ncsi_dev *nd = &ndp->ndev;
 522         struct ncsi_package *np = ndp->active_package;
 523         struct ncsi_channel *nc = ndp->active_channel;
 524         struct ncsi_cmd_arg nca;
 525         unsigned long flags;
 526         int ret;
 527
 528         nca.ndp = ndp;
 529         nca.driven = true;
 530         switch (nd->state) {
 531         case ncsi_dev_state_suspend:
 532                 nd->state = ncsi_dev_state_suspend_select;
 533                 /* Fall through */
 534         case ncsi_dev_state_suspend_select:
 535         case ncsi_dev_state_suspend_dcnt:
 536         case ncsi_dev_state_suspend_dc:
 537         case ncsi_dev_state_suspend_deselect:
 538                 ndp->pending_req_num = 1;
 539
 540                 np = ndp->active_package;
 541                 nc = ndp->active_channel;
 542                 nca.package = np->id;
 543                 if (nd->state == ncsi_dev_state_suspend_select) {
 544                         nca.type = NCSI_PKT_CMD_SP;
 545                         nca.channel = NCSI_RESERVED_CHANNEL;
 546                         if (ndp->flags & NCSI_DEV_HWA)
 547                                 nca.bytes[0] = 0;
 548                         else
 549                                 nca.bytes[0] = 1;
 550                         nd->state = ncsi_dev_state_suspend_dcnt;
 551                 } else if (nd->state == ncsi_dev_state_suspend_dcnt) {
 552                         nca.type = NCSI_PKT_CMD_DCNT;
 553                         nca.channel = nc->id;
 554                         nd->state = ncsi_dev_state_suspend_dc;
 555                 } else if (nd->state == ncsi_dev_state_suspend_dc) {
 556                         nca.type = NCSI_PKT_CMD_DC;
 557                         nca.channel = nc->id;
 558                         nca.bytes[0] = 1;
 559                         nd->state = ncsi_dev_state_suspend_deselect;
 560                 } else if (nd->state == ncsi_dev_state_suspend_deselect) {
 561                         nca.type = NCSI_PKT_CMD_DP;
 562                         nca.channel = NCSI_RESERVED_CHANNEL;
 563                         nd->state = ncsi_dev_state_suspend_done;
 564                 }
 565
 566                 ret = ncsi_xmit_cmd(&nca);
 567                 if (ret) {
 568                         nd->state = ncsi_dev_state_functional;
 569                         return;
 570                 }
 571
 572                 break;
 573         case ncsi_dev_state_suspend_done:
 574                 spin_lock_irqsave(&nc->lock, flags);
 575                 nc->state = NCSI_CHANNEL_INACTIVE;
 576                 spin_unlock_irqrestore(&nc->lock, flags);
 577                 ncsi_process_next_channel(ndp);
 578
 579                 break;
 580         default:
 581                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in suspend\n",
 582                             nd->state);
 583         }
 584 }
 585
 586 static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
 587 {
 588         struct ncsi_dev *nd = &ndp->ndev;
 589         struct net_device *dev = nd->dev;
 590         struct ncsi_package *np = ndp->active_package;
 591         struct ncsi_channel *nc = ndp->active_channel;
 592         struct ncsi_cmd_arg nca;
 593         unsigned char index;
 594         unsigned long flags;
 595         int ret;
 596
 597         nca.ndp = ndp;
 598         nca.driven = true;
 599         switch (nd->state) {
 600         case ncsi_dev_state_config:
 601         case ncsi_dev_state_config_sp:
 602                 ndp->pending_req_num = 1;
 603
 604                 /* Select the specific package */
 605                 nca.type = NCSI_PKT_CMD_SP;
 606                 if (ndp->flags & NCSI_DEV_HWA)
 607                         nca.bytes[0] = 0;
 608                 else
 609                         nca.bytes[0] = 1;
 610                 nca.package = np->id;
 611                 nca.channel = NCSI_RESERVED_CHANNEL;
 612                 ret = ncsi_xmit_cmd(&nca);
 613                 if (ret)
 614                         goto error;
 615
 616                 nd->state = ncsi_dev_state_config_cis;
 617                 break;
 618         case ncsi_dev_state_config_cis:
 619                 ndp->pending_req_num = 1;
 620
 621                 /* Clear initial state */
 622                 nca.type = NCSI_PKT_CMD_CIS;
 623                 nca.package = np->id;
 624                 nca.channel = nc->id;
 625                 ret = ncsi_xmit_cmd(&nca);
 626                 if (ret)
 627                         goto error;
 628
 629                 nd->state = ncsi_dev_state_config_sma;
 630                 break;
 631         case ncsi_dev_state_config_sma:
 632         case ncsi_dev_state_config_ebf:
 633 #if IS_ENABLED(CONFIG_IPV6)
 634         case ncsi_dev_state_config_egmf:
 635 #endif
 636         case ncsi_dev_state_config_ecnt:
 637         case ncsi_dev_state_config_ec:
 638         case ncsi_dev_state_config_ae:
 639         case ncsi_dev_state_config_gls:
 640                 ndp->pending_req_num = 1;
 641
 642                 nca.package = np->id;
 643                 nca.channel = nc->id;
 644
 645                 /* Use first entry in unicast filter table. Note that
 646                  * the MAC filter table starts from entry 1 instead of
 647                  * 0.
 648                  */
 649                 if (nd->state == ncsi_dev_state_config_sma) {
 650                         nca.type = NCSI_PKT_CMD_SMA;
 651                         for (index = 0; index < 6; index++)
 652                                 nca.bytes[index] = dev->dev_addr[index];
 653                         nca.bytes[6] = 0x1;
 654                         nca.bytes[7] = 0x1;
 655                         nd->state = ncsi_dev_state_config_ebf;
 656                 } else if (nd->state == ncsi_dev_state_config_ebf) {
 657                         nca.type = NCSI_PKT_CMD_EBF;
 658                         nca.dwords[0] = nc->caps[NCSI_CAP_BC].cap;
 659                         nd->state = ncsi_dev_state_config_ecnt;
 660 #if IS_ENABLED(CONFIG_IPV6)
 661                         if (ndp->inet6_addr_num > 0 &&
 662                             (nc->caps[NCSI_CAP_GENERIC].cap &
 663                              NCSI_CAP_GENERIC_MC))
 664                                 nd->state = ncsi_dev_state_config_egmf;
 665                         else
 666                                 nd->state = ncsi_dev_state_config_ecnt;
 667                 } else if (nd->state == ncsi_dev_state_config_egmf) {
 668                         nca.type = NCSI_PKT_CMD_EGMF;
 669                         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
 670                         nd->state = ncsi_dev_state_config_ecnt;
 671 #endif /* CONFIG_IPV6 */
 672                 } else if (nd->state == ncsi_dev_state_config_ecnt) {
 673                         nca.type = NCSI_PKT_CMD_ECNT;
 674                         nd->state = ncsi_dev_state_config_ec;
 675                 } else if (nd->state == ncsi_dev_state_config_ec) {
 676                         /* Enable AEN if it's supported */
 677                         nca.type = NCSI_PKT_CMD_EC;
 678                         nd->state = ncsi_dev_state_config_ae;
 679                         if (!(nc->caps[NCSI_CAP_AEN].cap & NCSI_CAP_AEN_MASK))
 680                                 nd->state = ncsi_dev_state_config_gls;
 681                 } else if (nd->state == ncsi_dev_state_config_ae) {
 682                         nca.type = NCSI_PKT_CMD_AE;
 683                         nca.bytes[0] = 0;
 684                         nca.dwords[1] = nc->caps[NCSI_CAP_AEN].cap;
 685                         nd->state = ncsi_dev_state_config_gls;
 686                 } else if (nd->state == ncsi_dev_state_config_gls) {
 687                         nca.type = NCSI_PKT_CMD_GLS;
 688                         nd->state = ncsi_dev_state_config_done;
 689                 }
 690
 691                 ret = ncsi_xmit_cmd(&nca);
 692                 if (ret)
 693                         goto error;
 694                 break;
 695         case ncsi_dev_state_config_done:
 696                 spin_lock_irqsave(&nc->lock, flags);
 697                 if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1)
 698                         nc->state = NCSI_CHANNEL_ACTIVE;
 699                 else
 700                         nc->state = NCSI_CHANNEL_INACTIVE;
 701                 spin_unlock_irqrestore(&nc->lock, flags);
 702
 703                 ncsi_start_channel_monitor(nc);
 704                 ncsi_process_next_channel(ndp);
 705                 break;
 706         default:
 707                 netdev_warn(dev, "Wrong NCSI state 0x%x in config\n",
 708                             nd->state);
 709         }
 710
 711         return;
 712
 713 error:
 714         ncsi_report_link(ndp, true);
 715 }
 716
 717 static int ncsi_choose_active_channel(struct ncsi_dev_priv *ndp)
 718 {
 719         struct ncsi_package *np;
 720         struct ncsi_channel *nc, *found;
 721         struct ncsi_channel_mode *ncm;
 722         unsigned long flags;
 723
 724         /* The search is done once an inactive channel with up
 725          * link is found.
 726          */
 727         found = NULL;
 728         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 729                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 730                         spin_lock_irqsave(&nc->lock, flags);
 731
 732                         if (!list_empty(&nc->link) ||
 733                             nc->state != NCSI_CHANNEL_INACTIVE) {
 734                                 spin_unlock_irqrestore(&nc->lock, flags);
 735                                 continue;
 736                         }
 737
 738                         if (!found)
 739                                 found = nc;
 740
 741                         ncm = &nc->modes[NCSI_MODE_LINK];
 742                         if (ncm->data[2] & 0x1) {
 743                                 spin_unlock_irqrestore(&nc->lock, flags);
 744                                 found = nc;
 745                                 goto out;
 746                         }
 747
 748                         spin_unlock_irqrestore(&nc->lock, flags);
 749                 }
 750         }
 751
 752         if (!found) {
 753                 ncsi_report_link(ndp, true);
 754                 return -ENODEV;
 755         }
 756
 757 out:
 758         spin_lock_irqsave(&ndp->lock, flags);
 759         list_add_tail_rcu(&found->link, &ndp->channel_queue);
 760         spin_unlock_irqrestore(&ndp->lock, flags);
 761
 762         return ncsi_process_next_channel(ndp);
 763 }
 764
 765 static bool ncsi_check_hwa(struct ncsi_dev_priv *ndp)
 766 {
 767         struct ncsi_package *np;
 768         struct ncsi_channel *nc;
 769         unsigned int cap;
 770
 771         /* The hardware arbitration is disabled if any one channel
 772          * doesn't support explicitly.
 773          */
 774         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 775                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 776                         cap = nc->caps[NCSI_CAP_GENERIC].cap;
 777                         if (!(cap & NCSI_CAP_GENERIC_HWA) ||
 778                             (cap & NCSI_CAP_GENERIC_HWA_MASK) !=
 779                             NCSI_CAP_GENERIC_HWA_SUPPORT) {
 780                                 ndp->flags &= ~NCSI_DEV_HWA;
 781                                 return false;
 782                         }
 783                 }
 784         }
 785
 786         ndp->flags |= NCSI_DEV_HWA;
 787         return true;
 788 }
 789
 790 static int ncsi_enable_hwa(struct ncsi_dev_priv *ndp)
 791 {
 792         struct ncsi_package *np;
 793         struct ncsi_channel *nc;
 794         unsigned long flags;
 795
 796         /* Move all available channels to processing queue */
 797         spin_lock_irqsave(&ndp->lock, flags);
 798         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 799                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 800                         WARN_ON_ONCE(nc->state != NCSI_CHANNEL_INACTIVE ||
 801                                      !list_empty(&nc->link));
 802                         ncsi_stop_channel_monitor(nc);
 803                         list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 804                 }
 805         }
 806         spin_unlock_irqrestore(&ndp->lock, flags);
 807
 808         /* We can have no channels in extremely case */
 809         if (list_empty(&ndp->channel_queue)) {
 810                 ncsi_report_link(ndp, false);
 811                 return -ENOENT;
 812         }
 813
 814         return ncsi_process_next_channel(ndp);
 815 }
 816
 817 static void ncsi_probe_channel(struct ncsi_dev_priv *ndp)
 818 {
 819         struct ncsi_dev *nd = &ndp->ndev;
 820         struct ncsi_package *np;
 821         struct ncsi_channel *nc;
 822         struct ncsi_cmd_arg nca;
 823         unsigned char index;
 824         int ret;
 825
 826         nca.ndp = ndp;
 827         nca.driven = true;
 828         switch (nd->state) {
 829         case ncsi_dev_state_probe:
 830                 nd->state = ncsi_dev_state_probe_deselect;
 831                 /* Fall through */
 832         case ncsi_dev_state_probe_deselect:
 833                 ndp->pending_req_num = 8;
 834
 835                 /* Deselect all possible packages */
 836                 nca.type = NCSI_PKT_CMD_DP;
 837                 nca.channel = NCSI_RESERVED_CHANNEL;
 838                 for (index = 0; index < 8; index++) {
 839                         nca.package = index;
 840                         ret = ncsi_xmit_cmd(&nca);
 841                         if (ret)
 842                                 goto error;
 843                 }
 844
 845                 nd->state = ncsi_dev_state_probe_package;
 846                 break;
 847         case ncsi_dev_state_probe_package:
 848                 ndp->pending_req_num = 16;
 849
 850                 /* Select all possible packages */
 851                 nca.type = NCSI_PKT_CMD_SP;
 852                 nca.bytes[0] = 1;
 853                 nca.channel = NCSI_RESERVED_CHANNEL;
 854                 for (index = 0; index < 8; index++) {
 855                         nca.package = index;
 856                         ret = ncsi_xmit_cmd(&nca);
 857                         if (ret)
 858                                 goto error;
 859                 }
 860
 861                 /* Disable all possible packages */
 862                 nca.type = NCSI_PKT_CMD_DP;
 863                 for (index = 0; index < 8; index++) {
 864                         nca.package = index;
 865                         ret = ncsi_xmit_cmd(&nca);
 866                         if (ret)
 867                                 goto error;
 868                 }
 869
 870                 nd->state = ncsi_dev_state_probe_channel;
 871                 break;
 872         case ncsi_dev_state_probe_channel:
 873                 if (!ndp->active_package)
 874                         ndp->active_package = list_first_or_null_rcu(
 875                                 &ndp->packages, struct ncsi_package, node);
 876                 else if (list_is_last(&ndp->active_package->node,
 877                                       &ndp->packages))
 878                         ndp->active_package = NULL;
 879                 else
 880                         ndp->active_package = list_next_entry(
 881                                 ndp->active_package, node);
 882
 883                 /* All available packages and channels are enumerated. The
 884                  * enumeration happens for once when the NCSI interface is
 885                  * started. So we need continue to start the interface after
 886                  * the enumeration.
 887                  *
 888                  * We have to choose an active channel before configuring it.
 889                  * Note that we possibly don't have active channel in extreme
 890                  * situation.
 891                  */
 892                 if (!ndp->active_package) {
 893                         ndp->flags |= NCSI_DEV_PROBED;
 894                         if (ncsi_check_hwa(ndp))
 895                                 ncsi_enable_hwa(ndp);
 896                         else
 897                                 ncsi_choose_active_channel(ndp);
 898                         return;
 899                 }
 900
 901                 /* Select the active package */
 902                 ndp->pending_req_num = 1;
 903                 nca.type = NCSI_PKT_CMD_SP;
 904                 nca.bytes[0] = 1;
 905                 nca.package = ndp->active_package->id;
 906                 nca.channel = NCSI_RESERVED_CHANNEL;
 907                 ret = ncsi_xmit_cmd(&nca);
 908                 if (ret)
 909                         goto error;
 910
 911                 nd->state = ncsi_dev_state_probe_cis;
 912                 break;
 913         case ncsi_dev_state_probe_cis:
 914                 ndp->pending_req_num = 32;
 915
 916                 /* Clear initial state */
 917                 nca.type = NCSI_PKT_CMD_CIS;
 918                 nca.package = ndp->active_package->id;
 919                 for (index = 0; index < 0x20; index++) {
 920                         nca.channel = index;
 921                         ret = ncsi_xmit_cmd(&nca);
 922                         if (ret)
 923                                 goto error;
 924                 }
 925
 926                 nd->state = ncsi_dev_state_probe_gvi;
 927                 break;
 928         case ncsi_dev_state_probe_gvi:
 929         case ncsi_dev_state_probe_gc:
 930         case ncsi_dev_state_probe_gls:
 931                 np = ndp->active_package;
 932                 ndp->pending_req_num = np->channel_num;
 933
 934                 /* Retrieve version, capability or link status */
 935                 if (nd->state == ncsi_dev_state_probe_gvi)
 936                         nca.type = NCSI_PKT_CMD_GVI;
 937                 else if (nd->state == ncsi_dev_state_probe_gc)
 938                         nca.type = NCSI_PKT_CMD_GC;
 939                 else
 940                         nca.type = NCSI_PKT_CMD_GLS;
 941
 942                 nca.package = np->id;
 943                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 944                         nca.channel = nc->id;
 945                         ret = ncsi_xmit_cmd(&nca);
 946                         if (ret)
 947                                 goto error;
 948                 }
 949
 950                 if (nd->state == ncsi_dev_state_probe_gvi)
 951                         nd->state = ncsi_dev_state_probe_gc;
 952                 else if (nd->state == ncsi_dev_state_probe_gc)
 953                         nd->state = ncsi_dev_state_probe_gls;
 954                 else
 955                         nd->state = ncsi_dev_state_probe_dp;
 956                 break;
 957         case ncsi_dev_state_probe_dp:
 958                 ndp->pending_req_num = 1;
 959
 960                 /* Deselect the active package */
 961                 nca.type = NCSI_PKT_CMD_DP;
 962                 nca.package = ndp->active_package->id;
 963                 nca.channel = NCSI_RESERVED_CHANNEL;
 964                 ret = ncsi_xmit_cmd(&nca);
 965                 if (ret)
 966                         goto error;
 967
 968                 /* Scan channels in next package */
 969                 nd->state = ncsi_dev_state_probe_channel;
 970                 break;
 971         default:
 972                 netdev_warn(nd->dev, "Wrong NCSI state 0x%0x in enumeration\n",
 973                             nd->state);
 974         }
 975
 976         return;
 977 error:
 978         ncsi_report_link(ndp, true);
 979 }
 980
 981 static void ncsi_dev_work(struct work_struct *work)
 982 {
 983         struct ncsi_dev_priv *ndp = container_of(work,
 984                         struct ncsi_dev_priv, work);
 985         struct ncsi_dev *nd = &ndp->ndev;
 986
 987         switch (nd->state & ncsi_dev_state_major) {
 988         case ncsi_dev_state_probe:
 989                 ncsi_probe_channel(ndp);
 990                 break;
 991         case ncsi_dev_state_suspend:
 992                 ncsi_suspend_channel(ndp);
 993                 break;
 994         case ncsi_dev_state_config:
 995                 ncsi_configure_channel(ndp);
 996                 break;
 997         default:
 998                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in workqueue\n",
 999                             nd->state);
1000         }
1001 }
1002
1003 int ncsi_process_next_channel(struct ncsi_dev_priv *ndp)
1004 {
1005         struct ncsi_channel *nc;
1006         int old_state;
1007         unsigned long flags;
1008
1009         spin_lock_irqsave(&ndp->lock, flags);
1010         nc = list_first_or_null_rcu(&ndp->channel_queue,
1011                                     struct ncsi_channel, link);
1012         if (!nc) {
1013                 spin_unlock_irqrestore(&ndp->lock, flags);
1014                 goto out;
1015         }
1016
1017         list_del_init(&nc->link);
1018         spin_unlock_irqrestore(&ndp->lock, flags);
1019
1020         spin_lock_irqsave(&nc->lock, flags);
1021         old_state = nc->state;
1022         nc->state = NCSI_CHANNEL_INVISIBLE;
1023         spin_unlock_irqrestore(&nc->lock, flags);
1024
1025         ndp->active_channel = nc;
1026         ndp->active_package = nc->package;
1027
1028         switch (old_state) {
1029         case NCSI_CHANNEL_INACTIVE:
1030                 ndp->ndev.state = ncsi_dev_state_config;
1031                 ncsi_configure_channel(ndp);
1032                 break;
1033         case NCSI_CHANNEL_ACTIVE:
1034                 ndp->ndev.state = ncsi_dev_state_suspend;
1035                 ncsi_suspend_channel(ndp);
1036                 break;
1037         default:
1038                 netdev_err(ndp->ndev.dev, "Invalid state 0x%x on %d:%d\n",
1039                            old_state, nc->package->id, nc->id);
1040                 ncsi_report_link(ndp, false);
1041                 return -EINVAL;
1042         }
1043
1044         return 0;
1045
1046 out:
1047         ndp->active_channel = NULL;
1048         ndp->active_package = NULL;
1049         if (ndp->flags & NCSI_DEV_RESHUFFLE) {
1050                 ndp->flags &= ~NCSI_DEV_RESHUFFLE;
1051                 return ncsi_choose_active_channel(ndp);
1052         }
1053
1054         ncsi_report_link(ndp, false);
1055         return -ENODEV;
1056 }
1057
1058 #if IS_ENABLED(CONFIG_IPV6)
1059 static int ncsi_inet6addr_event(struct notifier_block *this,
1060                                 unsigned long event, void *data)
1061 {
1062         struct inet6_ifaddr *ifa = data;
1063         struct net_device *dev = ifa->idev->dev;
1064         struct ncsi_dev *nd = ncsi_find_dev(dev);
1065         struct ncsi_dev_priv *ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
1066         struct ncsi_package *np;
1067         struct ncsi_channel *nc;
1068         struct ncsi_cmd_arg nca;
1069         bool action;
1070         int ret;
1071
1072         if (!ndp || (ipv6_addr_type(&ifa->addr) &
1073             (IPV6_ADDR_LINKLOCAL | IPV6_ADDR_LOOPBACK)))
1074                 return NOTIFY_OK;
1075
1076         switch (event) {
1077         case NETDEV_UP:
1078                 action = (++ndp->inet6_addr_num) == 1;
1079                 nca.type = NCSI_PKT_CMD_EGMF;
1080                 break;
1081         case NETDEV_DOWN:
1082                 action = (--ndp->inet6_addr_num == 0);
1083                 nca.type = NCSI_PKT_CMD_DGMF;
1084                 break;
1085         default:
1086                 return NOTIFY_OK;
1087         }
1088
1089         /* We might not have active channel or packages. The IPv6
1090          * required multicast will be enabled when active channel
1091          * or packages are chosen.
1092          */
1093         np = ndp->active_package;
1094         nc = ndp->active_channel;
1095         if (!action || !np || !nc)
1096                 return NOTIFY_OK;
1097
1098         /* We needn't enable or disable it if the function isn't supported */
1099         if (!(nc->caps[NCSI_CAP_GENERIC].cap & NCSI_CAP_GENERIC_MC))
1100                 return NOTIFY_OK;
1101
1102         nca.ndp = ndp;
1103         nca.driven = false;
1104         nca.package = np->id;
1105         nca.channel = nc->id;
1106         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
1107         ret = ncsi_xmit_cmd(&nca);
1108         if (ret) {
1109                 netdev_warn(dev, "Fail to %s global multicast filter (%d)\n",
1110                             (event == NETDEV_UP) ? "enable" : "disable", ret);
1111                 return NOTIFY_DONE;
1112         }
1113
1114         return NOTIFY_OK;
1115 }
1116
1117 static struct notifier_block ncsi_inet6addr_notifier = {
1118         .notifier_call = ncsi_inet6addr_event,
1119 };
1120 #endif /* CONFIG_IPV6 */
1121
1122 struct ncsi_dev *ncsi_register_dev(struct net_device *dev,
1123                                    void (*handler)(struct ncsi_dev *ndev))
1124 {
1125         struct ncsi_dev_priv *ndp;
1126         struct ncsi_dev *nd;
1127         unsigned long flags;
1128         int i;
1129
1130         /* Check if the device has been registered or not */
1131         nd = ncsi_find_dev(dev);
1132         if (nd)
1133                 return nd;
1134
1135         /* Create NCSI device */
1136         ndp = kzalloc(sizeof(*ndp), GFP_ATOMIC);
1137         if (!ndp)
1138                 return NULL;
1139
1140         nd = &ndp->ndev;
1141         nd->state = ncsi_dev_state_registered;
1142         nd->dev = dev;
1143         nd->handler = handler;
1144         ndp->pending_req_num = 0;
1145         INIT_LIST_HEAD(&ndp->channel_queue);
1146         INIT_WORK(&ndp->work, ncsi_dev_work);
1147
1148         /* Initialize private NCSI device */
1149         spin_lock_init(&ndp->lock);
1150         INIT_LIST_HEAD(&ndp->packages);
1151         ndp->request_id = 0;
1152         for (i = 0; i < ARRAY_SIZE(ndp->requests); i++) {
1153                 ndp->requests[i].id = i;
1154                 ndp->requests[i].ndp = ndp;
1155                 setup_timer(&ndp->requests[i].timer,
1156                             ncsi_request_timeout,
1157                             (unsigned long)&ndp->requests[i]);
1158         }
1159
1160         spin_lock_irqsave(&ncsi_dev_lock, flags);
1161 #if IS_ENABLED(CONFIG_IPV6)
1162         ndp->inet6_addr_num = 0;
1163         if (list_empty(&ncsi_dev_list))
1164                 register_inet6addr_notifier(&ncsi_inet6addr_notifier);
1165 #endif
1166         list_add_tail_rcu(&ndp->node, &ncsi_dev_list);
1167         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1168
1169         /* Register NCSI packet Rx handler */
1170         ndp->ptype.type = cpu_to_be16(ETH_P_NCSI);
1171         ndp->ptype.func = ncsi_rcv_rsp;
1172         ndp->ptype.dev = dev;
1173         dev_add_pack(&ndp->ptype);
1174
1175         return nd;
1176 }
1177 EXPORT_SYMBOL_GPL(ncsi_register_dev);
1178
1179 int ncsi_start_dev(struct ncsi_dev *nd)
1180 {
1181         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1182         struct ncsi_package *np;
1183         struct ncsi_channel *nc;
1184         unsigned long flags;
1185         bool chained;
1186         int old_state, ret;
1187
1188         if (nd->state != ncsi_dev_state_registered &&
1189             nd->state != ncsi_dev_state_functional)
1190                 return -ENOTTY;
1191
1192         if (!(ndp->flags & NCSI_DEV_PROBED)) {
1193                 nd->state = ncsi_dev_state_probe;
1194                 schedule_work(&ndp->work);
1195                 return 0;
1196         }
1197
1198         /* Reset channel's state and start over */
1199         NCSI_FOR_EACH_PACKAGE(ndp, np) {
1200                 NCSI_FOR_EACH_CHANNEL(np, nc) {
1201                         spin_lock_irqsave(&nc->lock, flags);
1202                         chained = !list_empty(&nc->link);
1203                         old_state = nc->state;
1204                         nc->state = NCSI_CHANNEL_INACTIVE;
1205                         spin_unlock_irqrestore(&nc->lock, flags);
1206
1207                         WARN_ON_ONCE(chained ||
1208                                      old_state == NCSI_CHANNEL_INVISIBLE);
1209                 }
1210         }
1211
1212         if (ndp->flags & NCSI_DEV_HWA)
1213                 ret = ncsi_enable_hwa(ndp);
1214         else
1215                 ret = ncsi_choose_active_channel(ndp);
1216
1217         return ret;
1218 }
1219 EXPORT_SYMBOL_GPL(ncsi_start_dev);
1220
1221 void ncsi_unregister_dev(struct ncsi_dev *nd)
1222 {
1223         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1224         struct ncsi_package *np, *tmp;
1225         unsigned long flags;
1226
1227         dev_remove_pack(&ndp->ptype);
1228
1229         list_for_each_entry_safe(np, tmp, &ndp->packages, node)
1230                 ncsi_remove_package(np);
1231
1232         spin_lock_irqsave(&ncsi_dev_lock, flags);
1233         list_del_rcu(&ndp->node);
1234 #if IS_ENABLED(CONFIG_IPV6)
1235         if (list_empty(&ncsi_dev_list))
1236                 unregister_inet6addr_notifier(&ncsi_inet6addr_notifier);
1237 #endif
1238         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1239
1240         kfree(ndp);
1241 }
1242 EXPORT_SYMBOL_GPL(ncsi_unregister_dev);