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