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 "lib/clock.h" 7 8 struct mlx5e_skb_cb_hwtstamp { 9 ktime_t cqe_hwtstamp; 10 ktime_t port_hwtstamp; 11 }; 12 13 void mlx5e_skb_cb_hwtstamp_init(struct sk_buff *skb) 14 { 15 memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp)); 16 } 17 18 static struct mlx5e_skb_cb_hwtstamp *mlx5e_skb_cb_get_hwts(struct sk_buff *skb) 19 { 20 BUILD_BUG_ON(sizeof(struct mlx5e_skb_cb_hwtstamp) > sizeof(skb->cb)); 21 return (struct mlx5e_skb_cb_hwtstamp *)skb->cb; 22 } 23 24 static void mlx5e_skb_cb_hwtstamp_tx(struct sk_buff *skb, 25 struct mlx5e_ptp_cq_stats *cq_stats) 26 { 27 struct skb_shared_hwtstamps hwts = {}; 28 ktime_t diff; 29 30 diff = abs(mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp - 31 mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp); 32 33 /* Maximal allowed diff is 1 / 128 second */ 34 if (diff > (NSEC_PER_SEC >> 7)) { 35 cq_stats->abort++; 36 cq_stats->abort_abs_diff_ns += diff; 37 return; 38 } 39 40 hwts.hwtstamp = mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp; 41 skb_tstamp_tx(skb, &hwts); 42 } 43 44 void mlx5e_skb_cb_hwtstamp_handler(struct sk_buff *skb, int hwtstamp_type, 45 ktime_t hwtstamp, 46 struct mlx5e_ptp_cq_stats *cq_stats) 47 { 48 switch (hwtstamp_type) { 49 case (MLX5E_SKB_CB_CQE_HWTSTAMP): 50 mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp = hwtstamp; 51 break; 52 case (MLX5E_SKB_CB_PORT_HWTSTAMP): 53 mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp = hwtstamp; 54 break; 55 } 56 57 /* If both CQEs arrive, check and report the port tstamp, and clear skb cb as 58 * skb soon to be released. 59 */ 60 if (!mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp || 61 !mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp) 62 return; 63 64 mlx5e_skb_cb_hwtstamp_tx(skb, cq_stats); 65 memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp)); 66 } 67 68 static void mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq *ptpsq, 69 struct mlx5_cqe64 *cqe, 70 int budget) 71 { 72 struct sk_buff *skb = mlx5e_skb_fifo_pop(&ptpsq->skb_fifo); 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 = mlx5_timecounter_cyc2time(ptpsq->txqsq.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 187 node = dev_to_node(mlx5_core_dma_dev(mdev)); 188 189 param->wq.db_numa_node = node; 190 err = mlx5_wq_cyc_create(mdev, ¶m->wq, sqc_wq, wq, &sq->wq_ctrl); 191 if (err) 192 return err; 193 wq->db = &wq->db[MLX5_SND_DBR]; 194 195 err = mlx5e_alloc_txqsq_db(sq, node); 196 if (err) 197 goto err_sq_wq_destroy; 198 199 return 0; 200 201 err_sq_wq_destroy: 202 mlx5_wq_destroy(&sq->wq_ctrl); 203 204 return err; 205 } 206 207 static void mlx5e_ptp_destroy_sq(struct mlx5_core_dev *mdev, u32 sqn) 208 { 209 mlx5_core_destroy_sq(mdev, sqn); 210 } 211 212 static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa) 213 { 214 int wq_sz = mlx5_wq_cyc_get_size(&ptpsq->txqsq.wq); 215 216 ptpsq->skb_fifo.fifo = kvzalloc_node(array_size(wq_sz, sizeof(*ptpsq->skb_fifo.fifo)), 217 GFP_KERNEL, numa); 218 if (!ptpsq->skb_fifo.fifo) 219 return -ENOMEM; 220 221 ptpsq->skb_fifo.pc = &ptpsq->skb_fifo_pc; 222 ptpsq->skb_fifo.cc = &ptpsq->skb_fifo_cc; 223 ptpsq->skb_fifo.mask = wq_sz - 1; 224 225 return 0; 226 } 227 228 static void mlx5e_ptp_drain_skb_fifo(struct mlx5e_skb_fifo *skb_fifo) 229 { 230 while (*skb_fifo->pc != *skb_fifo->cc) { 231 struct sk_buff *skb = mlx5e_skb_fifo_pop(skb_fifo); 232 233 dev_kfree_skb_any(skb); 234 } 235 } 236 237 static void mlx5e_ptp_free_traffic_db(struct mlx5e_skb_fifo *skb_fifo) 238 { 239 mlx5e_ptp_drain_skb_fifo(skb_fifo); 240 kvfree(skb_fifo->fifo); 241 } 242 243 static int mlx5e_ptp_open_txqsq(struct mlx5e_port_ptp *c, u32 tisn, 244 int txq_ix, struct mlx5e_ptp_params *cparams, 245 int tc, struct mlx5e_ptpsq *ptpsq) 246 { 247 struct mlx5e_sq_param *sqp = &cparams->txq_sq_param; 248 struct mlx5e_txqsq *txqsq = &ptpsq->txqsq; 249 struct mlx5e_create_sq_param csp = {}; 250 int err; 251 252 err = mlx5e_ptp_alloc_txqsq(c, txq_ix, &cparams->params, sqp, 253 txqsq, tc, ptpsq); 254 if (err) 255 return err; 256 257 csp.tisn = tisn; 258 csp.tis_lst_sz = 1; 259 csp.cqn = txqsq->cq.mcq.cqn; 260 csp.wq_ctrl = &txqsq->wq_ctrl; 261 csp.min_inline_mode = txqsq->min_inline_mode; 262 csp.ts_cqe_to_dest_cqn = ptpsq->ts_cq.mcq.cqn; 263 264 err = mlx5e_create_sq_rdy(c->mdev, sqp, &csp, &txqsq->sqn); 265 if (err) 266 goto err_free_txqsq; 267 268 err = mlx5e_ptp_alloc_traffic_db(ptpsq, 269 dev_to_node(mlx5_core_dma_dev(c->mdev))); 270 if (err) 271 goto err_free_txqsq; 272 273 return 0; 274 275 err_free_txqsq: 276 mlx5e_free_txqsq(txqsq); 277 278 return err; 279 } 280 281 static void mlx5e_ptp_close_txqsq(struct mlx5e_ptpsq *ptpsq) 282 { 283 struct mlx5e_txqsq *sq = &ptpsq->txqsq; 284 struct mlx5_core_dev *mdev = sq->mdev; 285 286 mlx5e_ptp_free_traffic_db(&ptpsq->skb_fifo); 287 cancel_work_sync(&sq->recover_work); 288 mlx5e_ptp_destroy_sq(mdev, sq->sqn); 289 mlx5e_free_txqsq_descs(sq); 290 mlx5e_free_txqsq(sq); 291 } 292 293 static int mlx5e_ptp_open_txqsqs(struct mlx5e_port_ptp *c, 294 struct mlx5e_ptp_params *cparams) 295 { 296 struct mlx5e_params *params = &cparams->params; 297 int ix_base; 298 int err; 299 int tc; 300 301 ix_base = params->num_tc * params->num_channels; 302 303 for (tc = 0; tc < params->num_tc; tc++) { 304 int txq_ix = ix_base + tc; 305 306 err = mlx5e_ptp_open_txqsq(c, c->priv->tisn[c->lag_port][tc], txq_ix, 307 cparams, tc, &c->ptpsq[tc]); 308 if (err) 309 goto close_txqsq; 310 } 311 312 return 0; 313 314 close_txqsq: 315 for (--tc; tc >= 0; tc--) 316 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]); 317 318 return err; 319 } 320 321 static void mlx5e_ptp_close_txqsqs(struct mlx5e_port_ptp *c) 322 { 323 int tc; 324 325 for (tc = 0; tc < c->num_tc; tc++) 326 mlx5e_ptp_close_txqsq(&c->ptpsq[tc]); 327 } 328 329 static int mlx5e_ptp_open_cqs(struct mlx5e_port_ptp *c, 330 struct mlx5e_ptp_params *cparams) 331 { 332 struct mlx5e_params *params = &cparams->params; 333 struct mlx5e_create_cq_param ccp = {}; 334 struct dim_cq_moder ptp_moder = {}; 335 struct mlx5e_cq_param *cq_param; 336 int err; 337 int tc; 338 339 ccp.node = dev_to_node(mlx5_core_dma_dev(c->mdev)); 340 ccp.ch_stats = c->stats; 341 ccp.napi = &c->napi; 342 ccp.ix = c->ix; 343 344 cq_param = &cparams->txq_sq_param.cqp; 345 346 for (tc = 0; tc < params->num_tc; tc++) { 347 struct mlx5e_cq *cq = &c->ptpsq[tc].txqsq.cq; 348 349 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq); 350 if (err) 351 goto out_err_txqsq_cq; 352 } 353 354 for (tc = 0; tc < params->num_tc; tc++) { 355 struct mlx5e_cq *cq = &c->ptpsq[tc].ts_cq; 356 struct mlx5e_ptpsq *ptpsq = &c->ptpsq[tc]; 357 358 err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq); 359 if (err) 360 goto out_err_ts_cq; 361 362 ptpsq->cq_stats = &c->priv->port_ptp_stats.cq[tc]; 363 } 364 365 return 0; 366 367 out_err_ts_cq: 368 for (--tc; tc >= 0; tc--) 369 mlx5e_close_cq(&c->ptpsq[tc].ts_cq); 370 tc = params->num_tc; 371 out_err_txqsq_cq: 372 for (--tc; tc >= 0; tc--) 373 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq); 374 375 return err; 376 } 377 378 static void mlx5e_ptp_close_cqs(struct mlx5e_port_ptp *c) 379 { 380 int tc; 381 382 for (tc = 0; tc < c->num_tc; tc++) 383 mlx5e_close_cq(&c->ptpsq[tc].ts_cq); 384 385 for (tc = 0; tc < c->num_tc; tc++) 386 mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq); 387 } 388 389 static void mlx5e_ptp_build_sq_param(struct mlx5e_priv *priv, 390 struct mlx5e_params *params, 391 struct mlx5e_sq_param *param) 392 { 393 void *sqc = param->sqc; 394 void *wq; 395 396 mlx5e_build_sq_param_common(priv, param); 397 398 wq = MLX5_ADDR_OF(sqc, sqc, wq); 399 MLX5_SET(wq, wq, log_wq_sz, params->log_sq_size); 400 param->stop_room = mlx5e_stop_room_for_wqe(MLX5_SEND_WQE_MAX_WQEBBS); 401 mlx5e_build_tx_cq_param(priv, params, ¶m->cqp); 402 } 403 404 static void mlx5e_ptp_build_params(struct mlx5e_port_ptp *c, 405 struct mlx5e_ptp_params *cparams, 406 struct mlx5e_params *orig) 407 { 408 struct mlx5e_params *params = &cparams->params; 409 410 params->tx_min_inline_mode = orig->tx_min_inline_mode; 411 params->num_channels = orig->num_channels; 412 params->hard_mtu = orig->hard_mtu; 413 params->sw_mtu = orig->sw_mtu; 414 params->num_tc = orig->num_tc; 415 416 /* SQ */ 417 params->log_sq_size = orig->log_sq_size; 418 419 mlx5e_ptp_build_sq_param(c->priv, params, &cparams->txq_sq_param); 420 } 421 422 static int mlx5e_ptp_open_queues(struct mlx5e_port_ptp *c, 423 struct mlx5e_ptp_params *cparams) 424 { 425 int err; 426 427 err = mlx5e_ptp_open_cqs(c, cparams); 428 if (err) 429 return err; 430 431 napi_enable(&c->napi); 432 433 err = mlx5e_ptp_open_txqsqs(c, cparams); 434 if (err) 435 goto disable_napi; 436 437 return 0; 438 439 disable_napi: 440 napi_disable(&c->napi); 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 napi_disable(&c->napi); 450 mlx5e_ptp_close_cqs(c); 451 } 452 453 int mlx5e_port_ptp_open(struct mlx5e_priv *priv, struct mlx5e_params *params, 454 u8 lag_port, struct mlx5e_port_ptp **cp) 455 { 456 struct net_device *netdev = priv->netdev; 457 struct mlx5_core_dev *mdev = priv->mdev; 458 struct mlx5e_ptp_params *cparams; 459 struct mlx5e_port_ptp *c; 460 unsigned int irq; 461 int err; 462 int eqn; 463 464 err = mlx5_vector2eqn(priv->mdev, 0, &eqn, &irq); 465 if (err) 466 return err; 467 468 c = kvzalloc_node(sizeof(*c), GFP_KERNEL, dev_to_node(mlx5_core_dma_dev(mdev))); 469 cparams = kvzalloc(sizeof(*cparams), GFP_KERNEL); 470 if (!c || !cparams) 471 return -ENOMEM; 472 473 c->priv = priv; 474 c->mdev = priv->mdev; 475 c->tstamp = &priv->tstamp; 476 c->ix = 0; 477 c->pdev = mlx5_core_dma_dev(priv->mdev); 478 c->netdev = priv->netdev; 479 c->mkey_be = cpu_to_be32(priv->mdev->mlx5e_res.mkey.key); 480 c->num_tc = params->num_tc; 481 c->stats = &priv->port_ptp_stats.ch; 482 c->lag_port = lag_port; 483 484 netif_napi_add(netdev, &c->napi, mlx5e_ptp_napi_poll, 64); 485 486 mlx5e_ptp_build_params(c, cparams, params); 487 488 err = mlx5e_ptp_open_queues(c, cparams); 489 if (unlikely(err)) 490 goto err_napi_del; 491 492 *cp = c; 493 494 kvfree(cparams); 495 496 return 0; 497 498 err_napi_del: 499 netif_napi_del(&c->napi); 500 501 kvfree(cparams); 502 kvfree(c); 503 return err; 504 } 505 506 void mlx5e_port_ptp_close(struct mlx5e_port_ptp *c) 507 { 508 mlx5e_ptp_close_queues(c); 509 netif_napi_del(&c->napi); 510 511 kvfree(c); 512 } 513 514 void mlx5e_ptp_activate_channel(struct mlx5e_port_ptp *c) 515 { 516 int tc; 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_port_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