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