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