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