net/ice/base: check failed acts allocation
[dpdk.git] / drivers / net / ice / base / ice_common.c
index ec8d460..46754a3 100644 (file)
@@ -760,8 +760,7 @@ enum ice_status ice_init_hw(struct ice_hw *hw)
        /* Query the allocated resources for Tx scheduler */
        status = ice_sched_query_res_alloc(hw);
        if (status) {
-               ice_debug(hw, ICE_DBG_SCHED,
-                         "Failed to get scheduler allocated resources\n");
+               ice_debug(hw, ICE_DBG_SCHED, "Failed to get scheduler allocated resources\n");
                goto err_unroll_alloc;
        }
        ice_sched_get_psm_clk_freq(hw);
@@ -770,7 +769,6 @@ enum ice_status ice_init_hw(struct ice_hw *hw)
        status = ice_sched_init_port(hw->port_info);
        if (status)
                goto err_unroll_sched;
-
        pcaps = (struct ice_aqc_get_phy_caps_data *)
                ice_malloc(hw, sizeof(*pcaps));
        if (!pcaps) {
@@ -783,7 +781,8 @@ enum ice_status ice_init_hw(struct ice_hw *hw)
                                     ICE_AQC_REPORT_TOPO_CAP, pcaps, NULL);
        ice_free(hw, pcaps);
        if (status)
-               goto err_unroll_sched;
+               ice_debug(hw, ICE_DBG_PHY, "%s: Get PHY capabilities failed, continuing anyway\n",
+                         __func__);
 
        /* Initialize port_info struct with link information */
        status = ice_aq_get_link_info(hw->port_info, false, NULL, NULL);
@@ -898,8 +897,7 @@ enum ice_status ice_check_reset(struct ice_hw *hw)
        }
 
        if (cnt == grst_timeout) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "Global reset polling failed to complete.\n");
+               ice_debug(hw, ICE_DBG_INIT, "Global reset polling failed to complete.\n");
                return ICE_ERR_RESET_FAILED;
        }
 
@@ -917,16 +915,14 @@ enum ice_status ice_check_reset(struct ice_hw *hw)
        for (cnt = 0; cnt < ICE_PF_RESET_WAIT_COUNT; cnt++) {
                reg = rd32(hw, GLNVM_ULD) & uld_mask;
                if (reg == uld_mask) {
-                       ice_debug(hw, ICE_DBG_INIT,
-                                 "Global reset processes done. %d\n", cnt);
+                       ice_debug(hw, ICE_DBG_INIT, "Global reset processes done. %d\n", cnt);
                        break;
                }
                ice_msec_delay(10, true);
        }
 
        if (cnt == ICE_PF_RESET_WAIT_COUNT) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "Wait for Reset Done timed out. GLNVM_ULD = 0x%x\n",
+               ice_debug(hw, ICE_DBG_INIT, "Wait for Reset Done timed out. GLNVM_ULD = 0x%x\n",
                          reg);
                return ICE_ERR_RESET_FAILED;
        }
@@ -978,8 +974,7 @@ static enum ice_status ice_pf_reset(struct ice_hw *hw)
        }
 
        if (cnt == ICE_PF_RESET_WAIT_COUNT) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "PF reset polling failed to complete.\n");
+               ice_debug(hw, ICE_DBG_INIT, "PF reset polling failed to complete.\n");
                return ICE_ERR_RESET_FAILED;
        }
 
@@ -1626,8 +1621,7 @@ ice_acquire_res(struct ice_hw *hw, enum ice_aq_res_ids res,
                goto ice_acquire_res_exit;
 
        if (status)
-               ice_debug(hw, ICE_DBG_RES,
-                         "resource %d acquire type %d failed.\n", res, access);
+               ice_debug(hw, ICE_DBG_RES, "resource %d acquire type %d failed.\n", res, access);
 
        /* If necessary, poll until the current lock owner timeouts */
        timeout = time_left;
@@ -1650,11 +1644,9 @@ ice_acquire_res(struct ice_hw *hw, enum ice_aq_res_ids res,
 ice_acquire_res_exit:
        if (status == ICE_ERR_AQ_NO_WORK) {
                if (access == ICE_RES_WRITE)
-                       ice_debug(hw, ICE_DBG_RES,
-                                 "resource indicates no work to do.\n");
+                       ice_debug(hw, ICE_DBG_RES, "resource indicates no work to do.\n");
                else
-                       ice_debug(hw, ICE_DBG_RES,
-                                 "Warning: ICE_ERR_AQ_NO_WORK not expected\n");
+                       ice_debug(hw, ICE_DBG_RES, "Warning: ICE_ERR_AQ_NO_WORK not expected\n");
        }
        return status;
 }
@@ -1739,9 +1731,8 @@ ice_alloc_hw_res(struct ice_hw *hw, u16 type, u16 num, bool btm, u16 *res)
        enum ice_status status;
        u16 buf_len;
 
-       buf_len = ice_struct_size(buf, elem, num - 1);
-       buf = (struct ice_aqc_alloc_free_res_elem *)
-               ice_malloc(hw, buf_len);
+       buf_len = ice_struct_size(buf, elem, num);
+       buf = (struct ice_aqc_alloc_free_res_elem *)ice_malloc(hw, buf_len);
        if (!buf)
                return ICE_ERR_NO_MEMORY;
 
@@ -1757,7 +1748,7 @@ ice_alloc_hw_res(struct ice_hw *hw, u16 type, u16 num, bool btm, u16 *res)
        if (status)
                goto ice_alloc_res_exit;
 
-       ice_memcpy(res, buf->elem, sizeof(buf->elem) * num,
+       ice_memcpy(res, buf->elem, sizeof(*buf->elem) * num,
                   ICE_NONDMA_TO_NONDMA);
 
 ice_alloc_res_exit:
@@ -1778,7 +1769,7 @@ enum ice_status ice_free_hw_res(struct ice_hw *hw, u16 type, u16 num, u16 *res)
        enum ice_status status;
        u16 buf_len;
 
-       buf_len = ice_struct_size(buf, elem, num - 1);
+       buf_len = ice_struct_size(buf, elem, num);
        buf = (struct ice_aqc_alloc_free_res_elem *)ice_malloc(hw, buf_len);
        if (!buf)
                return ICE_ERR_NO_MEMORY;
@@ -1786,7 +1777,7 @@ enum ice_status ice_free_hw_res(struct ice_hw *hw, u16 type, u16 num, u16 *res)
        /* Prepare buffer to free resource. */
        buf->num_elems = CPU_TO_LE16(num);
        buf->res_type = CPU_TO_LE16(type);
-       ice_memcpy(buf->elem, res, sizeof(buf->elem) * num,
+       ice_memcpy(buf->elem, res, sizeof(*buf->elem) * num,
                   ICE_NONDMA_TO_NONDMA);
 
        status = ice_aq_alloc_free_res(hw, num, buf, buf_len,
@@ -1847,60 +1838,48 @@ ice_parse_common_caps(struct ice_hw *hw, struct ice_hw_common_caps *caps,
        switch (cap) {
        case ICE_AQC_CAPS_VALID_FUNCTIONS:
                caps->valid_functions = number;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: valid_functions (bitmap) = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: valid_functions (bitmap) = %d\n", prefix,
                          caps->valid_functions);
                break;
        case ICE_AQC_CAPS_DCB:
                caps->dcb = (number == 1);
                caps->active_tc_bitmap = logical_id;
                caps->maxtc = phys_id;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: dcb = %d\n", prefix, caps->dcb);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: active_tc_bitmap = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: dcb = %d\n", prefix, caps->dcb);
+               ice_debug(hw, ICE_DBG_INIT, "%s: active_tc_bitmap = %d\n", prefix,
                          caps->active_tc_bitmap);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: maxtc = %d\n", prefix, caps->maxtc);
+               ice_debug(hw, ICE_DBG_INIT, "%s: maxtc = %d\n", prefix, caps->maxtc);
                break;
        case ICE_AQC_CAPS_RSS:
                caps->rss_table_size = number;
                caps->rss_table_entry_width = logical_id;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: rss_table_size = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: rss_table_size = %d\n", prefix,
                          caps->rss_table_size);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: rss_table_entry_width = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: rss_table_entry_width = %d\n", prefix,
                          caps->rss_table_entry_width);
                break;
        case ICE_AQC_CAPS_RXQS:
                caps->num_rxq = number;
                caps->rxq_first_id = phys_id;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: num_rxq = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: num_rxq = %d\n", prefix,
                          caps->num_rxq);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: rxq_first_id = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: rxq_first_id = %d\n", prefix,
                          caps->rxq_first_id);
                break;
        case ICE_AQC_CAPS_TXQS:
                caps->num_txq = number;
                caps->txq_first_id = phys_id;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: num_txq = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: num_txq = %d\n", prefix,
                          caps->num_txq);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: txq_first_id = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: txq_first_id = %d\n", prefix,
                          caps->txq_first_id);
                break;
        case ICE_AQC_CAPS_MSIX:
                caps->num_msix_vectors = number;
                caps->msix_vector_first_id = phys_id;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: num_msix_vectors = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: num_msix_vectors = %d\n", prefix,
                          caps->num_msix_vectors);
-               ice_debug(hw, ICE_DBG_INIT,
-                         "%s: msix_vector_first_id = %d\n", prefix,
+               ice_debug(hw, ICE_DBG_INIT, "%s: msix_vector_first_id = %d\n", prefix,
                          caps->msix_vector_first_id);
                break;
        case ICE_AQC_CAPS_MAX_MTU:
@@ -1934,8 +1913,7 @@ ice_recalc_port_limited_caps(struct ice_hw *hw, struct ice_hw_common_caps *caps)
        if (hw->dev_caps.num_funcs > 4) {
                /* Max 4 TCs per port */
                caps->maxtc = 4;
-               ice_debug(hw, ICE_DBG_INIT,
-                         "reducing maxtc to %d (based on #ports)\n",
+               ice_debug(hw, ICE_DBG_INIT, "reducing maxtc to %d (based on #ports)\n",
                          caps->maxtc);
        }
 }
@@ -1984,11 +1962,9 @@ ice_parse_fdir_func_caps(struct ice_hw *hw, struct ice_hw_func_caps *func_p,
                GLQF_FD_SIZE_FD_BSIZE_S;
        func_p->fd_fltr_best_effort = val;
 
-       ice_debug(hw, ICE_DBG_INIT,
-                 "func caps: fd_fltr_guar = %d\n",
+       ice_debug(hw, ICE_DBG_INIT, "func caps: fd_fltr_guar = %d\n",
                  func_p->fd_fltr_guar);
-       ice_debug(hw, ICE_DBG_INIT,
-                 "func caps: fd_fltr_best_effort = %d\n",
+       ice_debug(hw, ICE_DBG_INIT, "func caps: fd_fltr_best_effort = %d\n",
                  func_p->fd_fltr_best_effort);
 }
 
@@ -2034,8 +2010,7 @@ ice_parse_func_caps(struct ice_hw *hw, struct ice_hw_func_caps *func_p,
                default:
                        /* Don't list common capabilities as unknown */
                        if (!found)
-                               ice_debug(hw, ICE_DBG_INIT,
-                                         "func caps: unknown capability[%d]: 0x%x\n",
+                               ice_debug(hw, ICE_DBG_INIT, "func caps: unknown capability[%d]: 0x%x\n",
                                          i, cap);
                        break;
                }
@@ -2146,8 +2121,7 @@ ice_parse_dev_caps(struct ice_hw *hw, struct ice_hw_dev_caps *dev_p,
                default:
                        /* Don't list common capabilities as unknown */
                        if (!found)
-                               ice_debug(hw, ICE_DBG_INIT,
-                                         "dev caps: unknown capability[%d]: 0x%x\n",
+                               ice_debug(hw, ICE_DBG_INIT, "dev caps: unknown capability[%d]: 0x%x\n",
                                          i, cap);
                        break;
                }
@@ -2599,8 +2573,7 @@ ice_aq_set_phy_cfg(struct ice_hw *hw, struct ice_port_info *pi,
 
        /* Ensure that only valid bits of cfg->caps can be turned on. */
        if (cfg->caps & ~ICE_AQ_PHY_ENA_VALID_MASK) {
-               ice_debug(hw, ICE_DBG_PHY,
-                         "Invalid bit is set in ice_aqc_set_phy_cfg_data->caps : 0x%x\n",
+               ice_debug(hw, ICE_DBG_PHY, "Invalid bit is set in ice_aqc_set_phy_cfg_data->caps : 0x%x\n",
                          cfg->caps);
 
                cfg->caps &= ICE_AQ_PHY_ENA_VALID_MASK;
@@ -2750,40 +2723,49 @@ enum ice_fec_mode ice_caps_to_fec_mode(u8 caps, u8 fec_options)
        return ICE_FEC_NONE;
 }
 
+/**
+ * ice_cfg_phy_fc - Configure PHY FC data based on FC mode
+ * @pi: port information structure
+ * @cfg: PHY configuration data to set FC mode
+ * @req_mode: FC mode to configure
+ */
 static enum ice_status
 ice_cfg_phy_fc(struct ice_port_info *pi, struct ice_aqc_set_phy_cfg_data *cfg,
               enum ice_fc_mode req_mode)
 {
-       struct ice_aqc_get_phy_caps_data *pcaps = NULL;
        struct ice_phy_cache_mode_data cache_data;
-       enum ice_status status = ICE_SUCCESS;
        u8 pause_mask = 0x0;
 
        if (!pi || !cfg)
                return ICE_ERR_BAD_PTR;
 
-       pcaps = (struct ice_aqc_get_phy_caps_data *)
-               ice_malloc(pi->hw, sizeof(*pcaps));
-       if (!pcaps)
-               return ICE_ERR_NO_MEMORY;
-
-       /* Cache user FC request */
-       cache_data.data.curr_user_fc_req = req_mode;
-       ice_cache_phy_user_req(pi, cache_data, ICE_FC_MODE);
-
        switch (req_mode) {
        case ICE_FC_AUTO:
+       {
+               struct ice_aqc_get_phy_caps_data *pcaps;
+               enum ice_status status;
+
+               pcaps = (struct ice_aqc_get_phy_caps_data *)
+                       ice_malloc(pi->hw, sizeof(*pcaps));
+               if (!pcaps)
+                       return ICE_ERR_NO_MEMORY;
+
                /* Query the value of FC that both the NIC and attached media
                 * can do.
                 */
                status = ice_aq_get_phy_caps(pi, false, ICE_AQC_REPORT_TOPO_CAP,
                                             pcaps, NULL);
-               if (status)
-                       goto out;
+               if (status) {
+                       ice_free(pi->hw, pcaps);
+                       return status;
+               }
 
                pause_mask |= pcaps->caps & ICE_AQC_PHY_EN_TX_LINK_PAUSE;
                pause_mask |= pcaps->caps & ICE_AQC_PHY_EN_RX_LINK_PAUSE;
+
+               ice_free(pi->hw, pcaps);
                break;
+       }
        case ICE_FC_FULL:
                pause_mask |= ICE_AQC_PHY_EN_TX_LINK_PAUSE;
                pause_mask |= ICE_AQC_PHY_EN_RX_LINK_PAUSE;
@@ -2805,9 +2787,11 @@ ice_cfg_phy_fc(struct ice_port_info *pi, struct ice_aqc_set_phy_cfg_data *cfg,
        /* set the new capabilities */
        cfg->caps |= pause_mask;
 
-out:
-       ice_free(pi->hw, pcaps);
-       return status;
+       /* Cache user FC request */
+       cache_data.data.curr_user_fc_req = req_mode;
+       ice_cache_phy_user_req(pi, cache_data, ICE_FC_MODE);
+
+       return ICE_SUCCESS;
 }
 
 /**
@@ -3073,8 +3057,7 @@ enum ice_status ice_get_link_status(struct ice_port_info *pi, bool *link_up)
                status = ice_update_link_info(pi);
 
                if (status)
-                       ice_debug(pi->hw, ICE_DBG_LINK,
-                                 "get link status error, status = %d\n",
+                       ice_debug(pi->hw, ICE_DBG_LINK, "get link status error, status = %d\n",
                                  status);
        }
 
@@ -3469,10 +3452,10 @@ ice_aq_add_lan_txq(struct ice_hw *hw, u8 num_qgrps,
                   struct ice_aqc_add_tx_qgrp *qg_list, u16 buf_size,
                   struct ice_sq_cd *cd)
 {
-       u16 i, sum_header_size, sum_q_size = 0;
        struct ice_aqc_add_tx_qgrp *list;
        struct ice_aqc_add_txqs *cmd;
        struct ice_aq_desc desc;
+       u16 i, sum_size = 0;
 
        ice_debug(hw, ICE_DBG_TRACE, "%s\n", __func__);
 
@@ -3486,18 +3469,13 @@ ice_aq_add_lan_txq(struct ice_hw *hw, u8 num_qgrps,
        if (num_qgrps > ICE_LAN_TXQ_MAX_QGRPS)
                return ICE_ERR_PARAM;
 
-       sum_header_size = num_qgrps *
-               (sizeof(*qg_list) - sizeof(*qg_list->txqs));
-
-       list = qg_list;
-       for (i = 0; i < num_qgrps; i++) {
-               struct ice_aqc_add_txqs_perq *q = list->txqs;
-
-               sum_q_size += list->num_txqs * sizeof(*q);
-               list = (struct ice_aqc_add_tx_qgrp *)(q + list->num_txqs);
+       for (i = 0, list = qg_list; i < num_qgrps; i++) {
+               sum_size += ice_struct_size(list, txqs, list->num_txqs);
+               list = (struct ice_aqc_add_tx_qgrp *)(list->txqs +
+                                                     list->num_txqs);
        }
 
-       if (buf_size != (sum_header_size + sum_q_size))
+       if (buf_size != sum_size)
                return ICE_ERR_PARAM;
 
        desc.flags |= CPU_TO_LE16(ICE_AQ_FLAG_RD);
@@ -3525,6 +3503,7 @@ ice_aq_dis_lan_txq(struct ice_hw *hw, u8 num_qgrps,
                   enum ice_disq_rst_src rst_src, u16 vmvf_num,
                   struct ice_sq_cd *cd)
 {
+       struct ice_aqc_dis_txq_item *item;
        struct ice_aqc_dis_txqs *cmd;
        struct ice_aq_desc desc;
        enum ice_status status;
@@ -3568,16 +3547,16 @@ ice_aq_dis_lan_txq(struct ice_hw *hw, u8 num_qgrps,
         */
        desc.flags |= CPU_TO_LE16(ICE_AQ_FLAG_RD);
 
-       for (i = 0; i < num_qgrps; ++i) {
-               /* Calculate the size taken up by the queue IDs in this group */
-               sz += qg_list[i].num_qs * sizeof(qg_list[i].q_id);
-
-               /* Add the size of the group header */
-               sz += sizeof(qg_list[i]) - sizeof(qg_list[i].q_id);
+       for (i = 0, item = qg_list; i < num_qgrps; i++) {
+               u16 item_size = ice_struct_size(item, q_id, item->num_qs);
 
                /* If the num of queues is even, add 2 bytes of padding */
-               if ((qg_list[i].num_qs % 2) == 0)
-                       sz += 2;
+               if ((item->num_qs % 2) == 0)
+                       item_size += 2;
+
+               sz += item_size;
+
+               item = (struct ice_aqc_dis_txq_item *)((u8 *)item + item_size);
        }
 
        if (buf_size != sz)
@@ -3873,8 +3852,7 @@ ice_set_ctx(struct ice_hw *hw, u8 *src_ctx, u8 *dest_ctx,
                 * of the endianness of the machine.
                 */
                if (ce_info[f].width > (ce_info[f].size_of * BITS_PER_BYTE)) {
-                       ice_debug(hw, ICE_DBG_QCTX,
-                                 "Field %d width of %d bits larger than size of %d byte(s) ... skipping write\n",
+                       ice_debug(hw, ICE_DBG_QCTX, "Field %d width of %d bits larger than size of %d byte(s) ... skipping write\n",
                                  f, ce_info[f].width, ce_info[f].size_of);
                        continue;
                }
@@ -4263,24 +4241,32 @@ ice_dis_vsi_txq(struct ice_port_info *pi, u16 vsi_handle, u8 tc, u8 num_queues,
                struct ice_sq_cd *cd)
 {
        enum ice_status status = ICE_ERR_DOES_NOT_EXIST;
-       struct ice_aqc_dis_txq_item qg_list;
+       struct ice_aqc_dis_txq_item *qg_list;
        struct ice_q_ctx *q_ctx;
-       u16 i;
+       struct ice_hw *hw;
+       u16 i, buf_size;
 
        if (!pi || pi->port_state != ICE_SCHED_PORT_STATE_READY)
                return ICE_ERR_CFG;
 
+       hw = pi->hw;
+
        if (!num_queues) {
                /* if queue is disabled already yet the disable queue command
                 * has to be sent to complete the VF reset, then call
                 * ice_aq_dis_lan_txq without any queue information
                 */
                if (rst_src)
-                       return ice_aq_dis_lan_txq(pi->hw, 0, NULL, 0, rst_src,
+                       return ice_aq_dis_lan_txq(hw, 0, NULL, 0, rst_src,
                                                  vmvf_num, NULL);
                return ICE_ERR_CFG;
        }
 
+       buf_size = ice_struct_size(qg_list, q_id, 1);
+       qg_list = (struct ice_aqc_dis_txq_item *)ice_malloc(hw, buf_size);
+       if (!qg_list)
+               return ICE_ERR_NO_MEMORY;
+
        ice_acquire_lock(&pi->sched_lock);
 
        for (i = 0; i < num_queues; i++) {
@@ -4289,23 +4275,22 @@ ice_dis_vsi_txq(struct ice_port_info *pi, u16 vsi_handle, u8 tc, u8 num_queues,
                node = ice_sched_find_node_by_teid(pi->root, q_teids[i]);
                if (!node)
                        continue;
-               q_ctx = ice_get_lan_q_ctx(pi->hw, vsi_handle, tc, q_handles[i]);
+               q_ctx = ice_get_lan_q_ctx(hw, vsi_handle, tc, q_handles[i]);
                if (!q_ctx) {
-                       ice_debug(pi->hw, ICE_DBG_SCHED, "invalid queue handle%d\n",
+                       ice_debug(hw, ICE_DBG_SCHED, "invalid queue handle%d\n",
                                  q_handles[i]);
                        continue;
                }
                if (q_ctx->q_handle != q_handles[i]) {
-                       ice_debug(pi->hw, ICE_DBG_SCHED, "Err:handles %d %d\n",
+                       ice_debug(hw, ICE_DBG_SCHED, "Err:handles %d %d\n",
                                  q_ctx->q_handle, q_handles[i]);
                        continue;
                }
-               qg_list.parent_teid = node->info.parent_teid;
-               qg_list.num_qs = 1;
-               qg_list.q_id[0] = CPU_TO_LE16(q_ids[i]);
-               status = ice_aq_dis_lan_txq(pi->hw, 1, &qg_list,
-                                           sizeof(qg_list), rst_src, vmvf_num,
-                                           cd);
+               qg_list->parent_teid = node->info.parent_teid;
+               qg_list->num_qs = 1;
+               qg_list->q_id[0] = CPU_TO_LE16(q_ids[i]);
+               status = ice_aq_dis_lan_txq(hw, 1, qg_list, buf_size, rst_src,
+                                           vmvf_num, cd);
 
                if (status != ICE_SUCCESS)
                        break;
@@ -4313,6 +4298,7 @@ ice_dis_vsi_txq(struct ice_port_info *pi, u16 vsi_handle, u8 tc, u8 num_queues,
                q_ctx->q_handle = ICE_INVAL_Q_HANDLE;
        }
        ice_release_lock(&pi->sched_lock);
+       ice_free(hw, qg_list);
        return status;
 }
 
@@ -4396,6 +4382,7 @@ static bool ice_is_main_vsi(struct ice_hw *hw, u16 vsi_handle)
 static enum ice_status
 ice_replay_pre_init(struct ice_hw *hw, struct ice_switch_info *sw)
 {
+       enum ice_status status;
        u8 i;
 
        /* Delete old entries from replay filter list head if there is any */
@@ -4409,6 +4396,10 @@ ice_replay_pre_init(struct ice_hw *hw, struct ice_switch_info *sw)
                                  &sw->recp_list[i].filt_replay_rules);
        ice_sched_replay_agg_vsi_preinit(hw);
 
+       status = ice_sched_replay_root_node_bw(hw->port_info);
+       if (status)
+               return status;
+
        return ice_sched_replay_tc_node_bw(hw->port_info);
 }
 
@@ -4596,14 +4587,14 @@ ice_stat_update_repc(struct ice_hw *hw, u16 vsi_handle, bool prev_stat_loaded,
  */
 enum ice_status
 ice_sched_query_elem(struct ice_hw *hw, u32 node_teid,
-                    struct ice_aqc_get_elem *buf)
+                    struct ice_aqc_txsched_elem_data *buf)
 {
        u16 buf_size, num_elem_ret = 0;
        enum ice_status status;
 
        buf_size = sizeof(*buf);
        ice_memset(buf, 0, buf_size, ICE_NONDMA_MEM);
-       buf->generic[0].node_teid = CPU_TO_LE32(node_teid);
+       buf->node_teid = CPU_TO_LE32(node_teid);
        status = ice_aq_query_sched_elems(hw, 1, buf, buf_size, &num_elem_ret,
                                          NULL);
        if (status != ICE_SUCCESS || num_elem_ret != 1)
@@ -4678,8 +4669,7 @@ ice_get_link_default_override(struct ice_link_default_override_tlv *ldo,
        status = ice_get_pfa_module_tlv(hw, &tlv, &tlv_len,
                                        ICE_SR_LINK_DEFAULT_OVERRIDE_PTR);
        if (status) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "Failed to read link override TLV.\n");
+               ice_debug(hw, ICE_DBG_INIT, "Failed to read link override TLV.\n");
                return status;
        }
 
@@ -4690,8 +4680,7 @@ ice_get_link_default_override(struct ice_link_default_override_tlv *ldo,
        /* link options first */
        status = ice_read_sr_word(hw, tlv_start, &buf);
        if (status) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "Failed to read override link options.\n");
+               ice_debug(hw, ICE_DBG_INIT, "Failed to read override link options.\n");
                return status;
        }
        ldo->options = buf & ICE_LINK_OVERRIDE_OPT_M;
@@ -4702,8 +4691,7 @@ ice_get_link_default_override(struct ice_link_default_override_tlv *ldo,
        offset = tlv_start + ICE_SR_PFA_LINK_OVERRIDE_FEC_OFFSET;
        status = ice_read_sr_word(hw, offset, &buf);
        if (status) {
-               ice_debug(hw, ICE_DBG_INIT,
-                         "Failed to read override phy config.\n");
+               ice_debug(hw, ICE_DBG_INIT, "Failed to read override phy config.\n");
                return status;
        }
        ldo->fec_options = buf & ICE_LINK_OVERRIDE_FEC_OPT_M;
@@ -4713,8 +4701,7 @@ ice_get_link_default_override(struct ice_link_default_override_tlv *ldo,
        for (i = 0; i < ICE_SR_PFA_LINK_OVERRIDE_PHY_WORDS; i++) {
                status = ice_read_sr_word(hw, (offset + i), &buf);
                if (status) {
-                       ice_debug(hw, ICE_DBG_INIT,
-                                 "Failed to read override link options.\n");
+                       ice_debug(hw, ICE_DBG_INIT, "Failed to read override link options.\n");
                        return status;
                }
                /* shift 16 bits at a time to fill 64 bits */
@@ -4727,8 +4714,7 @@ ice_get_link_default_override(struct ice_link_default_override_tlv *ldo,
        for (i = 0; i < ICE_SR_PFA_LINK_OVERRIDE_PHY_WORDS; i++) {
                status = ice_read_sr_word(hw, (offset + i), &buf);
                if (status) {
-                       ice_debug(hw, ICE_DBG_INIT,
-                                 "Failed to read override link options.\n");
+                       ice_debug(hw, ICE_DBG_INIT, "Failed to read override link options.\n");
                        return status;
                }
                /* shift 16 bits at a time to fill 64 bits */
@@ -4785,3 +4771,50 @@ ice_aq_set_lldp_mib(struct ice_hw *hw, u8 mib_type, void *buf, u16 buf_size,
 
        return ice_aq_send_cmd(hw, &desc, buf, buf_size, cd);
 }
+
+/**
+ * ice_fw_supports_lldp_fltr - check NVM version supports lldp_fltr_ctrl
+ * @hw: pointer to HW struct
+ */
+bool ice_fw_supports_lldp_fltr_ctrl(struct ice_hw *hw)
+{
+       if (hw->mac_type != ICE_MAC_E810)
+               return false;
+
+       if (hw->api_maj_ver == ICE_FW_API_LLDP_FLTR_MAJ) {
+               if (hw->api_min_ver > ICE_FW_API_LLDP_FLTR_MIN)
+                       return true;
+               if (hw->api_min_ver == ICE_FW_API_LLDP_FLTR_MIN &&
+                   hw->api_patch >= ICE_FW_API_LLDP_FLTR_PATCH)
+                       return true;
+       } else if (hw->api_maj_ver > ICE_FW_API_LLDP_FLTR_MAJ) {
+               return true;
+       }
+       return false;
+}
+
+/**
+ * ice_lldp_fltr_add_remove - add or remove a LLDP Rx switch filter
+ * @hw: pointer to HW struct
+ * @vsi_num: absolute HW index for VSI
+ * @add: boolean for if adding or removing a filter
+ */
+enum ice_status
+ice_lldp_fltr_add_remove(struct ice_hw *hw, u16 vsi_num, bool add)
+{
+       struct ice_aqc_lldp_filter_ctrl *cmd;
+       struct ice_aq_desc desc;
+
+       cmd = &desc.params.lldp_filter_ctrl;
+
+       ice_fill_dflt_direct_cmd_desc(&desc, ice_aqc_opc_lldp_filter_ctrl);
+
+       if (add)
+               cmd->cmd_flags = ICE_AQC_LLDP_FILTER_ACTION_ADD;
+       else
+               cmd->cmd_flags = ICE_AQC_LLDP_FILTER_ACTION_DELETE;
+
+       cmd->vsi_num = CPU_TO_LE16(vsi_num);
+
+       return ice_aq_send_cmd(hw, &desc, NULL, 0, NULL);
+}