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