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
   8 #define MLX5E_PTP_CHANNEL_IX 0
   9
  10 struct mlx5e_ptp_params {
  11         struct mlx5e_params params;
  12         struct mlx5e_sq_param txq_sq_param;
  13 };
  14
  15 struct mlx5e_skb_cb_hwtstamp {
  16         ktime_t cqe_hwtstamp;
  17         ktime_t port_hwtstamp;
  18 };
  19
  20 void mlx5e_skb_cb_hwtstamp_init(struct sk_buff *skb)
  21 {
  22         memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
  23 }
  24
  25 static struct mlx5e_skb_cb_hwtstamp *mlx5e_skb_cb_get_hwts(struct sk_buff *skb)
  26 {
  27         BUILD_BUG_ON(sizeof(struct mlx5e_skb_cb_hwtstamp) > sizeof(skb->cb));
  28         return (struct mlx5e_skb_cb_hwtstamp *)skb->cb;
  29 }
  30
  31 static void mlx5e_skb_cb_hwtstamp_tx(struct sk_buff *skb,
  32                                      struct mlx5e_ptp_cq_stats *cq_stats)
  33 {
  34         struct skb_shared_hwtstamps hwts = {};
  35         ktime_t diff;
  36
  37         diff = abs(mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp -
  38                    mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp);
  39
  40         /* Maximal allowed diff is 1 / 128 second */
  41         if (diff > (NSEC_PER_SEC >> 7)) {
  42                 cq_stats->abort++;
  43                 cq_stats->abort_abs_diff_ns += diff;
  44                 return;
  45         }
  46
  47         hwts.hwtstamp = mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp;
  48         skb_tstamp_tx(skb, &hwts);
  49 }
  50
  51 void mlx5e_skb_cb_hwtstamp_handler(struct sk_buff *skb, int hwtstamp_type,
  52                                    ktime_t hwtstamp,
  53                                    struct mlx5e_ptp_cq_stats *cq_stats)
  54 {
  55         switch (hwtstamp_type) {
  56         case (MLX5E_SKB_CB_CQE_HWTSTAMP):
  57                 mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp = hwtstamp;
  58                 break;
  59         case (MLX5E_SKB_CB_PORT_HWTSTAMP):
  60                 mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp = hwtstamp;
  61                 break;
  62         }
  63
  64         /* If both CQEs arrive, check and report the port tstamp, and clear skb cb as
  65          * skb soon to be released.
  66          */
  67         if (!mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp ||
  68             !mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp)
  69                 return;
  70
  71         mlx5e_skb_cb_hwtstamp_tx(skb, cq_stats);
  72         memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
  73 }
  74
  75 static void mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq *ptpsq,
  76                                     struct mlx5_cqe64 *cqe,
  77                                     int budget)
  78 {
  79         struct sk_buff *skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo);
  80         struct mlx5e_txqsq *sq = &ptpsq->txqsq;
  81         ktime_t hwtstamp;
  82
  83         if (unlikely(MLX5E_RX_ERR_CQE(cqe))) {
  84                 ptpsq->cq_stats->err_cqe++;
  85                 goto out;
  86         }
  87
  88         hwtstamp = mlx5e_cqe_ts_to_ns(sq->ptp_cyc2time, sq->clock, get_cqe_ts(cqe));
  89         mlx5e_skb_cb_hwtstamp_handler(skb, MLX5E_SKB_CB_PORT_HWTSTAMP,
  90                                       hwtstamp, ptpsq->cq_stats);
  91         ptpsq->cq_stats->cqe++;
  92
  93 out:
  94         napi_consume_skb(skb, budget);
  95 }
  96
  97 static bool mlx5e_ptp_poll_ts_cq(struct mlx5e_cq *cq, int budget)
  98 {
  99         struct mlx5e_ptpsq *ptpsq = container_of(cq, struct mlx5e_ptpsq, ts_cq);
 100         struct mlx5_cqwq *cqwq = &cq->wq;
 101         struct mlx5_cqe64 *cqe;
 102         int work_done = 0;
 103
 104         if (unlikely(!test_bit(MLX5E_SQ_STATE_ENABLED, &ptpsq->txqsq.state)))
 105                 return false;
 106
 107         cqe = mlx5_cqwq_get_cqe(cqwq);
 108         if (!cqe)
 109                 return false;
 110
 111         do {
 112                 mlx5_cqwq_pop(cqwq);
 113
 114                 mlx5e_ptp_handle_ts_cqe(ptpsq, cqe, budget);
 115         } while ((++work_done < budget) && (cqe = mlx5_cqwq_get_cqe(cqwq)));
 116
 117         mlx5_cqwq_update_db_record(cqwq);
 118
 119         /* ensure cq space is freed before enabling more cqes */
 120         wmb();
 121
 122         return work_done == budget;
 123 }
 124
 125 static int mlx5e_ptp_napi_poll(struct napi_struct *napi, int budget)
 126 {
 127         struct mlx5e_ptp *c = container_of(napi, struct mlx5e_ptp, napi);
 128         struct mlx5e_ch_stats *ch_stats = c->stats;
 129         bool busy = false;
 130         int work_done = 0;
 131         int i;
 132
 133         rcu_read_lock();
 134
 135         ch_stats->poll++;
 136
 137         for (i = 0; i < c->num_tc; i++) {
 138                 busy |= mlx5e_poll_tx_cq(&c->ptpsq[i].txqsq.cq, budget);
 139                 busy |= mlx5e_ptp_poll_ts_cq(&c->ptpsq[i].ts_cq, budget);
 140         }
 141
 142         if (busy) {
 143                 work_done = budget;
 144                 goto out;
 145         }
 146
 147         if (unlikely(!napi_complete_done(napi, work_done)))
 148                 goto out;
 149
 150         ch_stats->arm++;
 151
 152         for (i = 0; i < c->num_tc; i++) {
 153                 mlx5e_cq_arm(&c->ptpsq[i].txqsq.cq);
 154                 mlx5e_cq_arm(&c->ptpsq[i].ts_cq);
 155         }
 156
 157 out:
 158         rcu_read_unlock();
 159
 160         return work_done;
 161 }
 162
 163 static int mlx5e_ptp_alloc_txqsq(struct mlx5e_ptp *c, int txq_ix,
 164                                  struct mlx5e_params *params,
 165                                  struct mlx5e_sq_param *param,
 166                                  struct mlx5e_txqsq *sq, int tc,
 167                                  struct mlx5e_ptpsq *ptpsq)
 168 {
 169         void *sqc_wq               = MLX5_ADDR_OF(sqc, param->sqc, wq);
 170         struct mlx5_core_dev *mdev = c->mdev;
 171         struct mlx5_wq_cyc *wq = &sq->wq;
 172         int err;
 173         int node;
 174
 175         sq->pdev      = c->pdev;
 176         sq->tstamp    = c->tstamp;
 177         sq->clock     = &mdev->clock;
 178         sq->mkey_be   = c->mkey_be;
 179         sq->netdev    = c->netdev;
 180         sq->priv      = c->priv;
 181         sq->mdev      = mdev;
 182         sq->ch_ix     = MLX5E_PTP_CHANNEL_IX;
 183         sq->txq_ix    = txq_ix;
 184         sq->uar_map   = mdev->mlx5e_res.hw_objs.bfreg.map;
 185         sq->min_inline_mode = params->tx_min_inline_mode;
 186         sq->hw_mtu    = MLX5E_SW2HW_MTU(params, params->sw_mtu);
 187         sq->stats     = &c->priv->ptp_stats.sq[tc];
 188         sq->ptpsq     = ptpsq;
 189         INIT_WORK(&sq->recover_work, mlx5e_tx_err_cqe_work);
 190         if (!MLX5_CAP_ETH(mdev, wqe_vlan_insert))
 191                 set_bit(MLX5E_SQ_STATE_VLAN_NEED_L2_INLINE, &sq->state);
 192         sq->stop_room = param->stop_room;
 193         sq->ptp_cyc2time = mlx5_sq_ts_translator(mdev);
 194
 195         node = dev_to_node(mlx5_core_dma_dev(mdev));
 196
 197         param->wq.db_numa_node = node;
 198         err = mlx5_wq_cyc_create(mdev, &param->wq, sqc_wq, wq, &sq->wq_ctrl);
 199         if (err)
 200                 return err;
 201         wq->db    = &wq->db[MLX5_SND_DBR];
 202
 203         err = mlx5e_alloc_txqsq_db(sq, node);
 204         if (err)
 205                 goto err_sq_wq_destroy;
 206
 207         return 0;
 208
 209 err_sq_wq_destroy:
 210         mlx5_wq_destroy(&sq->wq_ctrl);
 211
 212         return err;
 213 }
 214
 215 static void mlx5e_ptp_destroy_sq(struct mlx5_core_dev *mdev, u32 sqn)
 216 {
 217         mlx5_core_destroy_sq(mdev, sqn);
 218 }
 219
 220 static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa)
 221 {
 222         int wq_sz = mlx5_wq_cyc_get_size(&ptpsq->txqsq.wq);
 223
 224         ptpsq->skb_fifo.fifo = kvzalloc_node(array_size(wq_sz, sizeof(*ptpsq->skb_fifo.fifo)),
 225                                              GFP_KERNEL, numa);
 226         if (!ptpsq->skb_fifo.fifo)
 227                 return -ENOMEM;
 228
 229         ptpsq->skb_fifo.pc   = &ptpsq->skb_fifo_pc;
 230         ptpsq->skb_fifo.cc   = &ptpsq->skb_fifo_cc;
 231         ptpsq->skb_fifo.mask = wq_sz - 1;
 232
 233         return 0;
 234 }
 235
 236 static void mlx5e_ptp_drain_skb_fifo(struct mlx5e_skb_fifo *skb_fifo)
 237 {
 238         while (*skb_fifo->pc != *skb_fifo->cc) {
 239                 struct sk_buff *skb = mlx5e_skb_fifo_pop(skb_fifo);
 240
 241                 dev_kfree_skb_any(skb);
 242         }
 243 }
 244
 245 static void mlx5e_ptp_free_traffic_db(struct mlx5e_skb_fifo *skb_fifo)
 246 {
 247         mlx5e_ptp_drain_skb_fifo(skb_fifo);
 248         kvfree(skb_fifo->fifo);
 249 }
 250
 251 static int mlx5e_ptp_open_txqsq(struct mlx5e_ptp *c, u32 tisn,
 252                                 int txq_ix, struct mlx5e_ptp_params *cparams,
 253                                 int tc, struct mlx5e_ptpsq *ptpsq)
 254 {
 255         struct mlx5e_sq_param *sqp = &cparams->txq_sq_param;
 256         struct mlx5e_txqsq *txqsq = &ptpsq->txqsq;
 257         struct mlx5e_create_sq_param csp = {};
 258         int err;
 259
 260         err = mlx5e_ptp_alloc_txqsq(c, txq_ix, &cparams->params, sqp,
 261                                     txqsq, tc, ptpsq);
 262         if (err)
 263                 return err;
 264
 265         csp.tisn            = tisn;
 266         csp.tis_lst_sz      = 1;
 267         csp.cqn             = txqsq->cq.mcq.cqn;
 268         csp.wq_ctrl         = &txqsq->wq_ctrl;
 269         csp.min_inline_mode = txqsq->min_inline_mode;
 270         csp.ts_cqe_to_dest_cqn = ptpsq->ts_cq.mcq.cqn;
 271
 272         err = mlx5e_create_sq_rdy(c->mdev, sqp, &csp, 0, &txqsq->sqn);
 273         if (err)
 274                 goto err_free_txqsq;
 275
 276         err = mlx5e_ptp_alloc_traffic_db(ptpsq,
 277                                          dev_to_node(mlx5_core_dma_dev(c->mdev)));
 278         if (err)
 279                 goto err_free_txqsq;
 280
 281         return 0;
 282
 283 err_free_txqsq:
 284         mlx5e_free_txqsq(txqsq);
 285
 286         return err;
 287 }
 288
 289 static void mlx5e_ptp_close_txqsq(struct mlx5e_ptpsq *ptpsq)
 290 {
 291         struct mlx5e_txqsq *sq = &ptpsq->txqsq;
 292         struct mlx5_core_dev *mdev = sq->mdev;
 293
 294         mlx5e_ptp_free_traffic_db(&ptpsq->skb_fifo);
 295         cancel_work_sync(&sq->recover_work);
 296         mlx5e_ptp_destroy_sq(mdev, sq->sqn);
 297         mlx5e_free_txqsq_descs(sq);
 298         mlx5e_free_txqsq(sq);
 299 }
 300
 301 static int mlx5e_ptp_open_txqsqs(struct mlx5e_ptp *c,
 302                                  struct mlx5e_ptp_params *cparams)
 303 {
 304         struct mlx5e_params *params = &cparams->params;
 305         int ix_base;
 306         int err;
 307         int tc;
 308
 309         ix_base = params->num_tc * params->num_channels;
 310
 311         for (tc = 0; tc < params->num_tc; tc++) {
 312                 int txq_ix = ix_base + tc;
 313
 314                 err = mlx5e_ptp_open_txqsq(c, c->priv->tisn[c->lag_port][tc], txq_ix,
 315                                            cparams, tc, &c->ptpsq[tc]);
 316                 if (err)
 317                         goto close_txqsq;
 318         }
 319
 320         return 0;
 321
 322 close_txqsq:
 323         for (--tc; tc >= 0; tc--)
 324                 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
 325
 326         return err;
 327 }
 328
 329 static void mlx5e_ptp_close_txqsqs(struct mlx5e_ptp *c)
 330 {
 331         int tc;
 332
 333         for (tc = 0; tc < c->num_tc; tc++)
 334                 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
 335 }
 336
 337 static int mlx5e_ptp_open_cqs(struct mlx5e_ptp *c,
 338                               struct mlx5e_ptp_params *cparams)
 339 {
 340         struct mlx5e_params *params = &cparams->params;
 341         struct mlx5e_create_cq_param ccp = {};
 342         struct dim_cq_moder ptp_moder = {};
 343         struct mlx5e_cq_param *cq_param;
 344         int err;
 345         int tc;
 346
 347         ccp.node     = dev_to_node(mlx5_core_dma_dev(c->mdev));
 348         ccp.ch_stats = c->stats;
 349         ccp.napi     = &c->napi;
 350         ccp.ix       = MLX5E_PTP_CHANNEL_IX;
 351
 352         cq_param = &cparams->txq_sq_param.cqp;
 353
 354         for (tc = 0; tc < params->num_tc; tc++) {
 355                 struct mlx5e_cq *cq = &c->ptpsq[tc].txqsq.cq;
 356
 357                 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
 358                 if (err)
 359                         goto out_err_txqsq_cq;
 360         }
 361
 362         for (tc = 0; tc < params->num_tc; tc++) {
 363                 struct mlx5e_cq *cq = &c->ptpsq[tc].ts_cq;
 364                 struct mlx5e_ptpsq *ptpsq = &c->ptpsq[tc];
 365
 366                 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
 367                 if (err)
 368                         goto out_err_ts_cq;
 369
 370                 ptpsq->cq_stats = &c->priv->ptp_stats.cq[tc];
 371         }
 372
 373         return 0;
 374
 375 out_err_ts_cq:
 376         for (--tc; tc >= 0; tc--)
 377                 mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
 378         tc = params->num_tc;
 379 out_err_txqsq_cq:
 380         for (--tc; tc >= 0; tc--)
 381                 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
 382
 383         return err;
 384 }
 385
 386 static void mlx5e_ptp_close_cqs(struct mlx5e_ptp *c)
 387 {
 388         int tc;
 389
 390         for (tc = 0; tc < c->num_tc; tc++)
 391                 mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
 392
 393         for (tc = 0; tc < c->num_tc; tc++)
 394                 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
 395 }
 396
 397 static void mlx5e_ptp_build_sq_param(struct mlx5_core_dev *mdev,
 398                                      struct mlx5e_params *params,
 399                                      struct mlx5e_sq_param *param)
 400 {
 401         void *sqc = param->sqc;
 402         void *wq;
 403
 404         mlx5e_build_sq_param_common(mdev, param);
 405
 406         wq = MLX5_ADDR_OF(sqc, sqc, wq);
 407         MLX5_SET(wq, wq, log_wq_sz, params->log_sq_size);
 408         param->stop_room = mlx5e_stop_room_for_wqe(MLX5_SEND_WQE_MAX_WQEBBS);
 409         mlx5e_build_tx_cq_param(mdev, params, &param->cqp);
 410 }
 411
 412 static void mlx5e_ptp_build_params(struct mlx5e_ptp *c,
 413                                    struct mlx5e_ptp_params *cparams,
 414                                    struct mlx5e_params *orig)
 415 {
 416         struct mlx5e_params *params = &cparams->params;
 417
 418         params->tx_min_inline_mode = orig->tx_min_inline_mode;
 419         params->num_channels = orig->num_channels;
 420         params->hard_mtu = orig->hard_mtu;
 421         params->sw_mtu = orig->sw_mtu;
 422         params->num_tc = orig->num_tc;
 423
 424         /* SQ */
 425         params->log_sq_size = orig->log_sq_size;
 426
 427         mlx5e_ptp_build_sq_param(c->mdev, params, &cparams->txq_sq_param);
 428 }
 429
 430 static int mlx5e_ptp_open_queues(struct mlx5e_ptp *c,
 431                                  struct mlx5e_ptp_params *cparams)
 432 {
 433         int err;
 434
 435         err = mlx5e_ptp_open_cqs(c, cparams);
 436         if (err)
 437                 return err;
 438
 439         err = mlx5e_ptp_open_txqsqs(c, cparams);
 440         if (err)
 441                 goto close_cqs;
 442
 443         return 0;
 444
 445 close_cqs:
 446         mlx5e_ptp_close_cqs(c);
 447
 448         return err;
 449 }
 450
 451 static void mlx5e_ptp_close_queues(struct mlx5e_ptp *c)
 452 {
 453         mlx5e_ptp_close_txqsqs(c);
 454         mlx5e_ptp_close_cqs(c);
 455 }
 456
 457 int mlx5e_ptp_open(struct mlx5e_priv *priv, struct mlx5e_params *params,
 458                    u8 lag_port, struct mlx5e_ptp **cp)
 459 {
 460         struct net_device *netdev = priv->netdev;
 461         struct mlx5_core_dev *mdev = priv->mdev;
 462         struct mlx5e_ptp_params *cparams;
 463         struct mlx5e_ptp *c;
 464         int err;
 465
 466
 467         c = kvzalloc_node(sizeof(*c), GFP_KERNEL, dev_to_node(mlx5_core_dma_dev(mdev)));
 468         cparams = kvzalloc(sizeof(*cparams), GFP_KERNEL);
 469         if (!c || !cparams)
 470                 return -ENOMEM;
 471
 472         c->priv     = priv;
 473         c->mdev     = priv->mdev;
 474         c->tstamp   = &priv->tstamp;
 475         c->pdev     = mlx5_core_dma_dev(priv->mdev);
 476         c->netdev   = priv->netdev;
 477         c->mkey_be  = cpu_to_be32(priv->mdev->mlx5e_res.hw_objs.mkey.key);
 478         c->num_tc   = params->num_tc;
 479         c->stats    = &priv->ptp_stats.ch;
 480         c->lag_port = lag_port;
 481
 482         netif_napi_add(netdev, &c->napi, mlx5e_ptp_napi_poll, 64);
 483
 484         mlx5e_ptp_build_params(c, cparams, params);
 485
 486         err = mlx5e_ptp_open_queues(c, cparams);
 487         if (unlikely(err))
 488                 goto err_napi_del;
 489
 490         *cp = c;
 491
 492         kvfree(cparams);
 493
 494         return 0;
 495
 496 err_napi_del:
 497         netif_napi_del(&c->napi);
 498
 499         kvfree(cparams);
 500         kvfree(c);
 501         return err;
 502 }
 503
 504 void mlx5e_ptp_close(struct mlx5e_ptp *c)
 505 {
 506         mlx5e_ptp_close_queues(c);
 507         netif_napi_del(&c->napi);
 508
 509         kvfree(c);
 510 }
 511
 512 void mlx5e_ptp_activate_channel(struct mlx5e_ptp *c)
 513 {
 514         int tc;
 515
 516         napi_enable(&c->napi);
 517
 518         for (tc = 0; tc < c->num_tc; tc++)
 519                 mlx5e_activate_txqsq(&c->ptpsq[tc].txqsq);
 520 }
 521
 522 void mlx5e_ptp_deactivate_channel(struct mlx5e_ptp *c)
 523 {
 524         int tc;
 525
 526         for (tc = 0; tc < c->num_tc; tc++)
 527                 mlx5e_deactivate_txqsq(&c->ptpsq[tc].txqsq);
 528
 529         napi_disable(&c->napi);
 530 }