drivers/net/ethernet/chelsio/cxgb4/cxgb4_tc_mqprio.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Copyright (C) 2019 Chelsio Communications.  All rights reserved. */
   3
   4 #include "cxgb4.h"
   5 #include "cxgb4_tc_mqprio.h"
   6 #include "sched.h"
   7
   8 static int cxgb4_mqprio_validate(struct net_device *dev,
   9                                  struct tc_mqprio_qopt_offload *mqprio)
  10 {
  11         u64 min_rate = 0, max_rate = 0, max_link_rate;
  12         struct port_info *pi = netdev2pinfo(dev);
  13         struct adapter *adap = netdev2adap(dev);
  14         u32 speed, qcount = 0, qoffset = 0;
  15         int ret;
  16         u8 i;
  17
  18         if (!mqprio->qopt.num_tc)
  19                 return 0;
  20
  21         if (mqprio->qopt.hw != TC_MQPRIO_HW_OFFLOAD_TCS) {
  22                 netdev_err(dev, "Only full TC hardware offload is supported\n");
  23                 return -EINVAL;
  24         } else if (mqprio->mode != TC_MQPRIO_MODE_CHANNEL) {
  25                 netdev_err(dev, "Only channel mode offload is supported\n");
  26                 return -EINVAL;
  27         } else if (mqprio->shaper != TC_MQPRIO_SHAPER_BW_RATE) {
  28                 netdev_err(dev, "Only bandwidth rate shaper supported\n");
  29                 return -EINVAL;
  30         } else if (mqprio->qopt.num_tc > adap->params.nsched_cls) {
  31                 netdev_err(dev,
  32                            "Only %u traffic classes supported by hardware\n",
  33                            adap->params.nsched_cls);
  34                 return -ERANGE;
  35         }
  36
  37         ret = t4_get_link_params(pi, NULL, &speed, NULL);
  38         if (ret) {
  39                 netdev_err(dev, "Failed to get link speed, ret: %d\n", ret);
  40                 return -EINVAL;
  41         }
  42
  43         /* Convert from Mbps to bps */
  44         max_link_rate = (u64)speed * 1000 * 1000;
  45
  46         for (i = 0; i < mqprio->qopt.num_tc; i++) {
  47                 qoffset = max_t(u16, mqprio->qopt.offset[i], qoffset);
  48                 qcount += mqprio->qopt.count[i];
  49
  50                 /* Convert byte per second to bits per second */
  51                 min_rate += (mqprio->min_rate[i] * 8);
  52                 max_rate += (mqprio->max_rate[i] * 8);
  53         }
  54
  55         if (qoffset >= adap->tids.neotids || qcount > adap->tids.neotids)
  56                 return -ENOMEM;
  57
  58         if (min_rate > max_link_rate || max_rate > max_link_rate) {
  59                 netdev_err(dev,
  60                            "Total Min/Max (%llu/%llu) Rate > supported (%llu)\n",
  61                            min_rate, max_rate, max_link_rate);
  62                 return -EINVAL;
  63         }
  64
  65         return 0;
  66 }
  67
  68 static int cxgb4_init_eosw_txq(struct net_device *dev,
  69                                struct sge_eosw_txq *eosw_txq,
  70                                u32 eotid, u32 hwqid)
  71 {
  72         struct adapter *adap = netdev2adap(dev);
  73         struct tx_sw_desc *ring;
  74
  75         memset(eosw_txq, 0, sizeof(*eosw_txq));
  76
  77         ring = kcalloc(CXGB4_EOSW_TXQ_DEFAULT_DESC_NUM,
  78                        sizeof(*ring), GFP_KERNEL);
  79         if (!ring)
  80                 return -ENOMEM;
  81
  82         eosw_txq->desc = ring;
  83         eosw_txq->ndesc = CXGB4_EOSW_TXQ_DEFAULT_DESC_NUM;
  84         spin_lock_init(&eosw_txq->lock);
  85         eosw_txq->state = CXGB4_EO_STATE_CLOSED;
  86         eosw_txq->eotid = eotid;
  87         eosw_txq->hwtid = adap->tids.eotid_base + eosw_txq->eotid;
  88         eosw_txq->cred = adap->params.ofldq_wr_cred;
  89         eosw_txq->hwqid = hwqid;
  90         eosw_txq->netdev = dev;
  91         tasklet_init(&eosw_txq->qresume_tsk, cxgb4_ethofld_restart,
  92                      (unsigned long)eosw_txq);
  93         return 0;
  94 }
  95
  96 static void cxgb4_clean_eosw_txq(struct net_device *dev,
  97                                  struct sge_eosw_txq *eosw_txq)
  98 {
  99         struct adapter *adap = netdev2adap(dev);
 100
 101         cxgb4_eosw_txq_free_desc(adap, eosw_txq, eosw_txq->ndesc);
 102         eosw_txq->pidx = 0;
 103         eosw_txq->last_pidx = 0;
 104         eosw_txq->cidx = 0;
 105         eosw_txq->last_cidx = 0;
 106         eosw_txq->flowc_idx = 0;
 107         eosw_txq->inuse = 0;
 108         eosw_txq->cred = adap->params.ofldq_wr_cred;
 109         eosw_txq->ncompl = 0;
 110         eosw_txq->last_compl = 0;
 111         eosw_txq->state = CXGB4_EO_STATE_CLOSED;
 112 }
 113
 114 static void cxgb4_free_eosw_txq(struct net_device *dev,
 115                                 struct sge_eosw_txq *eosw_txq)
 116 {
 117         spin_lock_bh(&eosw_txq->lock);
 118         cxgb4_clean_eosw_txq(dev, eosw_txq);
 119         kfree(eosw_txq->desc);
 120         spin_unlock_bh(&eosw_txq->lock);
 121         tasklet_kill(&eosw_txq->qresume_tsk);
 122 }
 123
 124 static int cxgb4_mqprio_alloc_hw_resources(struct net_device *dev)
 125 {
 126         struct port_info *pi = netdev2pinfo(dev);
 127         struct adapter *adap = netdev2adap(dev);
 128         struct sge_ofld_rxq *eorxq;
 129         struct sge_eohw_txq *eotxq;
 130         int ret, msix = 0;
 131         u32 i;
 132
 133         /* Allocate ETHOFLD hardware queue structures if not done already */
 134         if (!refcount_read(&adap->tc_mqprio->refcnt)) {
 135                 adap->sge.eohw_rxq = kcalloc(adap->sge.eoqsets,
 136                                              sizeof(struct sge_ofld_rxq),
 137                                              GFP_KERNEL);
 138                 if (!adap->sge.eohw_rxq)
 139                         return -ENOMEM;
 140
 141                 adap->sge.eohw_txq = kcalloc(adap->sge.eoqsets,
 142                                              sizeof(struct sge_eohw_txq),
 143                                              GFP_KERNEL);
 144                 if (!adap->sge.eohw_txq) {
 145                         kfree(adap->sge.eohw_rxq);
 146                         return -ENOMEM;
 147                 }
 148         }
 149
 150         if (!(adap->flags & CXGB4_USING_MSIX))
 151                 msix = -((int)adap->sge.intrq.abs_id + 1);
 152
 153         for (i = 0; i < pi->nqsets; i++) {
 154                 eorxq = &adap->sge.eohw_rxq[pi->first_qset + i];
 155                 eotxq = &adap->sge.eohw_txq[pi->first_qset + i];
 156
 157                 /* Allocate Rxqs for receiving ETHOFLD Tx completions */
 158                 if (msix >= 0) {
 159                         msix = cxgb4_get_msix_idx_from_bmap(adap);
 160                         if (msix < 0) {
 161                                 ret = msix;
 162                                 goto out_free_queues;
 163                         }
 164
 165                         eorxq->msix = &adap->msix_info[msix];
 166                         snprintf(eorxq->msix->desc,
 167                                  sizeof(eorxq->msix->desc),
 168                                  "%s-eorxq%d", dev->name, i);
 169                 }
 170
 171                 init_rspq(adap, &eorxq->rspq,
 172                           CXGB4_EOHW_RXQ_DEFAULT_INTR_USEC,
 173                           CXGB4_EOHW_RXQ_DEFAULT_PKT_CNT,
 174                           CXGB4_EOHW_RXQ_DEFAULT_DESC_NUM,
 175                           CXGB4_EOHW_RXQ_DEFAULT_DESC_SIZE);
 176
 177                 eorxq->fl.size = CXGB4_EOHW_FLQ_DEFAULT_DESC_NUM;
 178
 179                 ret = t4_sge_alloc_rxq(adap, &eorxq->rspq, false,
 180                                        dev, msix, &eorxq->fl,
 181                                        cxgb4_ethofld_rx_handler,
 182                                        NULL, 0);
 183                 if (ret)
 184                         goto out_free_queues;
 185
 186                 /* Allocate ETHOFLD hardware Txqs */
 187                 eotxq->q.size = CXGB4_EOHW_TXQ_DEFAULT_DESC_NUM;
 188                 ret = t4_sge_alloc_ethofld_txq(adap, eotxq, dev,
 189                                                eorxq->rspq.cntxt_id);
 190                 if (ret)
 191                         goto out_free_queues;
 192
 193                 /* Allocate IRQs, set IRQ affinity, and start Rx */
 194                 if (adap->flags & CXGB4_USING_MSIX) {
 195                         ret = request_irq(eorxq->msix->vec, t4_sge_intr_msix, 0,
 196                                           eorxq->msix->desc, &eorxq->rspq);
 197                         if (ret)
 198                                 goto out_free_msix;
 199
 200                         cxgb4_set_msix_aff(adap, eorxq->msix->vec,
 201                                            &eorxq->msix->aff_mask, i);
 202                 }
 203
 204                 if (adap->flags & CXGB4_FULL_INIT_DONE)
 205                         cxgb4_enable_rx(adap, &eorxq->rspq);
 206         }
 207
 208         refcount_inc(&adap->tc_mqprio->refcnt);
 209         return 0;
 210
 211 out_free_msix:
 212         while (i-- > 0) {
 213                 eorxq = &adap->sge.eohw_rxq[pi->first_qset + i];
 214
 215                 if (adap->flags & CXGB4_FULL_INIT_DONE)
 216                         cxgb4_quiesce_rx(&eorxq->rspq);
 217
 218                 if (adap->flags & CXGB4_USING_MSIX) {
 219                         cxgb4_clear_msix_aff(eorxq->msix->vec,
 220                                              eorxq->msix->aff_mask);
 221                         free_irq(eorxq->msix->vec, &eorxq->rspq);
 222                 }
 223         }
 224
 225 out_free_queues:
 226         for (i = 0; i < pi->nqsets; i++) {
 227                 eorxq = &adap->sge.eohw_rxq[pi->first_qset + i];
 228                 eotxq = &adap->sge.eohw_txq[pi->first_qset + i];
 229
 230                 if (eorxq->rspq.desc)
 231                         free_rspq_fl(adap, &eorxq->rspq, &eorxq->fl);
 232                 if (eorxq->msix)
 233                         cxgb4_free_msix_idx_in_bmap(adap, eorxq->msix->idx);
 234                 t4_sge_free_ethofld_txq(adap, eotxq);
 235         }
 236
 237         kfree(adap->sge.eohw_txq);
 238         kfree(adap->sge.eohw_rxq);
 239
 240         return ret;
 241 }
 242
 243 static void cxgb4_mqprio_free_hw_resources(struct net_device *dev)
 244 {
 245         struct port_info *pi = netdev2pinfo(dev);
 246         struct adapter *adap = netdev2adap(dev);
 247         struct sge_ofld_rxq *eorxq;
 248         struct sge_eohw_txq *eotxq;
 249         u32 i;
 250
 251         /* Return if no ETHOFLD structures have been allocated yet */
 252         if (!refcount_read(&adap->tc_mqprio->refcnt))
 253                 return;
 254
 255         /* Return if no hardware queues have been allocated */
 256         if (!adap->sge.eohw_rxq[pi->first_qset].rspq.desc)
 257                 return;
 258
 259         for (i = 0; i < pi->nqsets; i++) {
 260                 eorxq = &adap->sge.eohw_rxq[pi->first_qset + i];
 261                 eotxq = &adap->sge.eohw_txq[pi->first_qset + i];
 262
 263                 /* Device removal path will already disable NAPI
 264                  * before unregistering netdevice. So, only disable
 265                  * NAPI if we're not in device removal path
 266                  */
 267                 if (!(adap->flags & CXGB4_SHUTTING_DOWN))
 268                         cxgb4_quiesce_rx(&eorxq->rspq);
 269
 270                 if (adap->flags & CXGB4_USING_MSIX) {
 271                         cxgb4_clear_msix_aff(eorxq->msix->vec,
 272                                              eorxq->msix->aff_mask);
 273                         free_irq(eorxq->msix->vec, &eorxq->rspq);
 274                 }
 275
 276                 free_rspq_fl(adap, &eorxq->rspq, &eorxq->fl);
 277                 t4_sge_free_ethofld_txq(adap, eotxq);
 278         }
 279
 280         /* Free up ETHOFLD structures if there are no users */
 281         if (refcount_dec_and_test(&adap->tc_mqprio->refcnt)) {
 282                 kfree(adap->sge.eohw_txq);
 283                 kfree(adap->sge.eohw_rxq);
 284         }
 285 }
 286
 287 static int cxgb4_mqprio_alloc_tc(struct net_device *dev,
 288                                  struct tc_mqprio_qopt_offload *mqprio)
 289 {
 290         struct ch_sched_params p = {
 291                 .type = SCHED_CLASS_TYPE_PACKET,
 292                 .u.params.level = SCHED_CLASS_LEVEL_CL_RL,
 293                 .u.params.mode = SCHED_CLASS_MODE_FLOW,
 294                 .u.params.rateunit = SCHED_CLASS_RATEUNIT_BITS,
 295                 .u.params.ratemode = SCHED_CLASS_RATEMODE_ABS,
 296                 .u.params.class = SCHED_CLS_NONE,
 297                 .u.params.weight = 0,
 298                 .u.params.pktsize = dev->mtu,
 299         };
 300         struct cxgb4_tc_port_mqprio *tc_port_mqprio;
 301         struct port_info *pi = netdev2pinfo(dev);
 302         struct adapter *adap = netdev2adap(dev);
 303         struct sched_class *e;
 304         int ret;
 305         u8 i;
 306
 307         tc_port_mqprio = &adap->tc_mqprio->port_mqprio[pi->port_id];
 308         p.u.params.channel = pi->tx_chan;
 309         for (i = 0; i < mqprio->qopt.num_tc; i++) {
 310                 /* Convert from bytes per second to Kbps */
 311                 p.u.params.minrate = div_u64(mqprio->min_rate[i] * 8, 1000);
 312                 p.u.params.maxrate = div_u64(mqprio->max_rate[i] * 8, 1000);
 313
 314                 e = cxgb4_sched_class_alloc(dev, &p);
 315                 if (!e) {
 316                         ret = -ENOMEM;
 317                         goto out_err;
 318                 }
 319
 320                 tc_port_mqprio->tc_hwtc_map[i] = e->idx;
 321         }
 322
 323         return 0;
 324
 325 out_err:
 326         while (i--)
 327                 cxgb4_sched_class_free(dev, tc_port_mqprio->tc_hwtc_map[i]);
 328
 329         return ret;
 330 }
 331
 332 static void cxgb4_mqprio_free_tc(struct net_device *dev)
 333 {
 334         struct cxgb4_tc_port_mqprio *tc_port_mqprio;
 335         struct port_info *pi = netdev2pinfo(dev);
 336         struct adapter *adap = netdev2adap(dev);
 337         u8 i;
 338
 339         tc_port_mqprio = &adap->tc_mqprio->port_mqprio[pi->port_id];
 340         for (i = 0; i < tc_port_mqprio->mqprio.qopt.num_tc; i++)
 341                 cxgb4_sched_class_free(dev, tc_port_mqprio->tc_hwtc_map[i]);
 342 }
 343
 344 static int cxgb4_mqprio_class_bind(struct net_device *dev,
 345                                    struct sge_eosw_txq *eosw_txq,
 346                                    u8 tc)
 347 {
 348         struct ch_sched_flowc fe;
 349         int ret;
 350
 351         init_completion(&eosw_txq->completion);
 352
 353         fe.tid = eosw_txq->eotid;
 354         fe.class = tc;
 355
 356         ret = cxgb4_sched_class_bind(dev, &fe, SCHED_FLOWC);
 357         if (ret)
 358                 return ret;
 359
 360         ret = wait_for_completion_timeout(&eosw_txq->completion,
 361                                           CXGB4_FLOWC_WAIT_TIMEOUT);
 362         if (!ret)
 363                 return -ETIMEDOUT;
 364
 365         return 0;
 366 }
 367
 368 static void cxgb4_mqprio_class_unbind(struct net_device *dev,
 369                                       struct sge_eosw_txq *eosw_txq,
 370                                       u8 tc)
 371 {
 372         struct adapter *adap = netdev2adap(dev);
 373         struct ch_sched_flowc fe;
 374
 375         /* If we're shutting down, interrupts are disabled and no completions
 376          * come back. So, skip waiting for completions in this scenario.
 377          */
 378         if (!(adap->flags & CXGB4_SHUTTING_DOWN))
 379                 init_completion(&eosw_txq->completion);
 380
 381         fe.tid = eosw_txq->eotid;
 382         fe.class = tc;
 383         cxgb4_sched_class_unbind(dev, &fe, SCHED_FLOWC);
 384
 385         if (!(adap->flags & CXGB4_SHUTTING_DOWN))
 386                 wait_for_completion_timeout(&eosw_txq->completion,
 387                                             CXGB4_FLOWC_WAIT_TIMEOUT);
 388 }
 389
 390 static int cxgb4_mqprio_enable_offload(struct net_device *dev,
 391                                        struct tc_mqprio_qopt_offload *mqprio)
 392 {
 393         struct cxgb4_tc_port_mqprio *tc_port_mqprio;
 394         u32 qoffset, qcount, tot_qcount, qid, hwqid;
 395         struct port_info *pi = netdev2pinfo(dev);
 396         struct adapter *adap = netdev2adap(dev);
 397         struct sge_eosw_txq *eosw_txq;
 398         int eotid, ret;
 399         u16 i, j;
 400         u8 hwtc;
 401
 402         ret = cxgb4_mqprio_alloc_hw_resources(dev);
 403         if (ret)
 404                 return -ENOMEM;
 405
 406         tc_port_mqprio = &adap->tc_mqprio->port_mqprio[pi->port_id];
 407         for (i = 0; i < mqprio->qopt.num_tc; i++) {
 408                 qoffset = mqprio->qopt.offset[i];
 409                 qcount = mqprio->qopt.count[i];
 410                 for (j = 0; j < qcount; j++) {
 411                         eotid = cxgb4_get_free_eotid(&adap->tids);
 412                         if (eotid < 0) {
 413                                 ret = -ENOMEM;
 414                                 goto out_free_eotids;
 415                         }
 416
 417                         qid = qoffset + j;
 418                         hwqid = pi->first_qset + (eotid % pi->nqsets);
 419                         eosw_txq = &tc_port_mqprio->eosw_txq[qid];
 420                         ret = cxgb4_init_eosw_txq(dev, eosw_txq,
 421                                                   eotid, hwqid);
 422                         if (ret)
 423                                 goto out_free_eotids;
 424
 425                         cxgb4_alloc_eotid(&adap->tids, eotid, eosw_txq);
 426
 427                         hwtc = tc_port_mqprio->tc_hwtc_map[i];
 428                         ret = cxgb4_mqprio_class_bind(dev, eosw_txq, hwtc);
 429                         if (ret)
 430                                 goto out_free_eotids;
 431                 }
 432         }
 433
 434         memcpy(&tc_port_mqprio->mqprio, mqprio,
 435                sizeof(struct tc_mqprio_qopt_offload));
 436
 437         /* Inform the stack about the configured tc params.
 438          *
 439          * Set the correct queue map. If no queue count has been
 440          * specified, then send the traffic through default NIC
 441          * queues; instead of ETHOFLD queues.
 442          */
 443         ret = netdev_set_num_tc(dev, mqprio->qopt.num_tc);
 444         if (ret)
 445                 goto out_free_eotids;
 446
 447         tot_qcount = pi->nqsets;
 448         for (i = 0; i < mqprio->qopt.num_tc; i++) {
 449                 qcount = mqprio->qopt.count[i];
 450                 if (qcount) {
 451                         qoffset = mqprio->qopt.offset[i] + pi->nqsets;
 452                 } else {
 453                         qcount = pi->nqsets;
 454                         qoffset = 0;
 455                 }
 456
 457                 ret = netdev_set_tc_queue(dev, i, qcount, qoffset);
 458                 if (ret)
 459                         goto out_reset_tc;
 460
 461                 tot_qcount += mqprio->qopt.count[i];
 462         }
 463
 464         ret = netif_set_real_num_tx_queues(dev, tot_qcount);
 465         if (ret)
 466                 goto out_reset_tc;
 467
 468         tc_port_mqprio->state = CXGB4_MQPRIO_STATE_ACTIVE;
 469         return 0;
 470
 471 out_reset_tc:
 472         netdev_reset_tc(dev);
 473         i = mqprio->qopt.num_tc;
 474
 475 out_free_eotids:
 476         while (i-- > 0) {
 477                 qoffset = mqprio->qopt.offset[i];
 478                 qcount = mqprio->qopt.count[i];
 479                 for (j = 0; j < qcount; j++) {
 480                         eosw_txq = &tc_port_mqprio->eosw_txq[qoffset + j];
 481
 482                         hwtc = tc_port_mqprio->tc_hwtc_map[i];
 483                         cxgb4_mqprio_class_unbind(dev, eosw_txq, hwtc);
 484
 485                         cxgb4_free_eotid(&adap->tids, eosw_txq->eotid);
 486                         cxgb4_free_eosw_txq(dev, eosw_txq);
 487                 }
 488         }
 489
 490         cxgb4_mqprio_free_hw_resources(dev);
 491         return ret;
 492 }
 493
 494 static void cxgb4_mqprio_disable_offload(struct net_device *dev)
 495 {
 496         struct cxgb4_tc_port_mqprio *tc_port_mqprio;
 497         struct port_info *pi = netdev2pinfo(dev);
 498         struct adapter *adap = netdev2adap(dev);
 499         struct sge_eosw_txq *eosw_txq;
 500         u32 qoffset, qcount;
 501         u16 i, j;
 502         u8 hwtc;
 503
 504         tc_port_mqprio = &adap->tc_mqprio->port_mqprio[pi->port_id];
 505         if (tc_port_mqprio->state != CXGB4_MQPRIO_STATE_ACTIVE)
 506                 return;
 507
 508         netdev_reset_tc(dev);
 509         netif_set_real_num_tx_queues(dev, pi->nqsets);
 510
 511         for (i = 0; i < tc_port_mqprio->mqprio.qopt.num_tc; i++) {
 512                 qoffset = tc_port_mqprio->mqprio.qopt.offset[i];
 513                 qcount = tc_port_mqprio->mqprio.qopt.count[i];
 514                 for (j = 0; j < qcount; j++) {
 515                         eosw_txq = &tc_port_mqprio->eosw_txq[qoffset + j];
 516
 517                         hwtc = tc_port_mqprio->tc_hwtc_map[i];
 518                         cxgb4_mqprio_class_unbind(dev, eosw_txq, hwtc);
 519
 520                         cxgb4_free_eotid(&adap->tids, eosw_txq->eotid);
 521                         cxgb4_free_eosw_txq(dev, eosw_txq);
 522                 }
 523         }
 524
 525         cxgb4_mqprio_free_hw_resources(dev);
 526
 527         /* Free up the traffic classes */
 528         cxgb4_mqprio_free_tc(dev);
 529
 530         memset(&tc_port_mqprio->mqprio, 0,
 531                sizeof(struct tc_mqprio_qopt_offload));
 532
 533         tc_port_mqprio->state = CXGB4_MQPRIO_STATE_DISABLED;
 534 }
 535
 536 int cxgb4_setup_tc_mqprio(struct net_device *dev,
 537                           struct tc_mqprio_qopt_offload *mqprio)
 538 {
 539         bool needs_bring_up = false;
 540         int ret;
 541
 542         ret = cxgb4_mqprio_validate(dev, mqprio);
 543         if (ret)
 544                 return ret;
 545
 546         /* To configure tc params, the current allocated EOTIDs must
 547          * be freed up. However, they can't be freed up if there's
 548          * traffic running on the interface. So, ensure interface is
 549          * down before configuring tc params.
 550          */
 551         if (netif_running(dev)) {
 552                 cxgb_close(dev);
 553                 needs_bring_up = true;
 554         }
 555
 556         cxgb4_mqprio_disable_offload(dev);
 557
 558         /* If requested for clear, then just return since resources are
 559          * already freed up by now.
 560          */
 561         if (!mqprio->qopt.num_tc)
 562                 goto out;
 563
 564         /* Allocate free available traffic classes and configure
 565          * their rate parameters.
 566          */
 567         ret = cxgb4_mqprio_alloc_tc(dev, mqprio);
 568         if (ret)
 569                 goto out;
 570
 571         ret = cxgb4_mqprio_enable_offload(dev, mqprio);
 572         if (ret) {
 573                 cxgb4_mqprio_free_tc(dev);
 574                 goto out;
 575         }
 576
 577 out:
 578         if (needs_bring_up)
 579                 cxgb_open(dev);
 580
 581         return ret;
 582 }
 583
 584 int cxgb4_init_tc_mqprio(struct adapter *adap)
 585 {
 586         struct cxgb4_tc_port_mqprio *tc_port_mqprio, *port_mqprio;
 587         struct cxgb4_tc_mqprio *tc_mqprio;
 588         struct sge_eosw_txq *eosw_txq;
 589         int ret = 0;
 590         u8 i;
 591
 592         tc_mqprio = kzalloc(sizeof(*tc_mqprio), GFP_KERNEL);
 593         if (!tc_mqprio)
 594                 return -ENOMEM;
 595
 596         tc_port_mqprio = kcalloc(adap->params.nports, sizeof(*tc_port_mqprio),
 597                                  GFP_KERNEL);
 598         if (!tc_port_mqprio) {
 599                 ret = -ENOMEM;
 600                 goto out_free_mqprio;
 601         }
 602
 603         tc_mqprio->port_mqprio = tc_port_mqprio;
 604         for (i = 0; i < adap->params.nports; i++) {
 605                 port_mqprio = &tc_mqprio->port_mqprio[i];
 606                 eosw_txq = kcalloc(adap->tids.neotids, sizeof(*eosw_txq),
 607                                    GFP_KERNEL);
 608                 if (!eosw_txq) {
 609                         ret = -ENOMEM;
 610                         goto out_free_ports;
 611                 }
 612                 port_mqprio->eosw_txq = eosw_txq;
 613         }
 614
 615         adap->tc_mqprio = tc_mqprio;
 616         refcount_set(&adap->tc_mqprio->refcnt, 0);
 617         return 0;
 618
 619 out_free_ports:
 620         for (i = 0; i < adap->params.nports; i++) {
 621                 port_mqprio = &tc_mqprio->port_mqprio[i];
 622                 kfree(port_mqprio->eosw_txq);
 623         }
 624         kfree(tc_port_mqprio);
 625
 626 out_free_mqprio:
 627         kfree(tc_mqprio);
 628         return ret;
 629 }
 630
 631 void cxgb4_cleanup_tc_mqprio(struct adapter *adap)
 632 {
 633         struct cxgb4_tc_port_mqprio *port_mqprio;
 634         u8 i;
 635
 636         if (adap->tc_mqprio) {
 637                 if (adap->tc_mqprio->port_mqprio) {
 638                         for (i = 0; i < adap->params.nports; i++) {
 639                                 struct net_device *dev = adap->port[i];
 640
 641                                 if (dev)
 642                                         cxgb4_mqprio_disable_offload(dev);
 643                                 port_mqprio = &adap->tc_mqprio->port_mqprio[i];
 644                                 kfree(port_mqprio->eosw_txq);
 645                         }
 646                         kfree(adap->tc_mqprio->port_mqprio);
 647                 }
 648                 kfree(adap->tc_mqprio);
 649         }
 650 }