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.req_flags = 0;
 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,
 423                                         unsigned int req_flags)
 424 {
 425         struct ncsi_request *nr = NULL;
 426         int i, limit = ARRAY_SIZE(ndp->requests);
 427         unsigned long flags;
 428
 429         /* Check if there is one available request until the ceiling */
 430         spin_lock_irqsave(&ndp->lock, flags);
 431         for (i = ndp->request_id; i < limit; i++) {
 432                 if (ndp->requests[i].used)
 433                         continue;
 434
 435                 nr = &ndp->requests[i];
 436                 nr->used = true;
 437                 nr->flags = req_flags;
 438                 ndp->request_id = i + 1;
 439                 goto found;
 440         }
 441
 442         /* Fail back to check from the starting cursor */
 443         for (i = NCSI_REQ_START_IDX; i < ndp->request_id; i++) {
 444                 if (ndp->requests[i].used)
 445                         continue;
 446
 447                 nr = &ndp->requests[i];
 448                 nr->used = true;
 449                 nr->flags = req_flags;
 450                 ndp->request_id = i + 1;
 451                 goto found;
 452         }
 453
 454 found:
 455         spin_unlock_irqrestore(&ndp->lock, flags);
 456         return nr;
 457 }
 458
 459 void ncsi_free_request(struct ncsi_request *nr)
 460 {
 461         struct ncsi_dev_priv *ndp = nr->ndp;
 462         struct sk_buff *cmd, *rsp;
 463         unsigned long flags;
 464         bool driven;
 465
 466         if (nr->enabled) {
 467                 nr->enabled = false;
 468                 del_timer_sync(&nr->timer);
 469         }
 470
 471         spin_lock_irqsave(&ndp->lock, flags);
 472         cmd = nr->cmd;
 473         rsp = nr->rsp;
 474         nr->cmd = NULL;
 475         nr->rsp = NULL;
 476         nr->used = false;
 477         driven = !!(nr->flags & NCSI_REQ_FLAG_EVENT_DRIVEN);
 478         spin_unlock_irqrestore(&ndp->lock, flags);
 479
 480         if (driven && cmd && --ndp->pending_req_num == 0)
 481                 schedule_work(&ndp->work);
 482
 483         /* Release command and response */
 484         consume_skb(cmd);
 485         consume_skb(rsp);
 486 }
 487
 488 struct ncsi_dev *ncsi_find_dev(struct net_device *dev)
 489 {
 490         struct ncsi_dev_priv *ndp;
 491
 492         NCSI_FOR_EACH_DEV(ndp) {
 493                 if (ndp->ndev.dev == dev)
 494                         return &ndp->ndev;
 495         }
 496
 497         return NULL;
 498 }
 499
 500 static void ncsi_request_timeout(unsigned long data)
 501 {
 502         struct ncsi_request *nr = (struct ncsi_request *)data;
 503         struct ncsi_dev_priv *ndp = nr->ndp;
 504         unsigned long flags;
 505
 506         /* If the request already had associated response,
 507          * let the response handler to release it.
 508          */
 509         spin_lock_irqsave(&ndp->lock, flags);
 510         nr->enabled = false;
 511         if (nr->rsp || !nr->cmd) {
 512                 spin_unlock_irqrestore(&ndp->lock, flags);
 513                 return;
 514         }
 515         spin_unlock_irqrestore(&ndp->lock, flags);
 516
 517         /* Release the request */
 518         ncsi_free_request(nr);
 519 }
 520
 521 static void ncsi_suspend_channel(struct ncsi_dev_priv *ndp)
 522 {
 523         struct ncsi_dev *nd = &ndp->ndev;
 524         struct ncsi_package *np = ndp->active_package;
 525         struct ncsi_channel *nc = ndp->active_channel;
 526         struct ncsi_cmd_arg nca;
 527         unsigned long flags;
 528         int ret;
 529
 530         nca.ndp = ndp;
 531         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 532         switch (nd->state) {
 533         case ncsi_dev_state_suspend:
 534                 nd->state = ncsi_dev_state_suspend_select;
 535                 /* Fall through */
 536         case ncsi_dev_state_suspend_select:
 537         case ncsi_dev_state_suspend_dcnt:
 538         case ncsi_dev_state_suspend_dc:
 539         case ncsi_dev_state_suspend_deselect:
 540                 ndp->pending_req_num = 1;
 541
 542                 np = ndp->active_package;
 543                 nc = ndp->active_channel;
 544                 nca.package = np->id;
 545                 if (nd->state == ncsi_dev_state_suspend_select) {
 546                         nca.type = NCSI_PKT_CMD_SP;
 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                         nd->state = ncsi_dev_state_suspend_dcnt;
 553                 } else if (nd->state == ncsi_dev_state_suspend_dcnt) {
 554                         nca.type = NCSI_PKT_CMD_DCNT;
 555                         nca.channel = nc->id;
 556                         nd->state = ncsi_dev_state_suspend_dc;
 557                 } else if (nd->state == ncsi_dev_state_suspend_dc) {
 558                         nca.type = NCSI_PKT_CMD_DC;
 559                         nca.channel = nc->id;
 560                         nca.bytes[0] = 1;
 561                         nd->state = ncsi_dev_state_suspend_deselect;
 562                 } else if (nd->state == ncsi_dev_state_suspend_deselect) {
 563                         nca.type = NCSI_PKT_CMD_DP;
 564                         nca.channel = NCSI_RESERVED_CHANNEL;
 565                         nd->state = ncsi_dev_state_suspend_done;
 566                 }
 567
 568                 ret = ncsi_xmit_cmd(&nca);
 569                 if (ret) {
 570                         nd->state = ncsi_dev_state_functional;
 571                         return;
 572                 }
 573
 574                 break;
 575         case ncsi_dev_state_suspend_done:
 576                 spin_lock_irqsave(&nc->lock, flags);
 577                 nc->state = NCSI_CHANNEL_INACTIVE;
 578                 spin_unlock_irqrestore(&nc->lock, flags);
 579                 ncsi_process_next_channel(ndp);
 580
 581                 break;
 582         default:
 583                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in suspend\n",
 584                             nd->state);
 585         }
 586 }
 587
 588 static void ncsi_configure_channel(struct ncsi_dev_priv *ndp)
 589 {
 590         struct ncsi_dev *nd = &ndp->ndev;
 591         struct net_device *dev = nd->dev;
 592         struct ncsi_package *np = ndp->active_package;
 593         struct ncsi_channel *nc = ndp->active_channel;
 594         struct ncsi_cmd_arg nca;
 595         unsigned char index;
 596         unsigned long flags;
 597         int ret;
 598
 599         nca.ndp = ndp;
 600         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 601         switch (nd->state) {
 602         case ncsi_dev_state_config:
 603         case ncsi_dev_state_config_sp:
 604                 ndp->pending_req_num = 1;
 605
 606                 /* Select the specific package */
 607                 nca.type = NCSI_PKT_CMD_SP;
 608                 if (ndp->flags & NCSI_DEV_HWA)
 609                         nca.bytes[0] = 0;
 610                 else
 611                         nca.bytes[0] = 1;
 612                 nca.package = np->id;
 613                 nca.channel = NCSI_RESERVED_CHANNEL;
 614                 ret = ncsi_xmit_cmd(&nca);
 615                 if (ret)
 616                         goto error;
 617
 618                 nd->state = ncsi_dev_state_config_cis;
 619                 break;
 620         case ncsi_dev_state_config_cis:
 621                 ndp->pending_req_num = 1;
 622
 623                 /* Clear initial state */
 624                 nca.type = NCSI_PKT_CMD_CIS;
 625                 nca.package = np->id;
 626                 nca.channel = nc->id;
 627                 ret = ncsi_xmit_cmd(&nca);
 628                 if (ret)
 629                         goto error;
 630
 631                 nd->state = ncsi_dev_state_config_sma;
 632                 break;
 633         case ncsi_dev_state_config_sma:
 634         case ncsi_dev_state_config_ebf:
 635 #if IS_ENABLED(CONFIG_IPV6)
 636         case ncsi_dev_state_config_egmf:
 637 #endif
 638         case ncsi_dev_state_config_ecnt:
 639         case ncsi_dev_state_config_ec:
 640         case ncsi_dev_state_config_ae:
 641         case ncsi_dev_state_config_gls:
 642                 ndp->pending_req_num = 1;
 643
 644                 nca.package = np->id;
 645                 nca.channel = nc->id;
 646
 647                 /* Use first entry in unicast filter table. Note that
 648                  * the MAC filter table starts from entry 1 instead of
 649                  * 0.
 650                  */
 651                 if (nd->state == ncsi_dev_state_config_sma) {
 652                         nca.type = NCSI_PKT_CMD_SMA;
 653                         for (index = 0; index < 6; index++)
 654                                 nca.bytes[index] = dev->dev_addr[index];
 655                         nca.bytes[6] = 0x1;
 656                         nca.bytes[7] = 0x1;
 657                         nd->state = ncsi_dev_state_config_ebf;
 658                 } else if (nd->state == ncsi_dev_state_config_ebf) {
 659                         nca.type = NCSI_PKT_CMD_EBF;
 660                         nca.dwords[0] = nc->caps[NCSI_CAP_BC].cap;
 661                         nd->state = ncsi_dev_state_config_ecnt;
 662 #if IS_ENABLED(CONFIG_IPV6)
 663                         if (ndp->inet6_addr_num > 0 &&
 664                             (nc->caps[NCSI_CAP_GENERIC].cap &
 665                              NCSI_CAP_GENERIC_MC))
 666                                 nd->state = ncsi_dev_state_config_egmf;
 667                         else
 668                                 nd->state = ncsi_dev_state_config_ecnt;
 669                 } else if (nd->state == ncsi_dev_state_config_egmf) {
 670                         nca.type = NCSI_PKT_CMD_EGMF;
 671                         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
 672                         nd->state = ncsi_dev_state_config_ecnt;
 673 #endif /* CONFIG_IPV6 */
 674                 } else if (nd->state == ncsi_dev_state_config_ecnt) {
 675                         nca.type = NCSI_PKT_CMD_ECNT;
 676                         nd->state = ncsi_dev_state_config_ec;
 677                 } else if (nd->state == ncsi_dev_state_config_ec) {
 678                         /* Enable AEN if it's supported */
 679                         nca.type = NCSI_PKT_CMD_EC;
 680                         nd->state = ncsi_dev_state_config_ae;
 681                         if (!(nc->caps[NCSI_CAP_AEN].cap & NCSI_CAP_AEN_MASK))
 682                                 nd->state = ncsi_dev_state_config_gls;
 683                 } else if (nd->state == ncsi_dev_state_config_ae) {
 684                         nca.type = NCSI_PKT_CMD_AE;
 685                         nca.bytes[0] = 0;
 686                         nca.dwords[1] = nc->caps[NCSI_CAP_AEN].cap;
 687                         nd->state = ncsi_dev_state_config_gls;
 688                 } else if (nd->state == ncsi_dev_state_config_gls) {
 689                         nca.type = NCSI_PKT_CMD_GLS;
 690                         nd->state = ncsi_dev_state_config_done;
 691                 }
 692
 693                 ret = ncsi_xmit_cmd(&nca);
 694                 if (ret)
 695                         goto error;
 696                 break;
 697         case ncsi_dev_state_config_done:
 698                 spin_lock_irqsave(&nc->lock, flags);
 699                 if (nc->modes[NCSI_MODE_LINK].data[2] & 0x1)
 700                         nc->state = NCSI_CHANNEL_ACTIVE;
 701                 else
 702                         nc->state = NCSI_CHANNEL_INACTIVE;
 703                 spin_unlock_irqrestore(&nc->lock, flags);
 704
 705                 ncsi_start_channel_monitor(nc);
 706                 ncsi_process_next_channel(ndp);
 707                 break;
 708         default:
 709                 netdev_warn(dev, "Wrong NCSI state 0x%x in config\n",
 710                             nd->state);
 711         }
 712
 713         return;
 714
 715 error:
 716         ncsi_report_link(ndp, true);
 717 }
 718
 719 static int ncsi_choose_active_channel(struct ncsi_dev_priv *ndp)
 720 {
 721         struct ncsi_package *np;
 722         struct ncsi_channel *nc, *found;
 723         struct ncsi_channel_mode *ncm;
 724         unsigned long flags;
 725
 726         /* The search is done once an inactive channel with up
 727          * link is found.
 728          */
 729         found = NULL;
 730         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 731                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 732                         spin_lock_irqsave(&nc->lock, flags);
 733
 734                         if (!list_empty(&nc->link) ||
 735                             nc->state != NCSI_CHANNEL_INACTIVE) {
 736                                 spin_unlock_irqrestore(&nc->lock, flags);
 737                                 continue;
 738                         }
 739
 740                         if (!found)
 741                                 found = nc;
 742
 743                         ncm = &nc->modes[NCSI_MODE_LINK];
 744                         if (ncm->data[2] & 0x1) {
 745                                 spin_unlock_irqrestore(&nc->lock, flags);
 746                                 found = nc;
 747                                 goto out;
 748                         }
 749
 750                         spin_unlock_irqrestore(&nc->lock, flags);
 751                 }
 752         }
 753
 754         if (!found) {
 755                 ncsi_report_link(ndp, true);
 756                 return -ENODEV;
 757         }
 758
 759 out:
 760         spin_lock_irqsave(&ndp->lock, flags);
 761         list_add_tail_rcu(&found->link, &ndp->channel_queue);
 762         spin_unlock_irqrestore(&ndp->lock, flags);
 763
 764         return ncsi_process_next_channel(ndp);
 765 }
 766
 767 static bool ncsi_check_hwa(struct ncsi_dev_priv *ndp)
 768 {
 769         struct ncsi_package *np;
 770         struct ncsi_channel *nc;
 771         unsigned int cap;
 772
 773         /* The hardware arbitration is disabled if any one channel
 774          * doesn't support explicitly.
 775          */
 776         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 777                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 778                         cap = nc->caps[NCSI_CAP_GENERIC].cap;
 779                         if (!(cap & NCSI_CAP_GENERIC_HWA) ||
 780                             (cap & NCSI_CAP_GENERIC_HWA_MASK) !=
 781                             NCSI_CAP_GENERIC_HWA_SUPPORT) {
 782                                 ndp->flags &= ~NCSI_DEV_HWA;
 783                                 return false;
 784                         }
 785                 }
 786         }
 787
 788         ndp->flags |= NCSI_DEV_HWA;
 789         return true;
 790 }
 791
 792 static int ncsi_enable_hwa(struct ncsi_dev_priv *ndp)
 793 {
 794         struct ncsi_package *np;
 795         struct ncsi_channel *nc;
 796         unsigned long flags;
 797
 798         /* Move all available channels to processing queue */
 799         spin_lock_irqsave(&ndp->lock, flags);
 800         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 801                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 802                         WARN_ON_ONCE(nc->state != NCSI_CHANNEL_INACTIVE ||
 803                                      !list_empty(&nc->link));
 804                         ncsi_stop_channel_monitor(nc);
 805                         list_add_tail_rcu(&nc->link, &ndp->channel_queue);
 806                 }
 807         }
 808         spin_unlock_irqrestore(&ndp->lock, flags);
 809
 810         /* We can have no channels in extremely case */
 811         if (list_empty(&ndp->channel_queue)) {
 812                 ncsi_report_link(ndp, false);
 813                 return -ENOENT;
 814         }
 815
 816         return ncsi_process_next_channel(ndp);
 817 }
 818
 819 static void ncsi_probe_channel(struct ncsi_dev_priv *ndp)
 820 {
 821         struct ncsi_dev *nd = &ndp->ndev;
 822         struct ncsi_package *np;
 823         struct ncsi_channel *nc;
 824         struct ncsi_cmd_arg nca;
 825         unsigned char index;
 826         int ret;
 827
 828         nca.ndp = ndp;
 829         nca.req_flags = NCSI_REQ_FLAG_EVENT_DRIVEN;
 830         switch (nd->state) {
 831         case ncsi_dev_state_probe:
 832                 nd->state = ncsi_dev_state_probe_deselect;
 833                 /* Fall through */
 834         case ncsi_dev_state_probe_deselect:
 835                 ndp->pending_req_num = 8;
 836
 837                 /* Deselect all possible packages */
 838                 nca.type = NCSI_PKT_CMD_DP;
 839                 nca.channel = NCSI_RESERVED_CHANNEL;
 840                 for (index = 0; index < 8; index++) {
 841                         nca.package = index;
 842                         ret = ncsi_xmit_cmd(&nca);
 843                         if (ret)
 844                                 goto error;
 845                 }
 846
 847                 nd->state = ncsi_dev_state_probe_package;
 848                 break;
 849         case ncsi_dev_state_probe_package:
 850                 ndp->pending_req_num = 16;
 851
 852                 /* Select all possible packages */
 853                 nca.type = NCSI_PKT_CMD_SP;
 854                 nca.bytes[0] = 1;
 855                 nca.channel = NCSI_RESERVED_CHANNEL;
 856                 for (index = 0; index < 8; index++) {
 857                         nca.package = index;
 858                         ret = ncsi_xmit_cmd(&nca);
 859                         if (ret)
 860                                 goto error;
 861                 }
 862
 863                 /* Disable all possible packages */
 864                 nca.type = NCSI_PKT_CMD_DP;
 865                 for (index = 0; index < 8; index++) {
 866                         nca.package = index;
 867                         ret = ncsi_xmit_cmd(&nca);
 868                         if (ret)
 869                                 goto error;
 870                 }
 871
 872                 nd->state = ncsi_dev_state_probe_channel;
 873                 break;
 874         case ncsi_dev_state_probe_channel:
 875                 if (!ndp->active_package)
 876                         ndp->active_package = list_first_or_null_rcu(
 877                                 &ndp->packages, struct ncsi_package, node);
 878                 else if (list_is_last(&ndp->active_package->node,
 879                                       &ndp->packages))
 880                         ndp->active_package = NULL;
 881                 else
 882                         ndp->active_package = list_next_entry(
 883                                 ndp->active_package, node);
 884
 885                 /* All available packages and channels are enumerated. The
 886                  * enumeration happens for once when the NCSI interface is
 887                  * started. So we need continue to start the interface after
 888                  * the enumeration.
 889                  *
 890                  * We have to choose an active channel before configuring it.
 891                  * Note that we possibly don't have active channel in extreme
 892                  * situation.
 893                  */
 894                 if (!ndp->active_package) {
 895                         ndp->flags |= NCSI_DEV_PROBED;
 896                         if (ncsi_check_hwa(ndp))
 897                                 ncsi_enable_hwa(ndp);
 898                         else
 899                                 ncsi_choose_active_channel(ndp);
 900                         return;
 901                 }
 902
 903                 /* Select the active package */
 904                 ndp->pending_req_num = 1;
 905                 nca.type = NCSI_PKT_CMD_SP;
 906                 nca.bytes[0] = 1;
 907                 nca.package = ndp->active_package->id;
 908                 nca.channel = NCSI_RESERVED_CHANNEL;
 909                 ret = ncsi_xmit_cmd(&nca);
 910                 if (ret)
 911                         goto error;
 912
 913                 nd->state = ncsi_dev_state_probe_cis;
 914                 break;
 915         case ncsi_dev_state_probe_cis:
 916                 ndp->pending_req_num = NCSI_RESERVED_CHANNEL;
 917
 918                 /* Clear initial state */
 919                 nca.type = NCSI_PKT_CMD_CIS;
 920                 nca.package = ndp->active_package->id;
 921                 for (index = 0; index < NCSI_RESERVED_CHANNEL; index++) {
 922                         nca.channel = index;
 923                         ret = ncsi_xmit_cmd(&nca);
 924                         if (ret)
 925                                 goto error;
 926                 }
 927
 928                 nd->state = ncsi_dev_state_probe_gvi;
 929                 break;
 930         case ncsi_dev_state_probe_gvi:
 931         case ncsi_dev_state_probe_gc:
 932         case ncsi_dev_state_probe_gls:
 933                 np = ndp->active_package;
 934                 ndp->pending_req_num = np->channel_num;
 935
 936                 /* Retrieve version, capability or link status */
 937                 if (nd->state == ncsi_dev_state_probe_gvi)
 938                         nca.type = NCSI_PKT_CMD_GVI;
 939                 else if (nd->state == ncsi_dev_state_probe_gc)
 940                         nca.type = NCSI_PKT_CMD_GC;
 941                 else
 942                         nca.type = NCSI_PKT_CMD_GLS;
 943
 944                 nca.package = np->id;
 945                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 946                         nca.channel = nc->id;
 947                         ret = ncsi_xmit_cmd(&nca);
 948                         if (ret)
 949                                 goto error;
 950                 }
 951
 952                 if (nd->state == ncsi_dev_state_probe_gvi)
 953                         nd->state = ncsi_dev_state_probe_gc;
 954                 else if (nd->state == ncsi_dev_state_probe_gc)
 955                         nd->state = ncsi_dev_state_probe_gls;
 956                 else
 957                         nd->state = ncsi_dev_state_probe_dp;
 958                 break;
 959         case ncsi_dev_state_probe_dp:
 960                 ndp->pending_req_num = 1;
 961
 962                 /* Deselect the active package */
 963                 nca.type = NCSI_PKT_CMD_DP;
 964                 nca.package = ndp->active_package->id;
 965                 nca.channel = NCSI_RESERVED_CHANNEL;
 966                 ret = ncsi_xmit_cmd(&nca);
 967                 if (ret)
 968                         goto error;
 969
 970                 /* Scan channels in next package */
 971                 nd->state = ncsi_dev_state_probe_channel;
 972                 break;
 973         default:
 974                 netdev_warn(nd->dev, "Wrong NCSI state 0x%0x in enumeration\n",
 975                             nd->state);
 976         }
 977
 978         return;
 979 error:
 980         ncsi_report_link(ndp, true);
 981 }
 982
 983 static void ncsi_dev_work(struct work_struct *work)
 984 {
 985         struct ncsi_dev_priv *ndp = container_of(work,
 986                         struct ncsi_dev_priv, work);
 987         struct ncsi_dev *nd = &ndp->ndev;
 988
 989         switch (nd->state & ncsi_dev_state_major) {
 990         case ncsi_dev_state_probe:
 991                 ncsi_probe_channel(ndp);
 992                 break;
 993         case ncsi_dev_state_suspend:
 994                 ncsi_suspend_channel(ndp);
 995                 break;
 996         case ncsi_dev_state_config:
 997                 ncsi_configure_channel(ndp);
 998                 break;
 999         default:
1000                 netdev_warn(nd->dev, "Wrong NCSI state 0x%x in workqueue\n",
1001                             nd->state);
1002         }
1003 }
1004
1005 int ncsi_process_next_channel(struct ncsi_dev_priv *ndp)
1006 {
1007         struct ncsi_channel *nc;
1008         int old_state;
1009         unsigned long flags;
1010
1011         spin_lock_irqsave(&ndp->lock, flags);
1012         nc = list_first_or_null_rcu(&ndp->channel_queue,
1013                                     struct ncsi_channel, link);
1014         if (!nc) {
1015                 spin_unlock_irqrestore(&ndp->lock, flags);
1016                 goto out;
1017         }
1018
1019         list_del_init(&nc->link);
1020         spin_unlock_irqrestore(&ndp->lock, flags);
1021
1022         spin_lock_irqsave(&nc->lock, flags);
1023         old_state = nc->state;
1024         nc->state = NCSI_CHANNEL_INVISIBLE;
1025         spin_unlock_irqrestore(&nc->lock, flags);
1026
1027         ndp->active_channel = nc;
1028         ndp->active_package = nc->package;
1029
1030         switch (old_state) {
1031         case NCSI_CHANNEL_INACTIVE:
1032                 ndp->ndev.state = ncsi_dev_state_config;
1033                 ncsi_configure_channel(ndp);
1034                 break;
1035         case NCSI_CHANNEL_ACTIVE:
1036                 ndp->ndev.state = ncsi_dev_state_suspend;
1037                 ncsi_suspend_channel(ndp);
1038                 break;
1039         default:
1040                 netdev_err(ndp->ndev.dev, "Invalid state 0x%x on %d:%d\n",
1041                            old_state, nc->package->id, nc->id);
1042                 ncsi_report_link(ndp, false);
1043                 return -EINVAL;
1044         }
1045
1046         return 0;
1047
1048 out:
1049         ndp->active_channel = NULL;
1050         ndp->active_package = NULL;
1051         if (ndp->flags & NCSI_DEV_RESHUFFLE) {
1052                 ndp->flags &= ~NCSI_DEV_RESHUFFLE;
1053                 return ncsi_choose_active_channel(ndp);
1054         }
1055
1056         ncsi_report_link(ndp, false);
1057         return -ENODEV;
1058 }
1059
1060 #if IS_ENABLED(CONFIG_IPV6)
1061 static int ncsi_inet6addr_event(struct notifier_block *this,
1062                                 unsigned long event, void *data)
1063 {
1064         struct inet6_ifaddr *ifa = data;
1065         struct net_device *dev = ifa->idev->dev;
1066         struct ncsi_dev *nd = ncsi_find_dev(dev);
1067         struct ncsi_dev_priv *ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
1068         struct ncsi_package *np;
1069         struct ncsi_channel *nc;
1070         struct ncsi_cmd_arg nca;
1071         bool action;
1072         int ret;
1073
1074         if (!ndp || (ipv6_addr_type(&ifa->addr) &
1075             (IPV6_ADDR_LINKLOCAL | IPV6_ADDR_LOOPBACK)))
1076                 return NOTIFY_OK;
1077
1078         switch (event) {
1079         case NETDEV_UP:
1080                 action = (++ndp->inet6_addr_num) == 1;
1081                 nca.type = NCSI_PKT_CMD_EGMF;
1082                 break;
1083         case NETDEV_DOWN:
1084                 action = (--ndp->inet6_addr_num == 0);
1085                 nca.type = NCSI_PKT_CMD_DGMF;
1086                 break;
1087         default:
1088                 return NOTIFY_OK;
1089         }
1090
1091         /* We might not have active channel or packages. The IPv6
1092          * required multicast will be enabled when active channel
1093          * or packages are chosen.
1094          */
1095         np = ndp->active_package;
1096         nc = ndp->active_channel;
1097         if (!action || !np || !nc)
1098                 return NOTIFY_OK;
1099
1100         /* We needn't enable or disable it if the function isn't supported */
1101         if (!(nc->caps[NCSI_CAP_GENERIC].cap & NCSI_CAP_GENERIC_MC))
1102                 return NOTIFY_OK;
1103
1104         nca.ndp = ndp;
1105         nca.req_flags = 0;
1106         nca.package = np->id;
1107         nca.channel = nc->id;
1108         nca.dwords[0] = nc->caps[NCSI_CAP_MC].cap;
1109         ret = ncsi_xmit_cmd(&nca);
1110         if (ret) {
1111                 netdev_warn(dev, "Fail to %s global multicast filter (%d)\n",
1112                             (event == NETDEV_UP) ? "enable" : "disable", ret);
1113                 return NOTIFY_DONE;
1114         }
1115
1116         return NOTIFY_OK;
1117 }
1118
1119 static struct notifier_block ncsi_inet6addr_notifier = {
1120         .notifier_call = ncsi_inet6addr_event,
1121 };
1122 #endif /* CONFIG_IPV6 */
1123
1124 struct ncsi_dev *ncsi_register_dev(struct net_device *dev,
1125                                    void (*handler)(struct ncsi_dev *ndev))
1126 {
1127         struct ncsi_dev_priv *ndp;
1128         struct ncsi_dev *nd;
1129         unsigned long flags;
1130         int i;
1131
1132         /* Check if the device has been registered or not */
1133         nd = ncsi_find_dev(dev);
1134         if (nd)
1135                 return nd;
1136
1137         /* Create NCSI device */
1138         ndp = kzalloc(sizeof(*ndp), GFP_ATOMIC);
1139         if (!ndp)
1140                 return NULL;
1141
1142         nd = &ndp->ndev;
1143         nd->state = ncsi_dev_state_registered;
1144         nd->dev = dev;
1145         nd->handler = handler;
1146         ndp->pending_req_num = 0;
1147         INIT_LIST_HEAD(&ndp->channel_queue);
1148         INIT_WORK(&ndp->work, ncsi_dev_work);
1149
1150         /* Initialize private NCSI device */
1151         spin_lock_init(&ndp->lock);
1152         INIT_LIST_HEAD(&ndp->packages);
1153         ndp->request_id = NCSI_REQ_START_IDX;
1154         for (i = 0; i < ARRAY_SIZE(ndp->requests); i++) {
1155                 ndp->requests[i].id = i;
1156                 ndp->requests[i].ndp = ndp;
1157                 setup_timer(&ndp->requests[i].timer,
1158                             ncsi_request_timeout,
1159                             (unsigned long)&ndp->requests[i]);
1160         }
1161
1162         spin_lock_irqsave(&ncsi_dev_lock, flags);
1163 #if IS_ENABLED(CONFIG_IPV6)
1164         ndp->inet6_addr_num = 0;
1165         if (list_empty(&ncsi_dev_list))
1166                 register_inet6addr_notifier(&ncsi_inet6addr_notifier);
1167 #endif
1168         list_add_tail_rcu(&ndp->node, &ncsi_dev_list);
1169         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1170
1171         /* Register NCSI packet Rx handler */
1172         ndp->ptype.type = cpu_to_be16(ETH_P_NCSI);
1173         ndp->ptype.func = ncsi_rcv_rsp;
1174         ndp->ptype.dev = dev;
1175         dev_add_pack(&ndp->ptype);
1176
1177         return nd;
1178 }
1179 EXPORT_SYMBOL_GPL(ncsi_register_dev);
1180
1181 int ncsi_start_dev(struct ncsi_dev *nd)
1182 {
1183         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1184         struct ncsi_package *np;
1185         struct ncsi_channel *nc;
1186         unsigned long flags;
1187         bool chained;
1188         int old_state, ret;
1189
1190         if (nd->state != ncsi_dev_state_registered &&
1191             nd->state != ncsi_dev_state_functional)
1192                 return -ENOTTY;
1193
1194         if (!(ndp->flags & NCSI_DEV_PROBED)) {
1195                 nd->state = ncsi_dev_state_probe;
1196                 schedule_work(&ndp->work);
1197                 return 0;
1198         }
1199
1200         /* Reset channel's state and start over */
1201         NCSI_FOR_EACH_PACKAGE(ndp, np) {
1202                 NCSI_FOR_EACH_CHANNEL(np, nc) {
1203                         spin_lock_irqsave(&nc->lock, flags);
1204                         chained = !list_empty(&nc->link);
1205                         old_state = nc->state;
1206                         nc->state = NCSI_CHANNEL_INACTIVE;
1207                         spin_unlock_irqrestore(&nc->lock, flags);
1208
1209                         WARN_ON_ONCE(chained ||
1210                                      old_state == NCSI_CHANNEL_INVISIBLE);
1211                 }
1212         }
1213
1214         if (ndp->flags & NCSI_DEV_HWA)
1215                 ret = ncsi_enable_hwa(ndp);
1216         else
1217                 ret = ncsi_choose_active_channel(ndp);
1218
1219         return ret;
1220 }
1221 EXPORT_SYMBOL_GPL(ncsi_start_dev);
1222
1223 void ncsi_unregister_dev(struct ncsi_dev *nd)
1224 {
1225         struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd);
1226         struct ncsi_package *np, *tmp;
1227         unsigned long flags;
1228
1229         dev_remove_pack(&ndp->ptype);
1230
1231         list_for_each_entry_safe(np, tmp, &ndp->packages, node)
1232                 ncsi_remove_package(np);
1233
1234         spin_lock_irqsave(&ncsi_dev_lock, flags);
1235         list_del_rcu(&ndp->node);
1236 #if IS_ENABLED(CONFIG_IPV6)
1237         if (list_empty(&ncsi_dev_list))
1238                 unregister_inet6addr_notifier(&ncsi_inet6addr_notifier);
1239 #endif
1240         spin_unlock_irqrestore(&ncsi_dev_lock, flags);
1241
1242         kfree(ndp);
1243 }
1244 EXPORT_SYMBOL_GPL(ncsi_unregister_dev);