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