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