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