drivers/net/ethernet/intel/ice/ice_lag.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (C) 2018-2021, Intel Corporation. */
   3
   4 /* Link Aggregation code */
   5
   6 #include "ice.h"
   7 #include "ice_lib.h"
   8 #include "ice_lag.h"
   9
  10 #define ICE_LAG_RES_SHARED      BIT(14)
  11 #define ICE_LAG_RES_VALID       BIT(15)
  12
  13 #define LACP_TRAIN_PKT_LEN              16
  14 static const u8 lacp_train_pkt[LACP_TRAIN_PKT_LEN] = { 0, 0, 0, 0, 0, 0,
  15                                                        0, 0, 0, 0, 0, 0,
  16                                                        0x88, 0x09, 0, 0 };
  17
  18 #define ICE_RECIPE_LEN                  64
  19 static const u8 ice_dflt_vsi_rcp[ICE_RECIPE_LEN] = {
  20         0x05, 0, 0, 0, 0x20, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  21         0x85, 0, 0x01, 0, 0, 0, 0xff, 0xff, 0x08, 0, 0, 0, 0, 0, 0, 0,
  22         0, 0, 0, 0, 0, 0, 0x30, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  23         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  24
  25 /**
  26  * ice_lag_set_primary - set PF LAG state as Primary
  27  * @lag: LAG info struct
  28  */
  29 static void ice_lag_set_primary(struct ice_lag *lag)
  30 {
  31         struct ice_pf *pf = lag->pf;
  32
  33         if (!pf)
  34                 return;
  35
  36         if (lag->role != ICE_LAG_UNSET && lag->role != ICE_LAG_BACKUP) {
  37                 dev_warn(ice_pf_to_dev(pf), "%s: Attempt to be Primary, but incompatible state.\n",
  38                          netdev_name(lag->netdev));
  39                 return;
  40         }
  41
  42         lag->role = ICE_LAG_PRIMARY;
  43 }
  44
  45 /**
  46  * ice_lag_set_backup - set PF LAG state to Backup
  47  * @lag: LAG info struct
  48  */
  49 static void ice_lag_set_backup(struct ice_lag *lag)
  50 {
  51         struct ice_pf *pf = lag->pf;
  52
  53         if (!pf)
  54                 return;
  55
  56         if (lag->role != ICE_LAG_UNSET && lag->role != ICE_LAG_PRIMARY) {
  57                 dev_dbg(ice_pf_to_dev(pf), "%s: Attempt to be Backup, but incompatible state\n",
  58                         netdev_name(lag->netdev));
  59                 return;
  60         }
  61
  62         lag->role = ICE_LAG_BACKUP;
  63 }
  64
  65 /**
  66  * netif_is_same_ice - determine if netdev is on the same ice NIC as local PF
  67  * @pf: local PF struct
  68  * @netdev: netdev we are evaluating
  69  */
  70 static bool netif_is_same_ice(struct ice_pf *pf, struct net_device *netdev)
  71 {
  72         struct ice_netdev_priv *np;
  73         struct ice_pf *test_pf;
  74         struct ice_vsi *vsi;
  75
  76         if (!netif_is_ice(netdev))
  77                 return false;
  78
  79         np = netdev_priv(netdev);
  80         if (!np)
  81                 return false;
  82
  83         vsi = np->vsi;
  84         if (!vsi)
  85                 return false;
  86
  87         test_pf = vsi->back;
  88         if (!test_pf)
  89                 return false;
  90
  91         if (pf->pdev->bus != test_pf->pdev->bus ||
  92             pf->pdev->slot != test_pf->pdev->slot)
  93                 return false;
  94
  95         return true;
  96 }
  97
  98 /**
  99  * ice_netdev_to_lag - return pointer to associated lag struct from netdev
 100  * @netdev: pointer to net_device struct to query
 101  */
 102 static struct ice_lag *ice_netdev_to_lag(struct net_device *netdev)
 103 {
 104         struct ice_netdev_priv *np;
 105         struct ice_vsi *vsi;
 106
 107         if (!netif_is_ice(netdev))
 108                 return NULL;
 109
 110         np = netdev_priv(netdev);
 111         if (!np)
 112                 return NULL;
 113
 114         vsi = np->vsi;
 115         if (!vsi)
 116                 return NULL;
 117
 118         return vsi->back->lag;
 119 }
 120
 121 /**
 122  * ice_lag_find_hw_by_lport - return an hw struct from bond members lport
 123  * @lag: lag struct
 124  * @lport: lport value to search for
 125  */
 126 static struct ice_hw *
 127 ice_lag_find_hw_by_lport(struct ice_lag *lag, u8 lport)
 128 {
 129         struct ice_lag_netdev_list *entry;
 130         struct net_device *tmp_netdev;
 131         struct ice_netdev_priv *np;
 132         struct ice_hw *hw;
 133
 134         list_for_each_entry(entry, lag->netdev_head, node) {
 135                 tmp_netdev = entry->netdev;
 136                 if (!tmp_netdev || !netif_is_ice(tmp_netdev))
 137                         continue;
 138
 139                 np = netdev_priv(tmp_netdev);
 140                 if (!np || !np->vsi)
 141                         continue;
 142
 143                 hw = &np->vsi->back->hw;
 144                 if (hw->port_info->lport == lport)
 145                         return hw;
 146         }
 147
 148         return NULL;
 149 }
 150
 151 /**
 152  * ice_lag_find_primary - returns pointer to primary interfaces lag struct
 153  * @lag: local interfaces lag struct
 154  */
 155 static struct ice_lag *ice_lag_find_primary(struct ice_lag *lag)
 156 {
 157         struct ice_lag *primary_lag = NULL;
 158         struct list_head *tmp;
 159
 160         list_for_each(tmp, lag->netdev_head) {
 161                 struct ice_lag_netdev_list *entry;
 162                 struct ice_lag *tmp_lag;
 163
 164                 entry = list_entry(tmp, struct ice_lag_netdev_list, node);
 165                 tmp_lag = ice_netdev_to_lag(entry->netdev);
 166                 if (tmp_lag && tmp_lag->primary) {
 167                         primary_lag = tmp_lag;
 168                         break;
 169                 }
 170         }
 171
 172         return primary_lag;
 173 }
 174
 175 /**
 176  * ice_lag_cfg_dflt_fltr - Add/Remove default VSI rule for LAG
 177  * @lag: lag struct for local interface
 178  * @add: boolean on whether we are adding filters
 179  */
 180 static int
 181 ice_lag_cfg_dflt_fltr(struct ice_lag *lag, bool add)
 182 {
 183         struct ice_sw_rule_lkup_rx_tx *s_rule;
 184         u16 s_rule_sz, vsi_num;
 185         struct ice_hw *hw;
 186         u32 act, opc;
 187         u8 *eth_hdr;
 188         int err;
 189
 190         hw = &lag->pf->hw;
 191         vsi_num = ice_get_hw_vsi_num(hw, 0);
 192
 193         s_rule_sz = ICE_SW_RULE_RX_TX_ETH_HDR_SIZE(s_rule);
 194         s_rule = kzalloc(s_rule_sz, GFP_KERNEL);
 195         if (!s_rule) {
 196                 dev_err(ice_pf_to_dev(lag->pf), "error allocating rule for LAG default VSI\n");
 197                 return -ENOMEM;
 198         }
 199
 200         if (add) {
 201                 eth_hdr = s_rule->hdr_data;
 202                 ice_fill_eth_hdr(eth_hdr);
 203
 204                 act = (vsi_num << ICE_SINGLE_ACT_VSI_ID_S) &
 205                         ICE_SINGLE_ACT_VSI_ID_M;
 206                 act |= ICE_SINGLE_ACT_VSI_FORWARDING |
 207                         ICE_SINGLE_ACT_VALID_BIT | ICE_SINGLE_ACT_LAN_ENABLE;
 208
 209                 s_rule->hdr.type = cpu_to_le16(ICE_AQC_SW_RULES_T_LKUP_RX);
 210                 s_rule->recipe_id = cpu_to_le16(lag->pf_recipe);
 211                 s_rule->src = cpu_to_le16(hw->port_info->lport);
 212                 s_rule->act = cpu_to_le32(act);
 213                 s_rule->hdr_len = cpu_to_le16(DUMMY_ETH_HDR_LEN);
 214                 opc = ice_aqc_opc_add_sw_rules;
 215         } else {
 216                 s_rule->index = cpu_to_le16(lag->pf_rule_id);
 217                 opc = ice_aqc_opc_remove_sw_rules;
 218         }
 219
 220         err = ice_aq_sw_rules(&lag->pf->hw, s_rule, s_rule_sz, 1, opc, NULL);
 221         if (err)
 222                 goto dflt_fltr_free;
 223
 224         if (add)
 225                 lag->pf_rule_id = le16_to_cpu(s_rule->index);
 226         else
 227                 lag->pf_rule_id = 0;
 228
 229 dflt_fltr_free:
 230         kfree(s_rule);
 231         return err;
 232 }
 233
 234 /**
 235  * ice_lag_cfg_pf_fltrs - set filters up for new active port
 236  * @lag: local interfaces lag struct
 237  * @ptr: opaque data containing notifier event
 238  */
 239 static void
 240 ice_lag_cfg_pf_fltrs(struct ice_lag *lag, void *ptr)
 241 {
 242         struct netdev_notifier_bonding_info *info;
 243         struct netdev_bonding_info *bonding_info;
 244         struct net_device *event_netdev;
 245         struct device *dev;
 246
 247         event_netdev = netdev_notifier_info_to_dev(ptr);
 248         /* not for this netdev */
 249         if (event_netdev != lag->netdev)
 250                 return;
 251
 252         info = (struct netdev_notifier_bonding_info *)ptr;
 253         bonding_info = &info->bonding_info;
 254         dev = ice_pf_to_dev(lag->pf);
 255
 256         /* interface not active - remove old default VSI rule */
 257         if (bonding_info->slave.state && lag->pf_rule_id) {
 258                 if (ice_lag_cfg_dflt_fltr(lag, false))
 259                         dev_err(dev, "Error removing old default VSI filter\n");
 260                 return;
 261         }
 262
 263         /* interface becoming active - add new default VSI rule */
 264         if (!bonding_info->slave.state && !lag->pf_rule_id)
 265                 if (ice_lag_cfg_dflt_fltr(lag, true))
 266                         dev_err(dev, "Error adding new default VSI filter\n");
 267 }
 268
 269 /**
 270  * ice_display_lag_info - print LAG info
 271  * @lag: LAG info struct
 272  */
 273 static void ice_display_lag_info(struct ice_lag *lag)
 274 {
 275         const char *name, *upper, *role, *bonded, *primary;
 276         struct device *dev = &lag->pf->pdev->dev;
 277
 278         name = lag->netdev ? netdev_name(lag->netdev) : "unset";
 279         upper = lag->upper_netdev ? netdev_name(lag->upper_netdev) : "unset";
 280         primary = lag->primary ? "TRUE" : "FALSE";
 281         bonded = lag->bonded ? "BONDED" : "UNBONDED";
 282
 283         switch (lag->role) {
 284         case ICE_LAG_NONE:
 285                 role = "NONE";
 286                 break;
 287         case ICE_LAG_PRIMARY:
 288                 role = "PRIMARY";
 289                 break;
 290         case ICE_LAG_BACKUP:
 291                 role = "BACKUP";
 292                 break;
 293         case ICE_LAG_UNSET:
 294                 role = "UNSET";
 295                 break;
 296         default:
 297                 role = "ERROR";
 298         }
 299
 300         dev_dbg(dev, "%s %s, upper:%s, role:%s, primary:%s\n", name, bonded,
 301                 upper, role, primary);
 302 }
 303
 304 /**
 305  * ice_lag_qbuf_recfg - generate a buffer of queues for a reconfigure command
 306  * @hw: HW struct that contains the queue contexts
 307  * @qbuf: pointer to buffer to populate
 308  * @vsi_num: index of the VSI in PF space
 309  * @numq: number of queues to search for
 310  * @tc: traffic class that contains the queues
 311  *
 312  * function returns the number of valid queues in buffer
 313  */
 314 static u16
 315 ice_lag_qbuf_recfg(struct ice_hw *hw, struct ice_aqc_cfg_txqs_buf *qbuf,
 316                    u16 vsi_num, u16 numq, u8 tc)
 317 {
 318         struct ice_q_ctx *q_ctx;
 319         u16 qid, count = 0;
 320         struct ice_pf *pf;
 321         int i;
 322
 323         pf = hw->back;
 324         for (i = 0; i < numq; i++) {
 325                 q_ctx = ice_get_lan_q_ctx(hw, vsi_num, tc, i);
 326                 if (!q_ctx) {
 327                         dev_dbg(ice_hw_to_dev(hw), "%s queue %d NO Q CONTEXT\n",
 328                                 __func__, i);
 329                         continue;
 330                 }
 331                 if (q_ctx->q_teid == ICE_INVAL_TEID) {
 332                         dev_dbg(ice_hw_to_dev(hw), "%s queue %d INVAL TEID\n",
 333                                 __func__, i);
 334                         continue;
 335                 }
 336                 if (q_ctx->q_handle == ICE_INVAL_Q_HANDLE) {
 337                         dev_dbg(ice_hw_to_dev(hw), "%s queue %d INVAL Q HANDLE\n",
 338                                 __func__, i);
 339                         continue;
 340                 }
 341
 342                 qid = pf->vsi[vsi_num]->txq_map[q_ctx->q_handle];
 343                 qbuf->queue_info[count].q_handle = cpu_to_le16(qid);
 344                 qbuf->queue_info[count].tc = tc;
 345                 qbuf->queue_info[count].q_teid = cpu_to_le32(q_ctx->q_teid);
 346                 count++;
 347         }
 348
 349         return count;
 350 }
 351
 352 /**
 353  * ice_lag_get_sched_parent - locate or create a sched node parent
 354  * @hw: HW struct for getting parent in
 355  * @tc: traffic class on parent/node
 356  */
 357 static struct ice_sched_node *
 358 ice_lag_get_sched_parent(struct ice_hw *hw, u8 tc)
 359 {
 360         struct ice_sched_node *tc_node, *aggnode, *parent = NULL;
 361         u16 num_nodes[ICE_AQC_TOPO_MAX_LEVEL_NUM] = { 0 };
 362         struct ice_port_info *pi = hw->port_info;
 363         struct device *dev;
 364         u8 aggl, vsil;
 365         int n;
 366
 367         dev = ice_hw_to_dev(hw);
 368
 369         tc_node = ice_sched_get_tc_node(pi, tc);
 370         if (!tc_node) {
 371                 dev_warn(dev, "Failure to find TC node for LAG move\n");
 372                 return parent;
 373         }
 374
 375         aggnode = ice_sched_get_agg_node(pi, tc_node, ICE_DFLT_AGG_ID);
 376         if (!aggnode) {
 377                 dev_warn(dev, "Failure to find aggregate node for LAG move\n");
 378                 return parent;
 379         }
 380
 381         aggl = ice_sched_get_agg_layer(hw);
 382         vsil = ice_sched_get_vsi_layer(hw);
 383
 384         for (n = aggl + 1; n < vsil; n++)
 385                 num_nodes[n] = 1;
 386
 387         for (n = 0; n < aggnode->num_children; n++) {
 388                 parent = ice_sched_get_free_vsi_parent(hw, aggnode->children[n],
 389                                                        num_nodes);
 390                 if (parent)
 391                         return parent;
 392         }
 393
 394         /* if free parent not found - add one */
 395         parent = aggnode;
 396         for (n = aggl + 1; n < vsil; n++) {
 397                 u16 num_nodes_added;
 398                 u32 first_teid;
 399                 int err;
 400
 401                 err = ice_sched_add_nodes_to_layer(pi, tc_node, parent, n,
 402                                                    num_nodes[n], &first_teid,
 403                                                    &num_nodes_added);
 404                 if (err || num_nodes[n] != num_nodes_added)
 405                         return NULL;
 406
 407                 if (num_nodes_added)
 408                         parent = ice_sched_find_node_by_teid(tc_node,
 409                                                              first_teid);
 410                 else
 411                         parent = parent->children[0];
 412                 if (!parent) {
 413                         dev_warn(dev, "Failure to add new parent for LAG move\n");
 414                         return parent;
 415                 }
 416         }
 417
 418         return parent;
 419 }
 420
 421 /**
 422  * ice_lag_move_vf_node_tc - move scheduling nodes for one VF on one TC
 423  * @lag: lag info struct
 424  * @oldport: lport of previous nodes location
 425  * @newport: lport of destination nodes location
 426  * @vsi_num: array index of VSI in PF space
 427  * @tc: traffic class to move
 428  */
 429 static void
 430 ice_lag_move_vf_node_tc(struct ice_lag *lag, u8 oldport, u8 newport,
 431                         u16 vsi_num, u8 tc)
 432 {
 433         u16 numq, valq, buf_size, num_moved, qbuf_size;
 434         struct device *dev = ice_pf_to_dev(lag->pf);
 435         struct ice_aqc_cfg_txqs_buf *qbuf;
 436         struct ice_aqc_move_elem *buf;
 437         struct ice_sched_node *n_prt;
 438         struct ice_hw *new_hw = NULL;
 439         __le32 teid, parent_teid;
 440         struct ice_vsi_ctx *ctx;
 441         u32 tmp_teid;
 442
 443         ctx = ice_get_vsi_ctx(&lag->pf->hw, vsi_num);
 444         if (!ctx) {
 445                 dev_warn(dev, "Unable to locate VSI context for LAG failover\n");
 446                 return;
 447         }
 448
 449         /* check to see if this VF is enabled on this TC */
 450         if (!ctx->sched.vsi_node[tc])
 451                 return;
 452
 453         /* locate HW struct for destination port */
 454         new_hw = ice_lag_find_hw_by_lport(lag, newport);
 455         if (!new_hw) {
 456                 dev_warn(dev, "Unable to locate HW struct for LAG node destination\n");
 457                 return;
 458         }
 459
 460         numq = ctx->num_lan_q_entries[tc];
 461         teid = ctx->sched.vsi_node[tc]->info.node_teid;
 462         tmp_teid = le32_to_cpu(teid);
 463         parent_teid = ctx->sched.vsi_node[tc]->info.parent_teid;
 464         /* if no teid assigned or numq == 0, then this TC is not active */
 465         if (!tmp_teid || !numq)
 466                 return;
 467
 468         /* suspend VSI subtree for Traffic Class "tc" on
 469          * this VF's VSI
 470          */
 471         if (ice_sched_suspend_resume_elems(&lag->pf->hw, 1, &tmp_teid, true))
 472                 dev_dbg(dev, "Problem suspending traffic for LAG node move\n");
 473
 474         /* reconfigure all VF's queues on this Traffic Class
 475          * to new port
 476          */
 477         qbuf_size = struct_size(qbuf, queue_info, numq);
 478         qbuf = kzalloc(qbuf_size, GFP_KERNEL);
 479         if (!qbuf) {
 480                 dev_warn(dev, "Failure allocating memory for VF queue recfg buffer\n");
 481                 goto resume_traffic;
 482         }
 483
 484         /* add the per queue info for the reconfigure command buffer */
 485         valq = ice_lag_qbuf_recfg(&lag->pf->hw, qbuf, vsi_num, numq, tc);
 486         if (!valq) {
 487                 dev_dbg(dev, "No valid queues found for LAG failover\n");
 488                 goto qbuf_none;
 489         }
 490
 491         if (ice_aq_cfg_lan_txq(&lag->pf->hw, qbuf, qbuf_size, valq, oldport,
 492                                newport, NULL)) {
 493                 dev_warn(dev, "Failure to configure queues for LAG failover\n");
 494                 goto qbuf_err;
 495         }
 496
 497 qbuf_none:
 498         kfree(qbuf);
 499
 500         /* find new parent in destination port's tree for VF VSI node on this
 501          * Traffic Class
 502          */
 503         n_prt = ice_lag_get_sched_parent(new_hw, tc);
 504         if (!n_prt)
 505                 goto resume_traffic;
 506
 507         /* Move Vf's VSI node for this TC to newport's scheduler tree */
 508         buf_size = struct_size(buf, teid, 1);
 509         buf = kzalloc(buf_size, GFP_KERNEL);
 510         if (!buf) {
 511                 dev_warn(dev, "Failure to alloc memory for VF node failover\n");
 512                 goto resume_traffic;
 513         }
 514
 515         buf->hdr.src_parent_teid = parent_teid;
 516         buf->hdr.dest_parent_teid = n_prt->info.node_teid;
 517         buf->hdr.num_elems = cpu_to_le16(1);
 518         buf->hdr.mode = ICE_AQC_MOVE_ELEM_MODE_KEEP_OWN;
 519         buf->teid[0] = teid;
 520
 521         if (ice_aq_move_sched_elems(&lag->pf->hw, 1, buf, buf_size, &num_moved,
 522                                     NULL))
 523                 dev_warn(dev, "Failure to move VF nodes for failover\n");
 524         else
 525                 ice_sched_update_parent(n_prt, ctx->sched.vsi_node[tc]);
 526
 527         kfree(buf);
 528         goto resume_traffic;
 529
 530 qbuf_err:
 531         kfree(qbuf);
 532
 533 resume_traffic:
 534         /* restart traffic for VSI node */
 535         if (ice_sched_suspend_resume_elems(&lag->pf->hw, 1, &tmp_teid, false))
 536                 dev_dbg(dev, "Problem restarting traffic for LAG node move\n");
 537 }
 538
 539 /**
 540  * ice_lag_move_single_vf_nodes - Move Tx scheduling nodes for single VF
 541  * @lag: primary interface LAG struct
 542  * @oldport: lport of previous interface
 543  * @newport: lport of destination interface
 544  * @vsi_num: SW index of VF's VSI
 545  */
 546 static void
 547 ice_lag_move_single_vf_nodes(struct ice_lag *lag, u8 oldport, u8 newport,
 548                              u16 vsi_num)
 549 {
 550         u8 tc;
 551
 552         ice_for_each_traffic_class(tc)
 553                 ice_lag_move_vf_node_tc(lag, oldport, newport, vsi_num, tc);
 554 }
 555
 556 /**
 557  * ice_lag_move_new_vf_nodes - Move Tx scheduling nodes for a VF if required
 558  * @vf: the VF to move Tx nodes for
 559  *
 560  * Called just after configuring new VF queues. Check whether the VF Tx
 561  * scheduling nodes need to be updated to fail over to the active port. If so,
 562  * move them now.
 563  */
 564 void ice_lag_move_new_vf_nodes(struct ice_vf *vf)
 565 {
 566         struct ice_lag_netdev_list ndlist;
 567         struct list_head *tmp, *n;
 568         u8 pri_port, act_port;
 569         struct ice_lag *lag;
 570         struct ice_vsi *vsi;
 571         struct ice_pf *pf;
 572
 573         vsi = ice_get_vf_vsi(vf);
 574
 575         if (WARN_ON(!vsi))
 576                 return;
 577
 578         if (WARN_ON(vsi->type != ICE_VSI_VF))
 579                 return;
 580
 581         pf = vf->pf;
 582         lag = pf->lag;
 583
 584         mutex_lock(&pf->lag_mutex);
 585         if (!lag->bonded)
 586                 goto new_vf_unlock;
 587
 588         pri_port = pf->hw.port_info->lport;
 589         act_port = lag->active_port;
 590
 591         if (lag->upper_netdev) {
 592                 struct ice_lag_netdev_list *nl;
 593                 struct net_device *tmp_nd;
 594
 595                 INIT_LIST_HEAD(&ndlist.node);
 596                 rcu_read_lock();
 597                 for_each_netdev_in_bond_rcu(lag->upper_netdev, tmp_nd) {
 598                         nl = kzalloc(sizeof(*nl), GFP_KERNEL);
 599                         if (!nl)
 600                                 break;
 601
 602                         nl->netdev = tmp_nd;
 603                         list_add(&nl->node, &ndlist.node);
 604                 }
 605                 rcu_read_unlock();
 606         }
 607
 608         lag->netdev_head = &ndlist.node;
 609
 610         if (ice_is_feature_supported(pf, ICE_F_SRIOV_LAG) &&
 611             lag->bonded && lag->primary && pri_port != act_port &&
 612             !list_empty(lag->netdev_head))
 613                 ice_lag_move_single_vf_nodes(lag, pri_port, act_port, vsi->idx);
 614
 615         list_for_each_safe(tmp, n, &ndlist.node) {
 616                 struct ice_lag_netdev_list *entry;
 617
 618                 entry = list_entry(tmp, struct ice_lag_netdev_list, node);
 619                 list_del(&entry->node);
 620                 kfree(entry);
 621         }
 622         lag->netdev_head = NULL;
 623
 624 new_vf_unlock:
 625         mutex_unlock(&pf->lag_mutex);
 626 }
 627
 628 /**
 629  * ice_lag_move_vf_nodes - move Tx scheduling nodes for all VFs to new port
 630  * @lag: lag info struct
 631  * @oldport: lport of previous interface
 632  * @newport: lport of destination interface
 633  */
 634 static void ice_lag_move_vf_nodes(struct ice_lag *lag, u8 oldport, u8 newport)
 635 {
 636         struct ice_pf *pf;
 637         int i;
 638
 639         if (!lag->primary)
 640                 return;
 641
 642         pf = lag->pf;
 643         ice_for_each_vsi(pf, i)
 644                 if (pf->vsi[i] && (pf->vsi[i]->type == ICE_VSI_VF ||
 645                                    pf->vsi[i]->type == ICE_VSI_SWITCHDEV_CTRL))
 646                         ice_lag_move_single_vf_nodes(lag, oldport, newport, i);
 647 }
 648
 649 #define ICE_LAG_SRIOV_CP_RECIPE         10
 650 #define ICE_LAG_SRIOV_TRAIN_PKT_LEN     16
 651
 652 /**
 653  * ice_lag_cfg_cp_fltr - configure filter for control packets
 654  * @lag: local interface's lag struct
 655  * @add: add or remove rule
 656  */
 657 static void
 658 ice_lag_cfg_cp_fltr(struct ice_lag *lag, bool add)
 659 {
 660         struct ice_sw_rule_lkup_rx_tx *s_rule = NULL;
 661         struct ice_vsi *vsi;
 662         u16 buf_len, opc;
 663
 664         vsi = lag->pf->vsi[0];
 665
 666         buf_len = ICE_SW_RULE_RX_TX_HDR_SIZE(s_rule,
 667                                              ICE_LAG_SRIOV_TRAIN_PKT_LEN);
 668         s_rule = kzalloc(buf_len, GFP_KERNEL);
 669         if (!s_rule) {
 670                 netdev_warn(lag->netdev, "-ENOMEM error configuring CP filter\n");
 671                 return;
 672         }
 673
 674         if (add) {
 675                 s_rule->hdr.type = cpu_to_le16(ICE_AQC_SW_RULES_T_LKUP_RX);
 676                 s_rule->recipe_id = cpu_to_le16(ICE_LAG_SRIOV_CP_RECIPE);
 677                 s_rule->src = cpu_to_le16(vsi->port_info->lport);
 678                 s_rule->act = cpu_to_le32(ICE_FWD_TO_VSI |
 679                                           ICE_SINGLE_ACT_LAN_ENABLE |
 680                                           ICE_SINGLE_ACT_VALID_BIT |
 681                                           ((vsi->vsi_num <<
 682                                             ICE_SINGLE_ACT_VSI_ID_S) &
 683                                            ICE_SINGLE_ACT_VSI_ID_M));
 684                 s_rule->hdr_len = cpu_to_le16(ICE_LAG_SRIOV_TRAIN_PKT_LEN);
 685                 memcpy(s_rule->hdr_data, lacp_train_pkt, LACP_TRAIN_PKT_LEN);
 686                 opc = ice_aqc_opc_add_sw_rules;
 687         } else {
 688                 opc = ice_aqc_opc_remove_sw_rules;
 689                 s_rule->index = cpu_to_le16(lag->cp_rule_idx);
 690         }
 691         if (ice_aq_sw_rules(&lag->pf->hw, s_rule, buf_len, 1, opc, NULL)) {
 692                 netdev_warn(lag->netdev, "Error %s CP rule for fail-over\n",
 693                             add ? "ADDING" : "REMOVING");
 694                 goto cp_free;
 695         }
 696
 697         if (add)
 698                 lag->cp_rule_idx = le16_to_cpu(s_rule->index);
 699         else
 700                 lag->cp_rule_idx = 0;
 701
 702 cp_free:
 703         kfree(s_rule);
 704 }
 705
 706 /**
 707  * ice_lag_info_event - handle NETDEV_BONDING_INFO event
 708  * @lag: LAG info struct
 709  * @ptr: opaque data pointer
 710  *
 711  * ptr is to be cast to (netdev_notifier_bonding_info *)
 712  */
 713 static void ice_lag_info_event(struct ice_lag *lag, void *ptr)
 714 {
 715         struct netdev_notifier_bonding_info *info;
 716         struct netdev_bonding_info *bonding_info;
 717         struct net_device *event_netdev;
 718         const char *lag_netdev_name;
 719
 720         event_netdev = netdev_notifier_info_to_dev(ptr);
 721         info = ptr;
 722         lag_netdev_name = netdev_name(lag->netdev);
 723         bonding_info = &info->bonding_info;
 724
 725         if (event_netdev != lag->netdev || !lag->bonded || !lag->upper_netdev)
 726                 return;
 727
 728         if (bonding_info->master.bond_mode != BOND_MODE_ACTIVEBACKUP) {
 729                 netdev_dbg(lag->netdev, "Bonding event recv, but mode not active/backup\n");
 730                 goto lag_out;
 731         }
 732
 733         if (strcmp(bonding_info->slave.slave_name, lag_netdev_name)) {
 734                 netdev_dbg(lag->netdev, "Bonding event recv, but secondary info not for us\n");
 735                 goto lag_out;
 736         }
 737
 738         if (bonding_info->slave.state)
 739                 ice_lag_set_backup(lag);
 740         else
 741                 ice_lag_set_primary(lag);
 742
 743 lag_out:
 744         ice_display_lag_info(lag);
 745 }
 746
 747 /**
 748  * ice_lag_reclaim_vf_tc - move scheduling nodes back to primary interface
 749  * @lag: primary interface lag struct
 750  * @src_hw: HW struct current node location
 751  * @vsi_num: VSI index in PF space
 752  * @tc: traffic class to move
 753  */
 754 static void
 755 ice_lag_reclaim_vf_tc(struct ice_lag *lag, struct ice_hw *src_hw, u16 vsi_num,
 756                       u8 tc)
 757 {
 758         u16 numq, valq, buf_size, num_moved, qbuf_size;
 759         struct device *dev = ice_pf_to_dev(lag->pf);
 760         struct ice_aqc_cfg_txqs_buf *qbuf;
 761         struct ice_aqc_move_elem *buf;
 762         struct ice_sched_node *n_prt;
 763         __le32 teid, parent_teid;
 764         struct ice_vsi_ctx *ctx;
 765         struct ice_hw *hw;
 766         u32 tmp_teid;
 767
 768         hw = &lag->pf->hw;
 769         ctx = ice_get_vsi_ctx(hw, vsi_num);
 770         if (!ctx) {
 771                 dev_warn(dev, "Unable to locate VSI context for LAG reclaim\n");
 772                 return;
 773         }
 774
 775         /* check to see if this VF is enabled on this TC */
 776         if (!ctx->sched.vsi_node[tc])
 777                 return;
 778
 779         numq = ctx->num_lan_q_entries[tc];
 780         teid = ctx->sched.vsi_node[tc]->info.node_teid;
 781         tmp_teid = le32_to_cpu(teid);
 782         parent_teid = ctx->sched.vsi_node[tc]->info.parent_teid;
 783
 784         /* if !teid or !numq, then this TC is not active */
 785         if (!tmp_teid || !numq)
 786                 return;
 787
 788         /* suspend traffic */
 789         if (ice_sched_suspend_resume_elems(hw, 1, &tmp_teid, true))
 790                 dev_dbg(dev, "Problem suspending traffic for LAG node move\n");
 791
 792         /* reconfig queues for new port */
 793         qbuf_size = struct_size(qbuf, queue_info, numq);
 794         qbuf = kzalloc(qbuf_size, GFP_KERNEL);
 795         if (!qbuf) {
 796                 dev_warn(dev, "Failure allocating memory for VF queue recfg buffer\n");
 797                 goto resume_reclaim;
 798         }
 799
 800         /* add the per queue info for the reconfigure command buffer */
 801         valq = ice_lag_qbuf_recfg(hw, qbuf, vsi_num, numq, tc);
 802         if (!valq) {
 803                 dev_dbg(dev, "No valid queues found for LAG reclaim\n");
 804                 goto reclaim_none;
 805         }
 806
 807         if (ice_aq_cfg_lan_txq(hw, qbuf, qbuf_size, numq,
 808                                src_hw->port_info->lport, hw->port_info->lport,
 809                                NULL)) {
 810                 dev_warn(dev, "Failure to configure queues for LAG failover\n");
 811                 goto reclaim_qerr;
 812         }
 813
 814 reclaim_none:
 815         kfree(qbuf);
 816
 817         /* find parent in primary tree */
 818         n_prt = ice_lag_get_sched_parent(hw, tc);
 819         if (!n_prt)
 820                 goto resume_reclaim;
 821
 822         /* Move node to new parent */
 823         buf_size = struct_size(buf, teid, 1);
 824         buf = kzalloc(buf_size, GFP_KERNEL);
 825         if (!buf) {
 826                 dev_warn(dev, "Failure to alloc memory for VF node failover\n");
 827                 goto resume_reclaim;
 828         }
 829
 830         buf->hdr.src_parent_teid = parent_teid;
 831         buf->hdr.dest_parent_teid = n_prt->info.node_teid;
 832         buf->hdr.num_elems = cpu_to_le16(1);
 833         buf->hdr.mode = ICE_AQC_MOVE_ELEM_MODE_KEEP_OWN;
 834         buf->teid[0] = teid;
 835
 836         if (ice_aq_move_sched_elems(&lag->pf->hw, 1, buf, buf_size, &num_moved,
 837                                     NULL))
 838                 dev_warn(dev, "Failure to move VF nodes for LAG reclaim\n");
 839         else
 840                 ice_sched_update_parent(n_prt, ctx->sched.vsi_node[tc]);
 841
 842         kfree(buf);
 843         goto resume_reclaim;
 844
 845 reclaim_qerr:
 846         kfree(qbuf);
 847
 848 resume_reclaim:
 849         /* restart traffic */
 850         if (ice_sched_suspend_resume_elems(hw, 1, &tmp_teid, false))
 851                 dev_warn(dev, "Problem restarting traffic for LAG node reclaim\n");
 852 }
 853
 854 /**
 855  * ice_lag_reclaim_vf_nodes - When interface leaving bond primary reclaims nodes
 856  * @lag: primary interface lag struct
 857  * @src_hw: HW struct for current node location
 858  */
 859 static void
 860 ice_lag_reclaim_vf_nodes(struct ice_lag *lag, struct ice_hw *src_hw)
 861 {
 862         struct ice_pf *pf;
 863         int i, tc;
 864
 865         if (!lag->primary || !src_hw)
 866                 return;
 867
 868         pf = lag->pf;
 869         ice_for_each_vsi(pf, i)
 870                 if (pf->vsi[i] && (pf->vsi[i]->type == ICE_VSI_VF ||
 871                                    pf->vsi[i]->type == ICE_VSI_SWITCHDEV_CTRL))
 872                         ice_for_each_traffic_class(tc)
 873                                 ice_lag_reclaim_vf_tc(lag, src_hw, i, tc);
 874 }
 875
 876 /**
 877  * ice_lag_link - handle LAG link event
 878  * @lag: LAG info struct
 879  */
 880 static void ice_lag_link(struct ice_lag *lag)
 881 {
 882         struct ice_pf *pf = lag->pf;
 883
 884         if (lag->bonded)
 885                 dev_warn(ice_pf_to_dev(pf), "%s Already part of a bond\n",
 886                          netdev_name(lag->netdev));
 887
 888         lag->bonded = true;
 889         lag->role = ICE_LAG_UNSET;
 890         netdev_info(lag->netdev, "Shared SR-IOV resources in bond are active\n");
 891 }
 892
 893 /**
 894  * ice_lag_unlink - handle unlink event
 895  * @lag: LAG info struct
 896  */
 897 static void ice_lag_unlink(struct ice_lag *lag)
 898 {
 899         u8 pri_port, act_port, loc_port;
 900         struct ice_pf *pf = lag->pf;
 901
 902         if (!lag->bonded) {
 903                 netdev_dbg(lag->netdev, "bonding unlink event on non-LAG netdev\n");
 904                 return;
 905         }
 906
 907         if (lag->primary) {
 908                 act_port = lag->active_port;
 909                 pri_port = lag->pf->hw.port_info->lport;
 910                 if (act_port != pri_port && act_port != ICE_LAG_INVALID_PORT)
 911                         ice_lag_move_vf_nodes(lag, act_port, pri_port);
 912                 lag->primary = false;
 913                 lag->active_port = ICE_LAG_INVALID_PORT;
 914         } else {
 915                 struct ice_lag *primary_lag;
 916
 917                 primary_lag = ice_lag_find_primary(lag);
 918                 if (primary_lag) {
 919                         act_port = primary_lag->active_port;
 920                         pri_port = primary_lag->pf->hw.port_info->lport;
 921                         loc_port = pf->hw.port_info->lport;
 922                         if (act_port == loc_port &&
 923                             act_port != ICE_LAG_INVALID_PORT) {
 924                                 ice_lag_reclaim_vf_nodes(primary_lag,
 925                                                          &lag->pf->hw);
 926                                 primary_lag->active_port = ICE_LAG_INVALID_PORT;
 927                         }
 928                 }
 929         }
 930
 931         lag->bonded = false;
 932         lag->role = ICE_LAG_NONE;
 933         lag->upper_netdev = NULL;
 934 }
 935
 936 /**
 937  * ice_lag_link_unlink - helper function to call lag_link/unlink
 938  * @lag: lag info struct
 939  * @ptr: opaque pointer data
 940  */
 941 static void ice_lag_link_unlink(struct ice_lag *lag, void *ptr)
 942 {
 943         struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
 944         struct netdev_notifier_changeupper_info *info = ptr;
 945
 946         if (netdev != lag->netdev)
 947                 return;
 948
 949         if (info->linking)
 950                 ice_lag_link(lag);
 951         else
 952                 ice_lag_unlink(lag);
 953 }
 954
 955 /**
 956  * ice_lag_set_swid - set the SWID on secondary interface
 957  * @primary_swid: primary interface's SWID
 958  * @local_lag: local interfaces LAG struct
 959  * @link: Is this a linking activity
 960  *
 961  * If link is false, then primary_swid should be expected to not be valid
 962  * This function should never be called in interrupt context.
 963  */
 964 static void
 965 ice_lag_set_swid(u16 primary_swid, struct ice_lag *local_lag,
 966                  bool link)
 967 {
 968         struct ice_aqc_alloc_free_res_elem *buf;
 969         struct ice_aqc_set_port_params *cmd;
 970         struct ice_aq_desc desc;
 971         u16 buf_len, swid;
 972         int status, i;
 973
 974         buf_len = struct_size(buf, elem, 1);
 975         buf = kzalloc(buf_len, GFP_KERNEL);
 976         if (!buf) {
 977                 dev_err(ice_pf_to_dev(local_lag->pf), "-ENOMEM error setting SWID\n");
 978                 return;
 979         }
 980
 981         buf->num_elems = cpu_to_le16(1);
 982         buf->res_type = cpu_to_le16(ICE_AQC_RES_TYPE_SWID);
 983         /* if unlinnking need to free the shared resource */
 984         if (!link && local_lag->bond_swid) {
 985                 buf->elem[0].e.sw_resp = cpu_to_le16(local_lag->bond_swid);
 986                 status = ice_aq_alloc_free_res(&local_lag->pf->hw, buf,
 987                                                buf_len, ice_aqc_opc_free_res);
 988                 if (status)
 989                         dev_err(ice_pf_to_dev(local_lag->pf), "Error freeing SWID during LAG unlink\n");
 990                 local_lag->bond_swid = 0;
 991         }
 992
 993         if (link) {
 994                 buf->res_type |=  cpu_to_le16(ICE_LAG_RES_SHARED |
 995                                               ICE_LAG_RES_VALID);
 996                 /* store the primary's SWID in case it leaves bond first */
 997                 local_lag->bond_swid = primary_swid;
 998                 buf->elem[0].e.sw_resp = cpu_to_le16(local_lag->bond_swid);
 999         } else {
1000                 buf->elem[0].e.sw_resp =
1001                         cpu_to_le16(local_lag->pf->hw.port_info->sw_id);
1002         }
1003
1004         status = ice_aq_alloc_free_res(&local_lag->pf->hw, buf, buf_len,
1005                                        ice_aqc_opc_alloc_res);
1006         if (status)
1007                 dev_err(ice_pf_to_dev(local_lag->pf), "Error subscribing to SWID 0x%04X\n",
1008                         local_lag->bond_swid);
1009
1010         kfree(buf);
1011
1012         /* Configure port param SWID to correct value */
1013         if (link)
1014                 swid = primary_swid;
1015         else
1016                 swid = local_lag->pf->hw.port_info->sw_id;
1017
1018         cmd = &desc.params.set_port_params;
1019         ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_set_port_params);
1020
1021         cmd->swid = cpu_to_le16(ICE_AQC_PORT_SWID_VALID | swid);
1022         /* If this is happening in reset context, it is possible that the
1023          * primary interface has not finished setting its SWID to SHARED
1024          * yet.  Allow retries to account for this timing issue between
1025          * interfaces.
1026          */
1027         for (i = 0; i < ICE_LAG_RESET_RETRIES; i++) {
1028                 status = ice_aq_send_cmd(&local_lag->pf->hw, &desc, NULL, 0,
1029                                          NULL);
1030                 if (!status)
1031                         break;
1032
1033                 usleep_range(1000, 2000);
1034         }
1035
1036         if (status)
1037                 dev_err(ice_pf_to_dev(local_lag->pf), "Error setting SWID in port params %d\n",
1038                         status);
1039 }
1040
1041 /**
1042  * ice_lag_primary_swid - set/clear the SHARED attrib of primary's SWID
1043  * @lag: primary interface's lag struct
1044  * @link: is this a linking activity
1045  *
1046  * Implement setting primary SWID as shared using 0x020B
1047  */
1048 static void ice_lag_primary_swid(struct ice_lag *lag, bool link)
1049 {
1050         struct ice_hw *hw;
1051         u16 swid;
1052
1053         hw = &lag->pf->hw;
1054         swid = hw->port_info->sw_id;
1055
1056         if (ice_share_res(hw, ICE_AQC_RES_TYPE_SWID, link, swid))
1057                 dev_warn(ice_pf_to_dev(lag->pf), "Failure to set primary interface shared status\n");
1058 }
1059
1060 /**
1061  * ice_lag_add_prune_list - Adds event_pf's VSI to primary's prune list
1062  * @lag: lag info struct
1063  * @event_pf: PF struct for VSI we are adding to primary's prune list
1064  */
1065 static void ice_lag_add_prune_list(struct ice_lag *lag, struct ice_pf *event_pf)
1066 {
1067         u16 num_vsi, rule_buf_sz, vsi_list_id, event_vsi_num, prim_vsi_idx;
1068         struct ice_sw_rule_vsi_list *s_rule = NULL;
1069         struct device *dev;
1070
1071         num_vsi = 1;
1072
1073         dev = ice_pf_to_dev(lag->pf);
1074         event_vsi_num = event_pf->vsi[0]->vsi_num;
1075         prim_vsi_idx = lag->pf->vsi[0]->idx;
1076
1077         if (!ice_find_vsi_list_entry(&lag->pf->hw, ICE_SW_LKUP_VLAN,
1078                                      prim_vsi_idx, &vsi_list_id)) {
1079                 dev_warn(dev, "Could not locate prune list when setting up SRIOV LAG\n");
1080                 return;
1081         }
1082
1083         rule_buf_sz = (u16)ICE_SW_RULE_VSI_LIST_SIZE(s_rule, num_vsi);
1084         s_rule = kzalloc(rule_buf_sz, GFP_KERNEL);
1085         if (!s_rule) {
1086                 dev_warn(dev, "Error allocating space for prune list when configuring SRIOV LAG\n");
1087                 return;
1088         }
1089
1090         s_rule->hdr.type = cpu_to_le16(ICE_AQC_SW_RULES_T_PRUNE_LIST_SET);
1091         s_rule->index = cpu_to_le16(vsi_list_id);
1092         s_rule->number_vsi = cpu_to_le16(num_vsi);
1093         s_rule->vsi[0] = cpu_to_le16(event_vsi_num);
1094
1095         if (ice_aq_sw_rules(&event_pf->hw, s_rule, rule_buf_sz, 1,
1096                             ice_aqc_opc_update_sw_rules, NULL))
1097                 dev_warn(dev, "Error adding VSI prune list\n");
1098         kfree(s_rule);
1099 }
1100
1101 /**
1102  * ice_lag_del_prune_list - Remove secondary's vsi from primary's prune list
1103  * @lag: primary interface's ice_lag struct
1104  * @event_pf: PF struct for unlinking interface
1105  */
1106 static void ice_lag_del_prune_list(struct ice_lag *lag, struct ice_pf *event_pf)
1107 {
1108         u16 num_vsi, vsi_num, vsi_idx, rule_buf_sz, vsi_list_id;
1109         struct ice_sw_rule_vsi_list *s_rule = NULL;
1110         struct device *dev;
1111
1112         num_vsi = 1;
1113
1114         dev = ice_pf_to_dev(lag->pf);
1115         vsi_num = event_pf->vsi[0]->vsi_num;
1116         vsi_idx = lag->pf->vsi[0]->idx;
1117
1118         if (!ice_find_vsi_list_entry(&lag->pf->hw, ICE_SW_LKUP_VLAN,
1119                                      vsi_idx, &vsi_list_id)) {
1120                 dev_warn(dev, "Could not locate prune list when unwinding SRIOV LAG\n");
1121                 return;
1122         }
1123
1124         rule_buf_sz = (u16)ICE_SW_RULE_VSI_LIST_SIZE(s_rule, num_vsi);
1125         s_rule = kzalloc(rule_buf_sz, GFP_KERNEL);
1126         if (!s_rule) {
1127                 dev_warn(dev, "Error allocating prune list when unwinding SRIOV LAG\n");
1128                 return;
1129         }
1130
1131         s_rule->hdr.type = cpu_to_le16(ICE_AQC_SW_RULES_T_PRUNE_LIST_CLEAR);
1132         s_rule->index = cpu_to_le16(vsi_list_id);
1133         s_rule->number_vsi = cpu_to_le16(num_vsi);
1134         s_rule->vsi[0] = cpu_to_le16(vsi_num);
1135
1136         if (ice_aq_sw_rules(&event_pf->hw, (struct ice_aqc_sw_rules *)s_rule,
1137                             rule_buf_sz, 1, ice_aqc_opc_update_sw_rules, NULL))
1138                 dev_warn(dev, "Error clearing VSI prune list\n");
1139
1140         kfree(s_rule);
1141 }
1142
1143 /**
1144  * ice_lag_init_feature_support_flag - Check for NVM support for LAG
1145  * @pf: PF struct
1146  */
1147 static void ice_lag_init_feature_support_flag(struct ice_pf *pf)
1148 {
1149         struct ice_hw_common_caps *caps;
1150
1151         caps = &pf->hw.dev_caps.common_cap;
1152         if (caps->roce_lag)
1153                 ice_set_feature_support(pf, ICE_F_ROCE_LAG);
1154         else
1155                 ice_clear_feature_support(pf, ICE_F_ROCE_LAG);
1156
1157         if (caps->sriov_lag)
1158                 ice_set_feature_support(pf, ICE_F_SRIOV_LAG);
1159         else
1160                 ice_clear_feature_support(pf, ICE_F_SRIOV_LAG);
1161 }
1162
1163 /**
1164  * ice_lag_changeupper_event - handle LAG changeupper event
1165  * @lag: LAG info struct
1166  * @ptr: opaque pointer data
1167  */
1168 static void ice_lag_changeupper_event(struct ice_lag *lag, void *ptr)
1169 {
1170         struct netdev_notifier_changeupper_info *info;
1171         struct ice_lag *primary_lag;
1172         struct net_device *netdev;
1173
1174         info = ptr;
1175         netdev = netdev_notifier_info_to_dev(ptr);
1176
1177         /* not for this netdev */
1178         if (netdev != lag->netdev)
1179                 return;
1180
1181         primary_lag = ice_lag_find_primary(lag);
1182         if (info->linking) {
1183                 lag->upper_netdev = info->upper_dev;
1184                 /* If there is not already a primary interface in the LAG,
1185                  * then mark this one as primary.
1186                  */
1187                 if (!primary_lag) {
1188                         lag->primary = true;
1189                         /* Configure primary's SWID to be shared */
1190                         ice_lag_primary_swid(lag, true);
1191                         primary_lag = lag;
1192                 } else {
1193                         u16 swid;
1194
1195                         swid = primary_lag->pf->hw.port_info->sw_id;
1196                         ice_lag_set_swid(swid, lag, true);
1197                         ice_lag_add_prune_list(primary_lag, lag->pf);
1198                 }
1199                 /* add filter for primary control packets */
1200                 ice_lag_cfg_cp_fltr(lag, true);
1201         } else {
1202                 if (!primary_lag && lag->primary)
1203                         primary_lag = lag;
1204
1205                 if (!lag->primary) {
1206                         ice_lag_set_swid(0, lag, false);
1207                 } else {
1208                         if (primary_lag && lag->primary) {
1209                                 ice_lag_primary_swid(lag, false);
1210                                 ice_lag_del_prune_list(primary_lag, lag->pf);
1211                         }
1212                 }
1213                 /* remove filter for control packets */
1214                 ice_lag_cfg_cp_fltr(lag, false);
1215         }
1216 }
1217
1218 /**
1219  * ice_lag_monitor_link - monitor interfaces entering/leaving the aggregate
1220  * @lag: lag info struct
1221  * @ptr: opaque data containing notifier event
1222  *
1223  * This function only operates after a primary has been set.
1224  */
1225 static void ice_lag_monitor_link(struct ice_lag *lag, void *ptr)
1226 {
1227         struct netdev_notifier_changeupper_info *info;
1228         struct ice_hw *prim_hw, *active_hw;
1229         struct net_device *event_netdev;
1230         struct ice_pf *pf;
1231         u8 prim_port;
1232
1233         if (!lag->primary)
1234                 return;
1235
1236         event_netdev = netdev_notifier_info_to_dev(ptr);
1237         if (!netif_is_same_ice(lag->pf, event_netdev))
1238                 return;
1239
1240         pf = lag->pf;
1241         prim_hw = &pf->hw;
1242         prim_port = prim_hw->port_info->lport;
1243
1244         info = (struct netdev_notifier_changeupper_info *)ptr;
1245         if (info->upper_dev != lag->upper_netdev)
1246                 return;
1247
1248         if (!info->linking) {
1249                 /* Since there are only two interfaces allowed in SRIOV+LAG, if
1250                  * one port is leaving, then nodes need to be on primary
1251                  * interface.
1252                  */
1253                 if (prim_port != lag->active_port &&
1254                     lag->active_port != ICE_LAG_INVALID_PORT) {
1255                         active_hw = ice_lag_find_hw_by_lport(lag,
1256                                                              lag->active_port);
1257                         ice_lag_reclaim_vf_nodes(lag, active_hw);
1258                         lag->active_port = ICE_LAG_INVALID_PORT;
1259                 }
1260         }
1261 }
1262
1263 /**
1264  * ice_lag_monitor_active - main PF keep track of which port is active
1265  * @lag: lag info struct
1266  * @ptr: opaque data containing notifier event
1267  *
1268  * This function is for the primary PF to monitor changes in which port is
1269  * active and handle changes for SRIOV VF functionality
1270  */
1271 static void ice_lag_monitor_active(struct ice_lag *lag, void *ptr)
1272 {
1273         struct net_device *event_netdev, *event_upper;
1274         struct netdev_notifier_bonding_info *info;
1275         struct netdev_bonding_info *bonding_info;
1276         struct ice_netdev_priv *event_np;
1277         struct ice_pf *pf, *event_pf;
1278         u8 prim_port, event_port;
1279
1280         if (!lag->primary)
1281                 return;
1282
1283         pf = lag->pf;
1284         if (!pf)
1285                 return;
1286
1287         event_netdev = netdev_notifier_info_to_dev(ptr);
1288         rcu_read_lock();
1289         event_upper = netdev_master_upper_dev_get_rcu(event_netdev);
1290         rcu_read_unlock();
1291         if (!netif_is_ice(event_netdev) || event_upper != lag->upper_netdev)
1292                 return;
1293
1294         event_np = netdev_priv(event_netdev);
1295         event_pf = event_np->vsi->back;
1296         event_port = event_pf->hw.port_info->lport;
1297         prim_port = pf->hw.port_info->lport;
1298
1299         info = (struct netdev_notifier_bonding_info *)ptr;
1300         bonding_info = &info->bonding_info;
1301
1302         if (!bonding_info->slave.state) {
1303                 /* if no port is currently active, then nodes and filters exist
1304                  * on primary port, check if we need to move them
1305                  */
1306                 if (lag->active_port == ICE_LAG_INVALID_PORT) {
1307                         if (event_port != prim_port)
1308                                 ice_lag_move_vf_nodes(lag, prim_port,
1309                                                       event_port);
1310                         lag->active_port = event_port;
1311                         return;
1312                 }
1313
1314                 /* active port is already set and is current event port */
1315                 if (lag->active_port == event_port)
1316                         return;
1317                 /* new active port */
1318                 ice_lag_move_vf_nodes(lag, lag->active_port, event_port);
1319                 lag->active_port = event_port;
1320         } else {
1321                 /* port not set as currently active (e.g. new active port
1322                  * has already claimed the nodes and filters
1323                  */
1324                 if (lag->active_port != event_port)
1325                         return;
1326                 /* This is the case when neither port is active (both link down)
1327                  * Link down on the bond - set active port to invalid and move
1328                  * nodes and filters back to primary if not already there
1329                  */
1330                 if (event_port != prim_port)
1331                         ice_lag_move_vf_nodes(lag, event_port, prim_port);
1332                 lag->active_port = ICE_LAG_INVALID_PORT;
1333         }
1334 }
1335
1336 /**
1337  * ice_lag_chk_comp - evaluate bonded interface for feature support
1338  * @lag: lag info struct
1339  * @ptr: opaque data for netdev event info
1340  */
1341 static bool
1342 ice_lag_chk_comp(struct ice_lag *lag, void *ptr)
1343 {
1344         struct net_device *event_netdev, *event_upper;
1345         struct netdev_notifier_bonding_info *info;
1346         struct netdev_bonding_info *bonding_info;
1347         struct list_head *tmp;
1348         struct device *dev;
1349         int count = 0;
1350
1351         if (!lag->primary)
1352                 return true;
1353
1354         event_netdev = netdev_notifier_info_to_dev(ptr);
1355         rcu_read_lock();
1356         event_upper = netdev_master_upper_dev_get_rcu(event_netdev);
1357         rcu_read_unlock();
1358         if (event_upper != lag->upper_netdev)
1359                 return true;
1360
1361         dev = ice_pf_to_dev(lag->pf);
1362
1363         /* only supporting switchdev mode for SRIOV VF LAG.
1364          * primary interface has to be in switchdev mode
1365          */
1366         if (!ice_is_switchdev_running(lag->pf)) {
1367                 dev_info(dev, "Primary interface not in switchdev mode - VF LAG disabled\n");
1368                 return false;
1369         }
1370
1371         info = (struct netdev_notifier_bonding_info *)ptr;
1372         bonding_info = &info->bonding_info;
1373         lag->bond_mode = bonding_info->master.bond_mode;
1374         if (lag->bond_mode != BOND_MODE_ACTIVEBACKUP) {
1375                 dev_info(dev, "Bond Mode not ACTIVE-BACKUP - VF LAG disabled\n");
1376                 return false;
1377         }
1378
1379         list_for_each(tmp, lag->netdev_head) {
1380                 struct ice_dcbx_cfg *dcb_cfg, *peer_dcb_cfg;
1381                 struct ice_lag_netdev_list *entry;
1382                 struct ice_netdev_priv *peer_np;
1383                 struct net_device *peer_netdev;
1384                 struct ice_vsi *vsi, *peer_vsi;
1385                 struct ice_pf *peer_pf;
1386
1387                 entry = list_entry(tmp, struct ice_lag_netdev_list, node);
1388                 peer_netdev = entry->netdev;
1389                 if (!netif_is_ice(peer_netdev)) {
1390                         dev_info(dev, "Found %s non-ice netdev in LAG - VF LAG disabled\n",
1391                                  netdev_name(peer_netdev));
1392                         return false;
1393                 }
1394
1395                 count++;
1396                 if (count > 2) {
1397                         dev_info(dev, "Found more than two netdevs in LAG - VF LAG disabled\n");
1398                         return false;
1399                 }
1400
1401                 peer_np = netdev_priv(peer_netdev);
1402                 vsi = ice_get_main_vsi(lag->pf);
1403                 peer_vsi = peer_np->vsi;
1404                 if (lag->pf->pdev->bus != peer_vsi->back->pdev->bus ||
1405                     lag->pf->pdev->slot != peer_vsi->back->pdev->slot) {
1406                         dev_info(dev, "Found %s on different device in LAG - VF LAG disabled\n",
1407                                  netdev_name(peer_netdev));
1408                         return false;
1409                 }
1410
1411                 dcb_cfg = &vsi->port_info->qos_cfg.local_dcbx_cfg;
1412                 peer_dcb_cfg = &peer_vsi->port_info->qos_cfg.local_dcbx_cfg;
1413                 if (memcmp(dcb_cfg, peer_dcb_cfg,
1414                            sizeof(struct ice_dcbx_cfg))) {
1415                         dev_info(dev, "Found %s with different DCB in LAG - VF LAG disabled\n",
1416                                  netdev_name(peer_netdev));
1417                         return false;
1418                 }
1419
1420                 peer_pf = peer_vsi->back;
1421                 if (test_bit(ICE_FLAG_FW_LLDP_AGENT, peer_pf->flags)) {
1422                         dev_warn(dev, "Found %s with FW LLDP agent active - VF LAG disabled\n",
1423                                  netdev_name(peer_netdev));
1424                         return false;
1425                 }
1426         }
1427
1428         return true;
1429 }
1430
1431 /**
1432  * ice_lag_unregister - handle netdev unregister events
1433  * @lag: LAG info struct
1434  * @event_netdev: netdev struct for target of notifier event
1435  */
1436 static void
1437 ice_lag_unregister(struct ice_lag *lag, struct net_device *event_netdev)
1438 {
1439         struct ice_netdev_priv *np;
1440         struct ice_pf *event_pf;
1441         struct ice_lag *p_lag;
1442
1443         p_lag = ice_lag_find_primary(lag);
1444         np = netdev_priv(event_netdev);
1445         event_pf = np->vsi->back;
1446
1447         if (p_lag) {
1448                 if (p_lag->active_port != p_lag->pf->hw.port_info->lport &&
1449                     p_lag->active_port != ICE_LAG_INVALID_PORT) {
1450                         struct ice_hw *active_hw;
1451
1452                         active_hw = ice_lag_find_hw_by_lport(lag,
1453                                                              p_lag->active_port);
1454                         if (active_hw)
1455                                 ice_lag_reclaim_vf_nodes(p_lag, active_hw);
1456                         lag->active_port = ICE_LAG_INVALID_PORT;
1457                 }
1458         }
1459
1460         /* primary processing for primary */
1461         if (lag->primary && lag->netdev == event_netdev)
1462                 ice_lag_primary_swid(lag, false);
1463
1464         /* primary processing for secondary */
1465         if (lag->primary && lag->netdev != event_netdev)
1466                 ice_lag_del_prune_list(lag, event_pf);
1467
1468         /* secondary processing for secondary */
1469         if (!lag->primary && lag->netdev == event_netdev)
1470                 ice_lag_set_swid(0, lag, false);
1471 }
1472
1473 /**
1474  * ice_lag_monitor_rdma - set and clear rdma functionality
1475  * @lag: pointer to lag struct
1476  * @ptr: opaque data for netdev event info
1477  */
1478 static void
1479 ice_lag_monitor_rdma(struct ice_lag *lag, void *ptr)
1480 {
1481         struct netdev_notifier_changeupper_info *info;
1482         struct net_device *netdev;
1483
1484         info = ptr;
1485         netdev = netdev_notifier_info_to_dev(ptr);
1486
1487         if (netdev != lag->netdev)
1488                 return;
1489
1490         if (info->linking)
1491                 ice_clear_rdma_cap(lag->pf);
1492         else
1493                 ice_set_rdma_cap(lag->pf);
1494 }
1495
1496 /**
1497  * ice_lag_chk_disabled_bond - monitor interfaces entering/leaving disabled bond
1498  * @lag: lag info struct
1499  * @ptr: opaque data containing event
1500  *
1501  * as interfaces enter a bond - determine if the bond is currently
1502  * SRIOV LAG compliant and flag if not.  As interfaces leave the
1503  * bond, reset their compliant status.
1504  */
1505 static void ice_lag_chk_disabled_bond(struct ice_lag *lag, void *ptr)
1506 {
1507         struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1508         struct netdev_notifier_changeupper_info *info = ptr;
1509         struct ice_lag *prim_lag;
1510
1511         if (netdev != lag->netdev)
1512                 return;
1513
1514         if (info->linking) {
1515                 prim_lag = ice_lag_find_primary(lag);
1516                 if (prim_lag &&
1517                     !ice_is_feature_supported(prim_lag->pf, ICE_F_SRIOV_LAG)) {
1518                         ice_clear_feature_support(lag->pf, ICE_F_SRIOV_LAG);
1519                         netdev_info(netdev, "Interface added to non-compliant SRIOV LAG aggregate\n");
1520                 }
1521         } else {
1522                 ice_lag_init_feature_support_flag(lag->pf);
1523         }
1524 }
1525
1526 /**
1527  * ice_lag_disable_sriov_bond - set members of bond as not supporting SRIOV LAG
1528  * @lag: primary interfaces lag struct
1529  */
1530 static void ice_lag_disable_sriov_bond(struct ice_lag *lag)
1531 {
1532         struct ice_lag_netdev_list *entry;
1533         struct ice_netdev_priv *np;
1534         struct net_device *netdev;
1535         struct ice_pf *pf;
1536
1537         list_for_each_entry(entry, lag->netdev_head, node) {
1538                 netdev = entry->netdev;
1539                 np = netdev_priv(netdev);
1540                 pf = np->vsi->back;
1541
1542                 ice_clear_feature_support(pf, ICE_F_SRIOV_LAG);
1543         }
1544 }
1545
1546 /**
1547  * ice_lag_process_event - process a task assigned to the lag_wq
1548  * @work: pointer to work_struct
1549  */
1550 static void ice_lag_process_event(struct work_struct *work)
1551 {
1552         struct netdev_notifier_changeupper_info *info;
1553         struct ice_lag_work *lag_work;
1554         struct net_device *netdev;
1555         struct list_head *tmp, *n;
1556         struct ice_pf *pf;
1557
1558         lag_work = container_of(work, struct ice_lag_work, lag_task);
1559         pf = lag_work->lag->pf;
1560
1561         mutex_lock(&pf->lag_mutex);
1562         lag_work->lag->netdev_head = &lag_work->netdev_list.node;
1563
1564         switch (lag_work->event) {
1565         case NETDEV_CHANGEUPPER:
1566                 info = &lag_work->info.changeupper_info;
1567                 ice_lag_chk_disabled_bond(lag_work->lag, info);
1568                 if (ice_is_feature_supported(pf, ICE_F_SRIOV_LAG)) {
1569                         ice_lag_monitor_link(lag_work->lag, info);
1570                         ice_lag_changeupper_event(lag_work->lag, info);
1571                         ice_lag_link_unlink(lag_work->lag, info);
1572                 }
1573                 ice_lag_monitor_rdma(lag_work->lag, info);
1574                 break;
1575         case NETDEV_BONDING_INFO:
1576                 if (ice_is_feature_supported(pf, ICE_F_SRIOV_LAG)) {
1577                         if (!ice_lag_chk_comp(lag_work->lag,
1578                                               &lag_work->info.bonding_info)) {
1579                                 netdev = lag_work->info.bonding_info.info.dev;
1580                                 ice_lag_disable_sriov_bond(lag_work->lag);
1581                                 ice_lag_unregister(lag_work->lag, netdev);
1582                                 goto lag_cleanup;
1583                         }
1584                         ice_lag_monitor_active(lag_work->lag,
1585                                                &lag_work->info.bonding_info);
1586                         ice_lag_cfg_pf_fltrs(lag_work->lag,
1587                                              &lag_work->info.bonding_info);
1588                 }
1589                 ice_lag_info_event(lag_work->lag, &lag_work->info.bonding_info);
1590                 break;
1591         case NETDEV_UNREGISTER:
1592                 if (ice_is_feature_supported(pf, ICE_F_SRIOV_LAG)) {
1593                         netdev = lag_work->info.bonding_info.info.dev;
1594                         if ((netdev == lag_work->lag->netdev ||
1595                              lag_work->lag->primary) && lag_work->lag->bonded)
1596                                 ice_lag_unregister(lag_work->lag, netdev);
1597                 }
1598                 break;
1599         default:
1600                 break;
1601         }
1602
1603 lag_cleanup:
1604         /* cleanup resources allocated for this work item */
1605         list_for_each_safe(tmp, n, &lag_work->netdev_list.node) {
1606                 struct ice_lag_netdev_list *entry;
1607
1608                 entry = list_entry(tmp, struct ice_lag_netdev_list, node);
1609                 list_del(&entry->node);
1610                 kfree(entry);
1611         }
1612         lag_work->lag->netdev_head = NULL;
1613
1614         mutex_unlock(&pf->lag_mutex);
1615
1616         kfree(lag_work);
1617 }
1618
1619 /**
1620  * ice_lag_event_handler - handle LAG events from netdev
1621  * @notif_blk: notifier block registered by this netdev
1622  * @event: event type
1623  * @ptr: opaque data containing notifier event
1624  */
1625 static int
1626 ice_lag_event_handler(struct notifier_block *notif_blk, unsigned long event,
1627                       void *ptr)
1628 {
1629         struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1630         struct net_device *upper_netdev;
1631         struct ice_lag_work *lag_work;
1632         struct ice_lag *lag;
1633
1634         if (!netif_is_ice(netdev))
1635                 return NOTIFY_DONE;
1636
1637         if (event != NETDEV_CHANGEUPPER && event != NETDEV_BONDING_INFO &&
1638             event != NETDEV_UNREGISTER)
1639                 return NOTIFY_DONE;
1640
1641         if (!(netdev->priv_flags & IFF_BONDING))
1642                 return NOTIFY_DONE;
1643
1644         lag = container_of(notif_blk, struct ice_lag, notif_block);
1645         if (!lag->netdev)
1646                 return NOTIFY_DONE;
1647
1648         if (!net_eq(dev_net(netdev), &init_net))
1649                 return NOTIFY_DONE;
1650
1651         /* This memory will be freed at the end of ice_lag_process_event */
1652         lag_work = kzalloc(sizeof(*lag_work), GFP_KERNEL);
1653         if (!lag_work)
1654                 return -ENOMEM;
1655
1656         lag_work->event_netdev = netdev;
1657         lag_work->lag = lag;
1658         lag_work->event = event;
1659         if (event == NETDEV_CHANGEUPPER) {
1660                 struct netdev_notifier_changeupper_info *info;
1661
1662                 info = ptr;
1663                 upper_netdev = info->upper_dev;
1664         } else {
1665                 upper_netdev = netdev_master_upper_dev_get(netdev);
1666         }
1667
1668         INIT_LIST_HEAD(&lag_work->netdev_list.node);
1669         if (upper_netdev) {
1670                 struct ice_lag_netdev_list *nd_list;
1671                 struct net_device *tmp_nd;
1672
1673                 rcu_read_lock();
1674                 for_each_netdev_in_bond_rcu(upper_netdev, tmp_nd) {
1675                         nd_list = kzalloc(sizeof(*nd_list), GFP_KERNEL);
1676                         if (!nd_list)
1677                                 break;
1678
1679                         nd_list->netdev = tmp_nd;
1680                         list_add(&nd_list->node, &lag_work->netdev_list.node);
1681                 }
1682                 rcu_read_unlock();
1683         }
1684
1685         switch (event) {
1686         case NETDEV_CHANGEUPPER:
1687                 lag_work->info.changeupper_info =
1688                         *((struct netdev_notifier_changeupper_info *)ptr);
1689                 break;
1690         case NETDEV_BONDING_INFO:
1691                 lag_work->info.bonding_info =
1692                         *((struct netdev_notifier_bonding_info *)ptr);
1693                 break;
1694         default:
1695                 lag_work->info.notifier_info =
1696                         *((struct netdev_notifier_info *)ptr);
1697                 break;
1698         }
1699
1700         INIT_WORK(&lag_work->lag_task, ice_lag_process_event);
1701         queue_work(ice_lag_wq, &lag_work->lag_task);
1702
1703         return NOTIFY_DONE;
1704 }
1705
1706 /**
1707  * ice_register_lag_handler - register LAG handler on netdev
1708  * @lag: LAG struct
1709  */
1710 static int ice_register_lag_handler(struct ice_lag *lag)
1711 {
1712         struct device *dev = ice_pf_to_dev(lag->pf);
1713         struct notifier_block *notif_blk;
1714
1715         notif_blk = &lag->notif_block;
1716
1717         if (!notif_blk->notifier_call) {
1718                 notif_blk->notifier_call = ice_lag_event_handler;
1719                 if (register_netdevice_notifier(notif_blk)) {
1720                         notif_blk->notifier_call = NULL;
1721                         dev_err(dev, "FAIL register LAG event handler!\n");
1722                         return -EINVAL;
1723                 }
1724                 dev_dbg(dev, "LAG event handler registered\n");
1725         }
1726         return 0;
1727 }
1728
1729 /**
1730  * ice_unregister_lag_handler - unregister LAG handler on netdev
1731  * @lag: LAG struct
1732  */
1733 static void ice_unregister_lag_handler(struct ice_lag *lag)
1734 {
1735         struct device *dev = ice_pf_to_dev(lag->pf);
1736         struct notifier_block *notif_blk;
1737
1738         notif_blk = &lag->notif_block;
1739         if (notif_blk->notifier_call) {
1740                 unregister_netdevice_notifier(notif_blk);
1741                 dev_dbg(dev, "LAG event handler unregistered\n");
1742         }
1743 }
1744
1745 /**
1746  * ice_create_lag_recipe
1747  * @hw: pointer to HW struct
1748  * @rid: pointer to u16 to pass back recipe index
1749  * @base_recipe: recipe to base the new recipe on
1750  * @prio: priority for new recipe
1751  *
1752  * function returns 0 on error
1753  */
1754 static int ice_create_lag_recipe(struct ice_hw *hw, u16 *rid,
1755                                  const u8 *base_recipe, u8 prio)
1756 {
1757         struct ice_aqc_recipe_data_elem *new_rcp;
1758         int err;
1759
1760         err = ice_alloc_recipe(hw, rid);
1761         if (err)
1762                 return err;
1763
1764         new_rcp = kzalloc(ICE_RECIPE_LEN * ICE_MAX_NUM_RECIPES, GFP_KERNEL);
1765         if (!new_rcp)
1766                 return -ENOMEM;
1767
1768         memcpy(new_rcp, base_recipe, ICE_RECIPE_LEN);
1769         new_rcp->content.act_ctrl_fwd_priority = prio;
1770         new_rcp->content.rid = *rid | ICE_AQ_RECIPE_ID_IS_ROOT;
1771         new_rcp->recipe_indx = *rid;
1772         bitmap_zero((unsigned long *)new_rcp->recipe_bitmap,
1773                     ICE_MAX_NUM_RECIPES);
1774         set_bit(*rid, (unsigned long *)new_rcp->recipe_bitmap);
1775
1776         err = ice_aq_add_recipe(hw, new_rcp, 1, NULL);
1777         if (err)
1778                 *rid = 0;
1779
1780         kfree(new_rcp);
1781         return err;
1782 }
1783
1784 /**
1785  * ice_lag_move_vf_nodes_tc_sync - move a VF's nodes for a tc during reset
1786  * @lag: primary interfaces lag struct
1787  * @dest_hw: HW struct for destination's interface
1788  * @vsi_num: VSI index in PF space
1789  * @tc: traffic class to move
1790  */
1791 static void
1792 ice_lag_move_vf_nodes_tc_sync(struct ice_lag *lag, struct ice_hw *dest_hw,
1793                               u16 vsi_num, u8 tc)
1794 {
1795         u16 numq, valq, buf_size, num_moved, qbuf_size;
1796         struct device *dev = ice_pf_to_dev(lag->pf);
1797         struct ice_aqc_cfg_txqs_buf *qbuf;
1798         struct ice_aqc_move_elem *buf;
1799         struct ice_sched_node *n_prt;
1800         __le32 teid, parent_teid;
1801         struct ice_vsi_ctx *ctx;
1802         struct ice_hw *hw;
1803         u32 tmp_teid;
1804
1805         hw = &lag->pf->hw;
1806         ctx = ice_get_vsi_ctx(hw, vsi_num);
1807         if (!ctx) {
1808                 dev_warn(dev, "LAG rebuild failed after reset due to VSI Context failure\n");
1809                 return;
1810         }
1811
1812         if (!ctx->sched.vsi_node[tc])
1813                 return;
1814
1815         numq = ctx->num_lan_q_entries[tc];
1816         teid = ctx->sched.vsi_node[tc]->info.node_teid;
1817         tmp_teid = le32_to_cpu(teid);
1818         parent_teid = ctx->sched.vsi_node[tc]->info.parent_teid;
1819
1820         if (!tmp_teid || !numq)
1821                 return;
1822
1823         if (ice_sched_suspend_resume_elems(hw, 1, &tmp_teid, true))
1824                 dev_dbg(dev, "Problem suspending traffic during reset rebuild\n");
1825
1826         /* reconfig queues for new port */
1827         qbuf_size = struct_size(qbuf, queue_info, numq);
1828         qbuf = kzalloc(qbuf_size, GFP_KERNEL);
1829         if (!qbuf) {
1830                 dev_warn(dev, "Failure allocating VF queue recfg buffer for reset rebuild\n");
1831                 goto resume_sync;
1832         }
1833
1834         /* add the per queue info for the reconfigure command buffer */
1835         valq = ice_lag_qbuf_recfg(hw, qbuf, vsi_num, numq, tc);
1836         if (!valq) {
1837                 dev_warn(dev, "Failure to reconfig queues for LAG reset rebuild\n");
1838                 goto sync_none;
1839         }
1840
1841         if (ice_aq_cfg_lan_txq(hw, qbuf, qbuf_size, numq, hw->port_info->lport,
1842                                dest_hw->port_info->lport, NULL)) {
1843                 dev_warn(dev, "Failure to configure queues for LAG reset rebuild\n");
1844                 goto sync_qerr;
1845         }
1846
1847 sync_none:
1848         kfree(qbuf);
1849
1850         /* find parent in destination tree */
1851         n_prt = ice_lag_get_sched_parent(dest_hw, tc);
1852         if (!n_prt)
1853                 goto resume_sync;
1854
1855         /* Move node to new parent */
1856         buf_size = struct_size(buf, teid, 1);
1857         buf = kzalloc(buf_size, GFP_KERNEL);
1858         if (!buf) {
1859                 dev_warn(dev, "Failure to alloc for VF node move in reset rebuild\n");
1860                 goto resume_sync;
1861         }
1862
1863         buf->hdr.src_parent_teid = parent_teid;
1864         buf->hdr.dest_parent_teid = n_prt->info.node_teid;
1865         buf->hdr.num_elems = cpu_to_le16(1);
1866         buf->hdr.mode = ICE_AQC_MOVE_ELEM_MODE_KEEP_OWN;
1867         buf->teid[0] = teid;
1868
1869         if (ice_aq_move_sched_elems(&lag->pf->hw, 1, buf, buf_size, &num_moved,
1870                                     NULL))
1871                 dev_warn(dev, "Failure to move VF nodes for LAG reset rebuild\n");
1872         else
1873                 ice_sched_update_parent(n_prt, ctx->sched.vsi_node[tc]);
1874
1875         kfree(buf);
1876         goto resume_sync;
1877
1878 sync_qerr:
1879         kfree(qbuf);
1880
1881 resume_sync:
1882         if (ice_sched_suspend_resume_elems(hw, 1, &tmp_teid, false))
1883                 dev_warn(dev, "Problem restarting traffic for LAG node reset rebuild\n");
1884 }
1885
1886 /**
1887  * ice_lag_move_vf_nodes_sync - move vf nodes to active interface
1888  * @lag: primary interfaces lag struct
1889  * @dest_hw: lport value for currently active port
1890  *
1891  * This function is used in a reset context, outside of event handling,
1892  * to move the VF nodes to the secondary interface when that interface
1893  * is the active interface during a reset rebuild
1894  */
1895 static void
1896 ice_lag_move_vf_nodes_sync(struct ice_lag *lag, struct ice_hw *dest_hw)
1897 {
1898         struct ice_pf *pf;
1899         int i, tc;
1900
1901         if (!lag->primary || !dest_hw)
1902                 return;
1903
1904         pf = lag->pf;
1905         ice_for_each_vsi(pf, i)
1906                 if (pf->vsi[i] && (pf->vsi[i]->type == ICE_VSI_VF ||
1907                                    pf->vsi[i]->type == ICE_VSI_SWITCHDEV_CTRL))
1908                         ice_for_each_traffic_class(tc)
1909                                 ice_lag_move_vf_nodes_tc_sync(lag, dest_hw, i,
1910                                                               tc);
1911 }
1912
1913 /**
1914  * ice_init_lag - initialize support for LAG
1915  * @pf: PF struct
1916  *
1917  * Alloc memory for LAG structs and initialize the elements.
1918  * Memory will be freed in ice_deinit_lag
1919  */
1920 int ice_init_lag(struct ice_pf *pf)
1921 {
1922         struct device *dev = ice_pf_to_dev(pf);
1923         struct ice_lag *lag;
1924         struct ice_vsi *vsi;
1925         u64 recipe_bits = 0;
1926         int n, err;
1927
1928         ice_lag_init_feature_support_flag(pf);
1929
1930         pf->lag = kzalloc(sizeof(*lag), GFP_KERNEL);
1931         if (!pf->lag)
1932                 return -ENOMEM;
1933         lag = pf->lag;
1934
1935         vsi = ice_get_main_vsi(pf);
1936         if (!vsi) {
1937                 dev_err(dev, "couldn't get main vsi, link aggregation init fail\n");
1938                 err = -EIO;
1939                 goto lag_error;
1940         }
1941
1942         lag->pf = pf;
1943         lag->netdev = vsi->netdev;
1944         lag->role = ICE_LAG_NONE;
1945         lag->active_port = ICE_LAG_INVALID_PORT;
1946         lag->bonded = false;
1947         lag->upper_netdev = NULL;
1948         lag->notif_block.notifier_call = NULL;
1949
1950         err = ice_register_lag_handler(lag);
1951         if (err) {
1952                 dev_warn(dev, "INIT LAG: Failed to register event handler\n");
1953                 goto lag_error;
1954         }
1955
1956         err = ice_create_lag_recipe(&pf->hw, &lag->pf_recipe, ice_dflt_vsi_rcp,
1957                                     1);
1958         if (err)
1959                 goto lag_error;
1960
1961         /* associate recipes to profiles */
1962         for (n = 0; n < ICE_PROFID_IPV6_GTPU_IPV6_TCP_INNER; n++) {
1963                 err = ice_aq_get_recipe_to_profile(&pf->hw, n,
1964                                                    (u8 *)&recipe_bits, NULL);
1965                 if (err)
1966                         continue;
1967
1968                 if (recipe_bits & BIT(ICE_SW_LKUP_DFLT)) {
1969                         recipe_bits |= BIT(lag->pf_recipe);
1970                         ice_aq_map_recipe_to_profile(&pf->hw, n,
1971                                                      (u8 *)&recipe_bits, NULL);
1972                 }
1973         }
1974
1975         ice_display_lag_info(lag);
1976
1977         dev_dbg(dev, "INIT LAG complete\n");
1978         return 0;
1979
1980 lag_error:
1981         kfree(lag);
1982         pf->lag = NULL;
1983         return err;
1984 }
1985
1986 /**
1987  * ice_deinit_lag - Clean up LAG
1988  * @pf: PF struct
1989  *
1990  * Clean up kernel LAG info and free memory
1991  * This function is meant to only be called on driver remove/shutdown
1992  */
1993 void ice_deinit_lag(struct ice_pf *pf)
1994 {
1995         struct ice_lag *lag;
1996
1997         lag = pf->lag;
1998
1999         if (!lag)
2000                 return;
2001
2002         if (lag->pf)
2003                 ice_unregister_lag_handler(lag);
2004
2005         flush_workqueue(ice_lag_wq);
2006
2007         ice_free_hw_res(&pf->hw, ICE_AQC_RES_TYPE_RECIPE, 1,
2008                         &pf->lag->pf_recipe);
2009
2010         kfree(lag);
2011
2012         pf->lag = NULL;
2013 }
2014
2015 /**
2016  * ice_lag_rebuild - rebuild lag resources after reset
2017  * @pf: pointer to local pf struct
2018  *
2019  * PF resets are promoted to CORER resets when interface in an aggregate.  This
2020  * means that we need to rebuild the PF resources for the interface.  Since
2021  * this will happen outside the normal event processing, need to acquire the lag
2022  * lock.
2023  *
2024  * This function will also evaluate the VF resources if this is the primary
2025  * interface.
2026  */
2027 void ice_lag_rebuild(struct ice_pf *pf)
2028 {
2029         struct ice_lag_netdev_list ndlist;
2030         struct ice_lag *lag, *prim_lag;
2031         struct list_head *tmp, *n;
2032         u8 act_port, loc_port;
2033
2034         if (!pf->lag || !pf->lag->bonded)
2035                 return;
2036
2037         mutex_lock(&pf->lag_mutex);
2038
2039         lag = pf->lag;
2040         if (lag->primary) {
2041                 prim_lag = lag;
2042         } else {
2043                 struct ice_lag_netdev_list *nl;
2044                 struct net_device *tmp_nd;
2045
2046                 INIT_LIST_HEAD(&ndlist.node);
2047                 rcu_read_lock();
2048                 for_each_netdev_in_bond_rcu(lag->upper_netdev, tmp_nd) {
2049                         nl = kzalloc(sizeof(*nl), GFP_KERNEL);
2050                         if (!nl)
2051                                 break;
2052
2053                         nl->netdev = tmp_nd;
2054                         list_add(&nl->node, &ndlist.node);
2055                 }
2056                 rcu_read_unlock();
2057                 lag->netdev_head = &ndlist.node;
2058                 prim_lag = ice_lag_find_primary(lag);
2059         }
2060
2061         if (!prim_lag) {
2062                 dev_dbg(ice_pf_to_dev(pf), "No primary interface in aggregate, can't rebuild\n");
2063                 goto lag_rebuild_out;
2064         }
2065
2066         act_port = prim_lag->active_port;
2067         loc_port = lag->pf->hw.port_info->lport;
2068
2069         /* configure SWID for this port */
2070         if (lag->primary) {
2071                 ice_lag_primary_swid(lag, true);
2072         } else {
2073                 ice_lag_set_swid(prim_lag->pf->hw.port_info->sw_id, lag, true);
2074                 ice_lag_add_prune_list(prim_lag, pf);
2075                 if (act_port == loc_port)
2076                         ice_lag_move_vf_nodes_sync(prim_lag, &pf->hw);
2077         }
2078
2079         ice_lag_cfg_cp_fltr(lag, true);
2080
2081         if (lag->pf_rule_id)
2082                 if (ice_lag_cfg_dflt_fltr(lag, true))
2083                         dev_err(ice_pf_to_dev(pf), "Error adding default VSI rule in rebuild\n");
2084
2085         ice_clear_rdma_cap(pf);
2086 lag_rebuild_out:
2087         list_for_each_safe(tmp, n, &ndlist.node) {
2088                 struct ice_lag_netdev_list *entry;
2089
2090                 entry = list_entry(tmp, struct ice_lag_netdev_list, node);
2091                 list_del(&entry->node);
2092                 kfree(entry);
2093         }
2094         mutex_unlock(&pf->lag_mutex);
2095 }