drivers/net/ethernet/mellanox/mlx5/core/en/ptp.c

   1 // SPDX-License-Identifier: GPL-2.0 OR Linux-OpenIB
   2 // Copyright (c) 2020 Mellanox Technologies
   3
   4 #include "en/ptp.h"
   5 #include "en/txrx.h"
   6 #include "en/params.h"
   7
   8 #define MLX5E_PTP_CHANNEL_IX 0
   9
  10 struct mlx5e_ptp_params {
  11         struct mlx5e_params params;
  12         struct mlx5e_sq_param txq_sq_param;
  13         struct mlx5e_rq_param rq_param;
  14 };
  15
  16 struct mlx5e_skb_cb_hwtstamp {
  17         ktime_t cqe_hwtstamp;
  18         ktime_t port_hwtstamp;
  19 };
  20
  21 void mlx5e_skb_cb_hwtstamp_init(struct sk_buff *skb)
  22 {
  23         memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
  24 }
  25
  26 static struct mlx5e_skb_cb_hwtstamp *mlx5e_skb_cb_get_hwts(struct sk_buff *skb)
  27 {
  28         BUILD_BUG_ON(sizeof(struct mlx5e_skb_cb_hwtstamp) > sizeof(skb->cb));
  29         return (struct mlx5e_skb_cb_hwtstamp *)skb->cb;
  30 }
  31
  32 static void mlx5e_skb_cb_hwtstamp_tx(struct sk_buff *skb,
  33                                      struct mlx5e_ptp_cq_stats *cq_stats)
  34 {
  35         struct skb_shared_hwtstamps hwts = {};
  36         ktime_t diff;
  37
  38         diff = abs(mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp -
  39                    mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp);
  40
  41         /* Maximal allowed diff is 1 / 128 second */
  42         if (diff > (NSEC_PER_SEC >> 7)) {
  43                 cq_stats->abort++;
  44                 cq_stats->abort_abs_diff_ns += diff;
  45                 return;
  46         }
  47
  48         hwts.hwtstamp = mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp;
  49         skb_tstamp_tx(skb, &hwts);
  50 }
  51
  52 void mlx5e_skb_cb_hwtstamp_handler(struct sk_buff *skb, int hwtstamp_type,
  53                                    ktime_t hwtstamp,
  54                                    struct mlx5e_ptp_cq_stats *cq_stats)
  55 {
  56         switch (hwtstamp_type) {
  57         case (MLX5E_SKB_CB_CQE_HWTSTAMP):
  58                 mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp = hwtstamp;
  59                 break;
  60         case (MLX5E_SKB_CB_PORT_HWTSTAMP):
  61                 mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp = hwtstamp;
  62                 break;
  63         }
  64
  65         /* If both CQEs arrive, check and report the port tstamp, and clear skb cb as
  66          * skb soon to be released.
  67          */
  68         if (!mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp ||
  69             !mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp)
  70                 return;
  71
  72         mlx5e_skb_cb_hwtstamp_tx(skb, cq_stats);
  73         memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
  74 }
  75
  76 static void mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq *ptpsq,
  77                                     struct mlx5_cqe64 *cqe,
  78                                     int budget)
  79 {
  80         struct sk_buff *skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
  81         struct mlx5e_txqsq *sq = &ptpsq->txqsq;
  82         ktime_t hwtstamp;
  83
  84         if (unlikely(MLX5E_RX_ERR_CQE(cqe))) {
  85                 ptpsq->cq_stats->err_cqe++;
  86                 goto out;
  87         }
  88
  89         hwtstamp = mlx5e_cqe_ts_to_ns(sq->ptp_cyc2time, sq->clock, get_cqe_ts(cqe));
  90         mlx5e_skb_cb_hwtstamp_handler(skb, MLX5E_SKB_CB_PORT_HWTSTAMP,
  91                                       hwtstamp, ptpsq->cq_stats);
  92         ptpsq->cq_stats->cqe++;
  93
  94 out:
  95         napi_consume_skb(skb, budget);
  96 }
  97
  98 static bool mlx5e_ptp_poll_ts_cq(struct mlx5e_cq *cq, int budget)
  99 {
 100         struct mlx5e_ptpsq *ptpsq = container_of(cq, struct mlx5e_ptpsq, ts_cq);
 101         struct mlx5_cqwq *cqwq = &cq->wq;
 102         struct mlx5_cqe64 *cqe;
 103         int work_done = 0;
 104
 105         if (unlikely(!test_bit(MLX5E_SQ_STATE_ENABLED, &ptpsq->txqsq.state)))
 106                 return false;
 107
 108         cqe = mlx5_cqwq_get_cqe(cqwq);
 109         if (!cqe)
 110                 return false;
 111
 112         do {
 113                 mlx5_cqwq_pop(cqwq);
 114
 115                 mlx5e_ptp_handle_ts_cqe(ptpsq, cqe, budget);
 116         } while ((++work_done < budget) && (cqe = mlx5_cqwq_get_cqe(cqwq)));
 117
 118         mlx5_cqwq_update_db_record(cqwq);
 119
 120         /* ensure cq space is freed before enabling more cqes */
 121         wmb();
 122
 123         return work_done == budget;
 124 }
 125
 126 static int mlx5e_ptp_napi_poll(struct napi_struct *napi, int budget)
 127 {
 128         struct mlx5e_ptp *c = container_of(napi, struct mlx5e_ptp, napi);
 129         struct mlx5e_ch_stats *ch_stats = c->stats;
 130         struct mlx5e_rq *rq = &c->rq;
 131         bool busy = false;
 132         int work_done = 0;
 133         int i;
 134
 135         rcu_read_lock();
 136
 137         ch_stats->poll++;
 138
 139         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 140                 for (i = 0; i < c->num_tc; i++) {
 141                         busy |= mlx5e_poll_tx_cq(&c->ptpsq[i].txqsq.cq, budget);
 142                         busy |= mlx5e_ptp_poll_ts_cq(&c->ptpsq[i].ts_cq, budget);
 143                 }
 144         }
 145         if (test_bit(MLX5E_PTP_STATE_RX, c->state) && likely(budget)) {
 146                 work_done = mlx5e_poll_rx_cq(&rq->cq, budget);
 147                 busy |= work_done == budget;
 148                 busy |= INDIRECT_CALL_2(rq->post_wqes,
 149                                         mlx5e_post_rx_mpwqes,
 150                                         mlx5e_post_rx_wqes,
 151                                         rq);
 152         }
 153
 154         if (busy) {
 155                 work_done = budget;
 156                 goto out;
 157         }
 158
 159         if (unlikely(!napi_complete_done(napi, work_done)))
 160                 goto out;
 161
 162         ch_stats->arm++;
 163
 164         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 165                 for (i = 0; i < c->num_tc; i++) {
 166                         mlx5e_cq_arm(&c->ptpsq[i].txqsq.cq);
 167                         mlx5e_cq_arm(&c->ptpsq[i].ts_cq);
 168                 }
 169         }
 170         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 171                 mlx5e_cq_arm(&rq->cq);
 172
 173 out:
 174         rcu_read_unlock();
 175
 176         return work_done;
 177 }
 178
 179 static int mlx5e_ptp_alloc_txqsq(struct mlx5e_ptp *c, int txq_ix,
 180                                  struct mlx5e_params *params,
 181                                  struct mlx5e_sq_param *param,
 182                                  struct mlx5e_txqsq *sq, int tc,
 183                                  struct mlx5e_ptpsq *ptpsq)
 184 {
 185         void *sqc_wq               = MLX5_ADDR_OF(sqc, param->sqc, wq);
 186         struct mlx5_core_dev *mdev = c->mdev;
 187         struct mlx5_wq_cyc *wq = &sq->wq;
 188         int err;
 189         int node;
 190
 191         sq->pdev      = c->pdev;
 192         sq->tstamp    = c->tstamp;
 193         sq->clock     = &mdev->clock;
 194         sq->mkey_be   = c->mkey_be;
 195         sq->netdev    = c->netdev;
 196         sq->priv      = c->priv;
 197         sq->mdev      = mdev;
 198         sq->ch_ix     = MLX5E_PTP_CHANNEL_IX;
 199         sq->txq_ix    = txq_ix;
 200         sq->uar_map   = mdev->mlx5e_res.hw_objs.bfreg.map;
 201         sq->min_inline_mode = params->tx_min_inline_mode;
 202         sq->hw_mtu    = MLX5E_SW2HW_MTU(params, params->sw_mtu);
 203         sq->stats     = &c->priv->ptp_stats.sq[tc];
 204         sq->ptpsq     = ptpsq;
 205         INIT_WORK(&sq->recover_work, mlx5e_tx_err_cqe_work);
 206         if (!MLX5_CAP_ETH(mdev, wqe_vlan_insert))
 207                 set_bit(MLX5E_SQ_STATE_VLAN_NEED_L2_INLINE, &sq->state);
 208         sq->stop_room = param->stop_room;
 209         sq->ptp_cyc2time = mlx5_sq_ts_translator(mdev);
 210
 211         node = dev_to_node(mlx5_core_dma_dev(mdev));
 212
 213         param->wq.db_numa_node = node;
 214         err = mlx5_wq_cyc_create(mdev, &param->wq, sqc_wq, wq, &sq->wq_ctrl);
 215         if (err)
 216                 return err;
 217         wq->db    = &wq->db[MLX5_SND_DBR];
 218
 219         err = mlx5e_alloc_txqsq_db(sq, node);
 220         if (err)
 221                 goto err_sq_wq_destroy;
 222
 223         return 0;
 224
 225 err_sq_wq_destroy:
 226         mlx5_wq_destroy(&sq->wq_ctrl);
 227
 228         return err;
 229 }
 230
 231 static void mlx5e_ptp_destroy_sq(struct mlx5_core_dev *mdev, u32 sqn)
 232 {
 233         mlx5_core_destroy_sq(mdev, sqn);
 234 }
 235
 236 static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa)
 237 {
 238         int wq_sz = mlx5_wq_cyc_get_size(&ptpsq->txqsq.wq);
 239
 240         ptpsq->skb_fifo.fifo = kvzalloc_node(array_size(wq_sz, sizeof(*ptpsq->skb_fifo.fifo)),
 241                                              GFP_KERNEL, numa);
 242         if (!ptpsq->skb_fifo.fifo)
 243                 return -ENOMEM;
 244
 245         ptpsq->skb_fifo.pc   = &ptpsq->skb_fifo_pc;
 246         ptpsq->skb_fifo.cc   = &ptpsq->skb_fifo_cc;
 247         ptpsq->skb_fifo.mask = wq_sz - 1;
 248
 249         return 0;
 250 }
 251
 252 static void mlx5e_ptp_drain_skb_fifo(struct mlx5e_skb_fifo *skb_fifo)
 253 {
 254         while (*skb_fifo->pc != *skb_fifo->cc) {
 255                 struct sk_buff *skb = mlx5e_skb_fifo_pop(skb_fifo);
 256
 257                 dev_kfree_skb_any(skb);
 258         }
 259 }
 260
 261 static void mlx5e_ptp_free_traffic_db(struct mlx5e_skb_fifo *skb_fifo)
 262 {
 263         mlx5e_ptp_drain_skb_fifo(skb_fifo);
 264         kvfree(skb_fifo->fifo);
 265 }
 266
 267 static int mlx5e_ptp_open_txqsq(struct mlx5e_ptp *c, u32 tisn,
 268                                 int txq_ix, struct mlx5e_ptp_params *cparams,
 269                                 int tc, struct mlx5e_ptpsq *ptpsq)
 270 {
 271         struct mlx5e_sq_param *sqp = &cparams->txq_sq_param;
 272         struct mlx5e_txqsq *txqsq = &ptpsq->txqsq;
 273         struct mlx5e_create_sq_param csp = {};
 274         int err;
 275
 276         err = mlx5e_ptp_alloc_txqsq(c, txq_ix, &cparams->params, sqp,
 277                                     txqsq, tc, ptpsq);
 278         if (err)
 279                 return err;
 280
 281         csp.tisn            = tisn;
 282         csp.tis_lst_sz      = 1;
 283         csp.cqn             = txqsq->cq.mcq.cqn;
 284         csp.wq_ctrl         = &txqsq->wq_ctrl;
 285         csp.min_inline_mode = txqsq->min_inline_mode;
 286         csp.ts_cqe_to_dest_cqn = ptpsq->ts_cq.mcq.cqn;
 287
 288         err = mlx5e_create_sq_rdy(c->mdev, sqp, &csp, 0, &txqsq->sqn);
 289         if (err)
 290                 goto err_free_txqsq;
 291
 292         err = mlx5e_ptp_alloc_traffic_db(ptpsq,
 293                                          dev_to_node(mlx5_core_dma_dev(c->mdev)));
 294         if (err)
 295                 goto err_free_txqsq;
 296
 297         return 0;
 298
 299 err_free_txqsq:
 300         mlx5e_free_txqsq(txqsq);
 301
 302         return err;
 303 }
 304
 305 static void mlx5e_ptp_close_txqsq(struct mlx5e_ptpsq *ptpsq)
 306 {
 307         struct mlx5e_txqsq *sq = &ptpsq->txqsq;
 308         struct mlx5_core_dev *mdev = sq->mdev;
 309
 310         mlx5e_ptp_free_traffic_db(&ptpsq->skb_fifo);
 311         cancel_work_sync(&sq->recover_work);
 312         mlx5e_ptp_destroy_sq(mdev, sq->sqn);
 313         mlx5e_free_txqsq_descs(sq);
 314         mlx5e_free_txqsq(sq);
 315 }
 316
 317 static int mlx5e_ptp_open_txqsqs(struct mlx5e_ptp *c,
 318                                  struct mlx5e_ptp_params *cparams)
 319 {
 320         struct mlx5e_params *params = &cparams->params;
 321         int ix_base;
 322         int err;
 323         int tc;
 324
 325         ix_base = params->num_tc * params->num_channels;
 326
 327         for (tc = 0; tc < params->num_tc; tc++) {
 328                 int txq_ix = ix_base + tc;
 329
 330                 err = mlx5e_ptp_open_txqsq(c, c->priv->tisn[c->lag_port][tc], txq_ix,
 331                                            cparams, tc, &c->ptpsq[tc]);
 332                 if (err)
 333                         goto close_txqsq;
 334         }
 335
 336         return 0;
 337
 338 close_txqsq:
 339         for (--tc; tc >= 0; tc--)
 340                 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
 341
 342         return err;
 343 }
 344
 345 static void mlx5e_ptp_close_txqsqs(struct mlx5e_ptp *c)
 346 {
 347         int tc;
 348
 349         for (tc = 0; tc < c->num_tc; tc++)
 350                 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
 351 }
 352
 353 static int mlx5e_ptp_open_tx_cqs(struct mlx5e_ptp *c,
 354                                  struct mlx5e_ptp_params *cparams)
 355 {
 356         struct mlx5e_params *params = &cparams->params;
 357         struct mlx5e_create_cq_param ccp = {};
 358         struct dim_cq_moder ptp_moder = {};
 359         struct mlx5e_cq_param *cq_param;
 360         int err;
 361         int tc;
 362
 363         ccp.node     = dev_to_node(mlx5_core_dma_dev(c->mdev));
 364         ccp.ch_stats = c->stats;
 365         ccp.napi     = &c->napi;
 366         ccp.ix       = MLX5E_PTP_CHANNEL_IX;
 367
 368         cq_param = &cparams->txq_sq_param.cqp;
 369
 370         for (tc = 0; tc < params->num_tc; tc++) {
 371                 struct mlx5e_cq *cq = &c->ptpsq[tc].txqsq.cq;
 372
 373                 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
 374                 if (err)
 375                         goto out_err_txqsq_cq;
 376         }
 377
 378         for (tc = 0; tc < params->num_tc; tc++) {
 379                 struct mlx5e_cq *cq = &c->ptpsq[tc].ts_cq;
 380                 struct mlx5e_ptpsq *ptpsq = &c->ptpsq[tc];
 381
 382                 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
 383                 if (err)
 384                         goto out_err_ts_cq;
 385
 386                 ptpsq->cq_stats = &c->priv->ptp_stats.cq[tc];
 387         }
 388
 389         return 0;
 390
 391 out_err_ts_cq:
 392         for (--tc; tc >= 0; tc--)
 393                 mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
 394         tc = params->num_tc;
 395 out_err_txqsq_cq:
 396         for (--tc; tc >= 0; tc--)
 397                 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
 398
 399         return err;
 400 }
 401
 402 static int mlx5e_ptp_open_rx_cq(struct mlx5e_ptp *c,
 403                                 struct mlx5e_ptp_params *cparams)
 404 {
 405         struct mlx5e_create_cq_param ccp = {};
 406         struct dim_cq_moder ptp_moder = {};
 407         struct mlx5e_cq_param *cq_param;
 408         struct mlx5e_cq *cq = &c->rq.cq;
 409
 410         ccp.node     = dev_to_node(mlx5_core_dma_dev(c->mdev));
 411         ccp.ch_stats = c->stats;
 412         ccp.napi     = &c->napi;
 413         ccp.ix       = MLX5E_PTP_CHANNEL_IX;
 414
 415         cq_param = &cparams->rq_param.cqp;
 416
 417         return mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
 418 }
 419
 420 static void mlx5e_ptp_close_tx_cqs(struct mlx5e_ptp *c)
 421 {
 422         int tc;
 423
 424         for (tc = 0; tc < c->num_tc; tc++)
 425                 mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
 426
 427         for (tc = 0; tc < c->num_tc; tc++)
 428                 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
 429 }
 430
 431 static void mlx5e_ptp_build_sq_param(struct mlx5_core_dev *mdev,
 432                                      struct mlx5e_params *params,
 433                                      struct mlx5e_sq_param *param)
 434 {
 435         void *sqc = param->sqc;
 436         void *wq;
 437
 438         mlx5e_build_sq_param_common(mdev, param);
 439
 440         wq = MLX5_ADDR_OF(sqc, sqc, wq);
 441         MLX5_SET(wq, wq, log_wq_sz, params->log_sq_size);
 442         param->stop_room = mlx5e_stop_room_for_wqe(MLX5_SEND_WQE_MAX_WQEBBS);
 443         mlx5e_build_tx_cq_param(mdev, params, &param->cqp);
 444 }
 445
 446 static void mlx5e_ptp_build_rq_param(struct mlx5_core_dev *mdev,
 447                                      struct net_device *netdev,
 448                                      u16 q_counter,
 449                                      struct mlx5e_ptp_params *ptp_params)
 450 {
 451         struct mlx5e_rq_param *rq_params = &ptp_params->rq_param;
 452         struct mlx5e_params *params = &ptp_params->params;
 453
 454         params->rq_wq_type = MLX5_WQ_TYPE_CYCLIC;
 455         mlx5e_init_rq_type_params(mdev, params);
 456         params->sw_mtu = netdev->max_mtu;
 457         mlx5e_build_rq_param(mdev, params, NULL, q_counter, rq_params);
 458 }
 459
 460 static void mlx5e_ptp_build_params(struct mlx5e_ptp *c,
 461                                    struct mlx5e_ptp_params *cparams,
 462                                    struct mlx5e_params *orig)
 463 {
 464         struct mlx5e_params *params = &cparams->params;
 465
 466         params->tx_min_inline_mode = orig->tx_min_inline_mode;
 467         params->num_channels = orig->num_channels;
 468         params->hard_mtu = orig->hard_mtu;
 469         params->sw_mtu = orig->sw_mtu;
 470         params->num_tc = orig->num_tc;
 471
 472         /* SQ */
 473         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 474                 params->log_sq_size = orig->log_sq_size;
 475                 mlx5e_ptp_build_sq_param(c->mdev, params, &cparams->txq_sq_param);
 476         }
 477         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 478                 mlx5e_ptp_build_rq_param(c->mdev, c->netdev, c->priv->q_counter, cparams);
 479 }
 480
 481 static int mlx5e_init_ptp_rq(struct mlx5e_ptp *c, struct mlx5e_params *params,
 482                              struct mlx5e_rq *rq)
 483 {
 484         struct mlx5_core_dev *mdev = c->mdev;
 485         struct mlx5e_priv *priv = c->priv;
 486         int err;
 487
 488         rq->wq_type      = params->rq_wq_type;
 489         rq->pdev         = mdev->device;
 490         rq->netdev       = priv->netdev;
 491         rq->priv         = priv;
 492         rq->clock        = &mdev->clock;
 493         rq->tstamp       = &priv->tstamp;
 494         rq->mdev         = mdev;
 495         rq->hw_mtu       = MLX5E_SW2HW_MTU(params, params->sw_mtu);
 496         rq->stats        = &c->priv->ptp_stats.rq;
 497         rq->ptp_cyc2time = mlx5_rq_ts_translator(mdev);
 498         err = mlx5e_rq_set_handlers(rq, params, false);
 499         if (err)
 500                 return err;
 501
 502         return xdp_rxq_info_reg(&rq->xdp_rxq, rq->netdev, rq->ix, 0);
 503 }
 504
 505 static int mlx5e_ptp_open_rq(struct mlx5e_ptp *c, struct mlx5e_params *params,
 506                              struct mlx5e_rq_param *rq_param)
 507 {
 508         int node = dev_to_node(c->mdev->device);
 509         int err;
 510
 511         err = mlx5e_init_ptp_rq(c, params, &c->rq);
 512         if (err)
 513                 return err;
 514
 515         return mlx5e_open_rq(params, rq_param, NULL, node, &c->rq);
 516 }
 517
 518 static int mlx5e_ptp_open_queues(struct mlx5e_ptp *c,
 519                                  struct mlx5e_ptp_params *cparams)
 520 {
 521         int err;
 522
 523         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 524                 err = mlx5e_ptp_open_tx_cqs(c, cparams);
 525                 if (err)
 526                         return err;
 527
 528                 err = mlx5e_ptp_open_txqsqs(c, cparams);
 529                 if (err)
 530                         goto close_tx_cqs;
 531         }
 532         if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
 533                 err = mlx5e_ptp_open_rx_cq(c, cparams);
 534                 if (err)
 535                         goto close_txqsq;
 536
 537                 err = mlx5e_ptp_open_rq(c, &cparams->params, &cparams->rq_param);
 538                 if (err)
 539                         goto close_rx_cq;
 540         }
 541         return 0;
 542
 543 close_rx_cq:
 544         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 545                 mlx5e_close_cq(&c->rq.cq);
 546 close_txqsq:
 547         if (test_bit(MLX5E_PTP_STATE_TX, c->state))
 548                 mlx5e_ptp_close_txqsqs(c);
 549 close_tx_cqs:
 550         if (test_bit(MLX5E_PTP_STATE_TX, c->state))
 551                 mlx5e_ptp_close_tx_cqs(c);
 552
 553         return err;
 554 }
 555
 556 static void mlx5e_ptp_close_queues(struct mlx5e_ptp *c)
 557 {
 558         if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
 559                 mlx5e_close_rq(&c->rq);
 560                 mlx5e_close_cq(&c->rq.cq);
 561         }
 562         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 563                 mlx5e_ptp_close_txqsqs(c);
 564                 mlx5e_ptp_close_tx_cqs(c);
 565         }
 566 }
 567
 568 static int mlx5e_ptp_set_state(struct mlx5e_ptp *c, struct mlx5e_params *params)
 569 {
 570         if (MLX5E_GET_PFLAG(params, MLX5E_PFLAG_TX_PORT_TS))
 571                 __set_bit(MLX5E_PTP_STATE_TX, c->state);
 572
 573         return bitmap_empty(c->state, MLX5E_PTP_STATE_NUM_STATES) ? -EINVAL : 0;
 574 }
 575
 576 int mlx5e_ptp_open(struct mlx5e_priv *priv, struct mlx5e_params *params,
 577                    u8 lag_port, struct mlx5e_ptp **cp)
 578 {
 579         struct net_device *netdev = priv->netdev;
 580         struct mlx5_core_dev *mdev = priv->mdev;
 581         struct mlx5e_ptp_params *cparams;
 582         struct mlx5e_ptp *c;
 583         int err;
 584
 585
 586         c = kvzalloc_node(sizeof(*c), GFP_KERNEL, dev_to_node(mlx5_core_dma_dev(mdev)));
 587         cparams = kvzalloc(sizeof(*cparams), GFP_KERNEL);
 588         if (!c || !cparams)
 589                 return -ENOMEM;
 590
 591         c->priv     = priv;
 592         c->mdev     = priv->mdev;
 593         c->tstamp   = &priv->tstamp;
 594         c->pdev     = mlx5_core_dma_dev(priv->mdev);
 595         c->netdev   = priv->netdev;
 596         c->mkey_be  = cpu_to_be32(priv->mdev->mlx5e_res.hw_objs.mkey.key);
 597         c->num_tc   = params->num_tc;
 598         c->stats    = &priv->ptp_stats.ch;
 599         c->lag_port = lag_port;
 600
 601         err = mlx5e_ptp_set_state(c, params);
 602         if (err)
 603                 goto err_free;
 604
 605         netif_napi_add(netdev, &c->napi, mlx5e_ptp_napi_poll, 64);
 606
 607         mlx5e_ptp_build_params(c, cparams, params);
 608
 609         err = mlx5e_ptp_open_queues(c, cparams);
 610         if (unlikely(err))
 611                 goto err_napi_del;
 612
 613         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 614                 priv->rx_ptp_opened = true;
 615
 616         *cp = c;
 617
 618         kvfree(cparams);
 619
 620         return 0;
 621
 622 err_napi_del:
 623         netif_napi_del(&c->napi);
 624 err_free:
 625         kvfree(cparams);
 626         kvfree(c);
 627         return err;
 628 }
 629
 630 void mlx5e_ptp_close(struct mlx5e_ptp *c)
 631 {
 632         mlx5e_ptp_close_queues(c);
 633         netif_napi_del(&c->napi);
 634
 635         kvfree(c);
 636 }
 637
 638 void mlx5e_ptp_activate_channel(struct mlx5e_ptp *c)
 639 {
 640         int tc;
 641
 642         napi_enable(&c->napi);
 643
 644         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 645                 for (tc = 0; tc < c->num_tc; tc++)
 646                         mlx5e_activate_txqsq(&c->ptpsq[tc].txqsq);
 647         }
 648         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 649                 mlx5e_activate_rq(&c->rq);
 650 }
 651
 652 void mlx5e_ptp_deactivate_channel(struct mlx5e_ptp *c)
 653 {
 654         int tc;
 655
 656         if (test_bit(MLX5E_PTP_STATE_RX, c->state))
 657                 mlx5e_deactivate_rq(&c->rq);
 658
 659         if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
 660                 for (tc = 0; tc < c->num_tc; tc++)
 661                         mlx5e_deactivate_txqsq(&c->ptpsq[tc].txqsq);
 662         }
 663
 664         napi_disable(&c->napi);
 665 }
 666
 667 int mlx5e_ptp_get_rqn(struct mlx5e_ptp *c, u32 *rqn)
 668 {
 669         if (!c || !test_bit(MLX5E_PTP_STATE_RX, c->state))
 670                 return -EINVAL;
 671
 672         *rqn = c->rq.rqn;
 673         return 0;
 674 }