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