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