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