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/health.h"
6 #include "en/txrx.h"
7 #include "en/params.h"
8 #include "en/fs_tt_redirect.h"
9 #include <linux/list.h>
10 #include <linux/spinlock.h>
11 
12 struct mlx5e_ptp_fs {
13 	struct mlx5_flow_handle *l2_rule;
14 	struct mlx5_flow_handle *udp_v4_rule;
15 	struct mlx5_flow_handle *udp_v6_rule;
16 	bool valid;
17 };
18 
19 struct mlx5e_ptp_params {
20 	struct mlx5e_params params;
21 	struct mlx5e_sq_param txq_sq_param;
22 	struct mlx5e_rq_param rq_param;
23 };
24 
25 struct mlx5e_ptp_port_ts_cqe_tracker {
26 	u8 metadata_id;
27 	bool inuse : 1;
28 	struct list_head entry;
29 };
30 
31 struct mlx5e_ptp_port_ts_cqe_list {
32 	struct mlx5e_ptp_port_ts_cqe_tracker *nodes;
33 	struct list_head tracker_list_head;
34 	/* Sync list operations in xmit and napi_poll contexts */
35 	spinlock_t tracker_list_lock;
36 };
37 
38 static inline void
mlx5e_ptp_port_ts_cqe_list_add(struct mlx5e_ptp_port_ts_cqe_list * list,u8 metadata)39 mlx5e_ptp_port_ts_cqe_list_add(struct mlx5e_ptp_port_ts_cqe_list *list, u8 metadata)
40 {
41 	struct mlx5e_ptp_port_ts_cqe_tracker *tracker = &list->nodes[metadata];
42 
43 	WARN_ON_ONCE(tracker->inuse);
44 	tracker->inuse = true;
45 	spin_lock(&list->tracker_list_lock);
46 	list_add_tail(&tracker->entry, &list->tracker_list_head);
47 	spin_unlock(&list->tracker_list_lock);
48 }
49 
50 static void
mlx5e_ptp_port_ts_cqe_list_remove(struct mlx5e_ptp_port_ts_cqe_list * list,u8 metadata)51 mlx5e_ptp_port_ts_cqe_list_remove(struct mlx5e_ptp_port_ts_cqe_list *list, u8 metadata)
52 {
53 	struct mlx5e_ptp_port_ts_cqe_tracker *tracker = &list->nodes[metadata];
54 
55 	WARN_ON_ONCE(!tracker->inuse);
56 	tracker->inuse = false;
57 	spin_lock(&list->tracker_list_lock);
58 	list_del(&tracker->entry);
59 	spin_unlock(&list->tracker_list_lock);
60 }
61 
mlx5e_ptpsq_track_metadata(struct mlx5e_ptpsq * ptpsq,u8 metadata)62 void mlx5e_ptpsq_track_metadata(struct mlx5e_ptpsq *ptpsq, u8 metadata)
63 {
64 	mlx5e_ptp_port_ts_cqe_list_add(ptpsq->ts_cqe_pending_list, metadata);
65 }
66 
67 struct mlx5e_skb_cb_hwtstamp {
68 	ktime_t cqe_hwtstamp;
69 	ktime_t port_hwtstamp;
70 };
71 
mlx5e_skb_cb_hwtstamp_init(struct sk_buff * skb)72 void mlx5e_skb_cb_hwtstamp_init(struct sk_buff *skb)
73 {
74 	memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
75 }
76 
mlx5e_skb_cb_get_hwts(struct sk_buff * skb)77 static struct mlx5e_skb_cb_hwtstamp *mlx5e_skb_cb_get_hwts(struct sk_buff *skb)
78 {
79 	BUILD_BUG_ON(sizeof(struct mlx5e_skb_cb_hwtstamp) > sizeof(skb->cb));
80 	return (struct mlx5e_skb_cb_hwtstamp *)skb->cb;
81 }
82 
mlx5e_skb_cb_hwtstamp_tx(struct sk_buff * skb,struct mlx5e_ptp_cq_stats * cq_stats)83 static void mlx5e_skb_cb_hwtstamp_tx(struct sk_buff *skb,
84 				     struct mlx5e_ptp_cq_stats *cq_stats)
85 {
86 	struct skb_shared_hwtstamps hwts = {};
87 	ktime_t diff;
88 
89 	diff = abs(mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp -
90 		   mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp);
91 
92 	/* Maximal allowed diff is 1 / 128 second */
93 	if (diff > (NSEC_PER_SEC >> 7)) {
94 		cq_stats->abort++;
95 		cq_stats->abort_abs_diff_ns += diff;
96 		return;
97 	}
98 
99 	hwts.hwtstamp = mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp;
100 	skb_tstamp_tx(skb, &hwts);
101 }
102 
mlx5e_skb_cb_hwtstamp_handler(struct sk_buff * skb,int hwtstamp_type,ktime_t hwtstamp,struct mlx5e_ptp_cq_stats * cq_stats)103 void mlx5e_skb_cb_hwtstamp_handler(struct sk_buff *skb, int hwtstamp_type,
104 				   ktime_t hwtstamp,
105 				   struct mlx5e_ptp_cq_stats *cq_stats)
106 {
107 	switch (hwtstamp_type) {
108 	case (MLX5E_SKB_CB_CQE_HWTSTAMP):
109 		mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp = hwtstamp;
110 		break;
111 	case (MLX5E_SKB_CB_PORT_HWTSTAMP):
112 		mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp = hwtstamp;
113 		break;
114 	}
115 
116 	/* If both CQEs arrive, check and report the port tstamp, and clear skb cb as
117 	 * skb soon to be released.
118 	 */
119 	if (!mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp ||
120 	    !mlx5e_skb_cb_get_hwts(skb)->port_hwtstamp)
121 		return;
122 
123 	mlx5e_skb_cb_hwtstamp_tx(skb, cq_stats);
124 	memset(skb->cb, 0, sizeof(struct mlx5e_skb_cb_hwtstamp));
125 }
126 
127 static struct sk_buff *
mlx5e_ptp_metadata_map_lookup(struct mlx5e_ptp_metadata_map * map,u16 metadata)128 mlx5e_ptp_metadata_map_lookup(struct mlx5e_ptp_metadata_map *map, u16 metadata)
129 {
130 	return map->data[metadata];
131 }
132 
133 static struct sk_buff *
mlx5e_ptp_metadata_map_remove(struct mlx5e_ptp_metadata_map * map,u16 metadata)134 mlx5e_ptp_metadata_map_remove(struct mlx5e_ptp_metadata_map *map, u16 metadata)
135 {
136 	struct sk_buff *skb;
137 
138 	skb = map->data[metadata];
139 	map->data[metadata] = NULL;
140 
141 	return skb;
142 }
143 
mlx5e_ptp_metadata_map_unhealthy(struct mlx5e_ptp_metadata_map * map)144 static bool mlx5e_ptp_metadata_map_unhealthy(struct mlx5e_ptp_metadata_map *map)
145 {
146 	/* Considered beginning unhealthy state if size * 15 / 2^4 cannot be reclaimed. */
147 	return map->undelivered_counter > (map->capacity >> 4) * 15;
148 }
149 
mlx5e_ptpsq_mark_ts_cqes_undelivered(struct mlx5e_ptpsq * ptpsq,ktime_t port_tstamp)150 static void mlx5e_ptpsq_mark_ts_cqes_undelivered(struct mlx5e_ptpsq *ptpsq,
151 						 ktime_t port_tstamp)
152 {
153 	struct mlx5e_ptp_port_ts_cqe_list *cqe_list = ptpsq->ts_cqe_pending_list;
154 	ktime_t timeout = ns_to_ktime(MLX5E_PTP_TS_CQE_UNDELIVERED_TIMEOUT);
155 	struct mlx5e_ptp_metadata_map *metadata_map = &ptpsq->metadata_map;
156 	struct mlx5e_ptp_port_ts_cqe_tracker *pos, *n;
157 
158 	spin_lock(&cqe_list->tracker_list_lock);
159 	list_for_each_entry_safe(pos, n, &cqe_list->tracker_list_head, entry) {
160 		struct sk_buff *skb =
161 			mlx5e_ptp_metadata_map_lookup(metadata_map, pos->metadata_id);
162 		ktime_t dma_tstamp = mlx5e_skb_cb_get_hwts(skb)->cqe_hwtstamp;
163 
164 		if (!dma_tstamp ||
165 		    ktime_after(ktime_add(dma_tstamp, timeout), port_tstamp))
166 			break;
167 
168 		metadata_map->undelivered_counter++;
169 		WARN_ON_ONCE(!pos->inuse);
170 		pos->inuse = false;
171 		list_del(&pos->entry);
172 	}
173 	spin_unlock(&cqe_list->tracker_list_lock);
174 }
175 
176 #define PTP_WQE_CTR2IDX(val) ((val) & ptpsq->ts_cqe_ctr_mask)
177 
mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq * ptpsq,struct mlx5_cqe64 * cqe,u8 * md_buff,u8 * md_buff_sz,int budget)178 static void mlx5e_ptp_handle_ts_cqe(struct mlx5e_ptpsq *ptpsq,
179 				    struct mlx5_cqe64 *cqe,
180 				    u8 *md_buff,
181 				    u8 *md_buff_sz,
182 				    int budget)
183 {
184 	struct mlx5e_ptp_port_ts_cqe_list *pending_cqe_list = ptpsq->ts_cqe_pending_list;
185 	u8 metadata_id = PTP_WQE_CTR2IDX(be16_to_cpu(cqe->wqe_counter));
186 	bool is_err_cqe = !!MLX5E_RX_ERR_CQE(cqe);
187 	struct mlx5e_txqsq *sq = &ptpsq->txqsq;
188 	struct sk_buff *skb;
189 	ktime_t hwtstamp;
190 
191 	if (likely(pending_cqe_list->nodes[metadata_id].inuse)) {
192 		mlx5e_ptp_port_ts_cqe_list_remove(pending_cqe_list, metadata_id);
193 	} else {
194 		/* Reclaim space in the unlikely event CQE was delivered after
195 		 * marking it late.
196 		 */
197 		ptpsq->metadata_map.undelivered_counter--;
198 		ptpsq->cq_stats->late_cqe++;
199 	}
200 
201 	skb = mlx5e_ptp_metadata_map_remove(&ptpsq->metadata_map, metadata_id);
202 
203 	if (unlikely(is_err_cqe)) {
204 		ptpsq->cq_stats->err_cqe++;
205 		goto out;
206 	}
207 
208 	hwtstamp = mlx5e_cqe_ts_to_ns(sq->ptp_cyc2time, sq->clock, get_cqe_ts(cqe));
209 	mlx5e_skb_cb_hwtstamp_handler(skb, MLX5E_SKB_CB_PORT_HWTSTAMP,
210 				      hwtstamp, ptpsq->cq_stats);
211 	ptpsq->cq_stats->cqe++;
212 
213 	mlx5e_ptpsq_mark_ts_cqes_undelivered(ptpsq, hwtstamp);
214 out:
215 	napi_consume_skb(skb, budget);
216 	md_buff[(*md_buff_sz)++] = metadata_id;
217 	if (unlikely(mlx5e_ptp_metadata_map_unhealthy(&ptpsq->metadata_map)) &&
218 	    !test_and_set_bit(MLX5E_SQ_STATE_RECOVERING, &sq->state))
219 		queue_work(ptpsq->txqsq.priv->wq, &ptpsq->report_unhealthy_work);
220 }
221 
mlx5e_ptp_poll_ts_cq(struct mlx5e_cq * cq,int napi_budget)222 static bool mlx5e_ptp_poll_ts_cq(struct mlx5e_cq *cq, int napi_budget)
223 {
224 	struct mlx5e_ptpsq *ptpsq = container_of(cq, struct mlx5e_ptpsq, ts_cq);
225 	int budget = min(napi_budget, MLX5E_TX_CQ_POLL_BUDGET);
226 	u8 metadata_buff[MLX5E_TX_CQ_POLL_BUDGET];
227 	u8 metadata_buff_sz = 0;
228 	struct mlx5_cqwq *cqwq;
229 	struct mlx5_cqe64 *cqe;
230 	int work_done = 0;
231 
232 	cqwq = &cq->wq;
233 
234 	if (unlikely(!test_bit(MLX5E_SQ_STATE_ENABLED, &ptpsq->txqsq.state)))
235 		return false;
236 
237 	cqe = mlx5_cqwq_get_cqe(cqwq);
238 	if (!cqe)
239 		return false;
240 
241 	do {
242 		mlx5_cqwq_pop(cqwq);
243 
244 		mlx5e_ptp_handle_ts_cqe(ptpsq, cqe,
245 					metadata_buff, &metadata_buff_sz, napi_budget);
246 	} while ((++work_done < budget) && (cqe = mlx5_cqwq_get_cqe(cqwq)));
247 
248 	mlx5_cqwq_update_db_record(cqwq);
249 
250 	/* ensure cq space is freed before enabling more cqes */
251 	wmb();
252 
253 	while (metadata_buff_sz > 0)
254 		mlx5e_ptp_metadata_fifo_push(&ptpsq->metadata_freelist,
255 					     metadata_buff[--metadata_buff_sz]);
256 
257 	mlx5e_txqsq_wake(&ptpsq->txqsq);
258 
259 	return work_done == budget;
260 }
261 
mlx5e_ptp_napi_poll(struct napi_struct * napi,int budget)262 static int mlx5e_ptp_napi_poll(struct napi_struct *napi, int budget)
263 {
264 	struct mlx5e_ptp *c = container_of(napi, struct mlx5e_ptp, napi);
265 	struct mlx5e_ch_stats *ch_stats = c->stats;
266 	struct mlx5e_rq *rq = &c->rq;
267 	bool busy = false;
268 	int work_done = 0;
269 	int i;
270 
271 	rcu_read_lock();
272 
273 	ch_stats->poll++;
274 
275 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
276 		for (i = 0; i < c->num_tc; i++) {
277 			busy |= mlx5e_poll_tx_cq(&c->ptpsq[i].txqsq.cq, budget);
278 			busy |= mlx5e_ptp_poll_ts_cq(&c->ptpsq[i].ts_cq, budget);
279 		}
280 	}
281 	if (test_bit(MLX5E_PTP_STATE_RX, c->state) && likely(budget)) {
282 		work_done = mlx5e_poll_rx_cq(&rq->cq, budget);
283 		busy |= work_done == budget;
284 		busy |= INDIRECT_CALL_2(rq->post_wqes,
285 					mlx5e_post_rx_mpwqes,
286 					mlx5e_post_rx_wqes,
287 					rq);
288 	}
289 
290 	if (busy) {
291 		work_done = budget;
292 		goto out;
293 	}
294 
295 	if (unlikely(!napi_complete_done(napi, work_done)))
296 		goto out;
297 
298 	ch_stats->arm++;
299 
300 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
301 		for (i = 0; i < c->num_tc; i++) {
302 			mlx5e_cq_arm(&c->ptpsq[i].txqsq.cq);
303 			mlx5e_cq_arm(&c->ptpsq[i].ts_cq);
304 		}
305 	}
306 	if (test_bit(MLX5E_PTP_STATE_RX, c->state))
307 		mlx5e_cq_arm(&rq->cq);
308 
309 out:
310 	rcu_read_unlock();
311 
312 	return work_done;
313 }
314 
mlx5e_ptp_alloc_txqsq(struct mlx5e_ptp * c,int txq_ix,struct mlx5e_params * params,struct mlx5e_sq_param * param,struct mlx5e_txqsq * sq,int tc,struct mlx5e_ptpsq * ptpsq)315 static int mlx5e_ptp_alloc_txqsq(struct mlx5e_ptp *c, int txq_ix,
316 				 struct mlx5e_params *params,
317 				 struct mlx5e_sq_param *param,
318 				 struct mlx5e_txqsq *sq, int tc,
319 				 struct mlx5e_ptpsq *ptpsq)
320 {
321 	void *sqc_wq               = MLX5_ADDR_OF(sqc, param->sqc, wq);
322 	struct mlx5_core_dev *mdev = c->mdev;
323 	struct mlx5_wq_cyc *wq = &sq->wq;
324 	int err;
325 	int node;
326 
327 	sq->pdev      = c->pdev;
328 	sq->clock     = &mdev->clock;
329 	sq->mkey_be   = c->mkey_be;
330 	sq->netdev    = c->netdev;
331 	sq->priv      = c->priv;
332 	sq->mdev      = mdev;
333 	sq->ch_ix     = MLX5E_PTP_CHANNEL_IX;
334 	sq->txq_ix    = txq_ix;
335 	sq->uar_map   = mdev->mlx5e_res.hw_objs.bfreg.map;
336 	sq->min_inline_mode = params->tx_min_inline_mode;
337 	sq->hw_mtu    = MLX5E_SW2HW_MTU(params, params->sw_mtu);
338 	sq->stats     = &c->priv->ptp_stats.sq[tc];
339 	sq->ptpsq     = ptpsq;
340 	INIT_WORK(&sq->recover_work, mlx5e_tx_err_cqe_work);
341 	if (!MLX5_CAP_ETH(mdev, wqe_vlan_insert))
342 		set_bit(MLX5E_SQ_STATE_VLAN_NEED_L2_INLINE, &sq->state);
343 	sq->stop_room = param->stop_room;
344 	sq->ptp_cyc2time = mlx5_sq_ts_translator(mdev);
345 
346 	node = dev_to_node(mlx5_core_dma_dev(mdev));
347 
348 	param->wq.db_numa_node = node;
349 	err = mlx5_wq_cyc_create(mdev, &param->wq, sqc_wq, wq, &sq->wq_ctrl);
350 	if (err)
351 		return err;
352 	wq->db    = &wq->db[MLX5_SND_DBR];
353 
354 	err = mlx5e_alloc_txqsq_db(sq, node);
355 	if (err)
356 		goto err_sq_wq_destroy;
357 
358 	return 0;
359 
360 err_sq_wq_destroy:
361 	mlx5_wq_destroy(&sq->wq_ctrl);
362 
363 	return err;
364 }
365 
mlx5e_ptp_destroy_sq(struct mlx5_core_dev * mdev,u32 sqn)366 static void mlx5e_ptp_destroy_sq(struct mlx5_core_dev *mdev, u32 sqn)
367 {
368 	mlx5_core_destroy_sq(mdev, sqn);
369 }
370 
mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq * ptpsq,int numa)371 static int mlx5e_ptp_alloc_traffic_db(struct mlx5e_ptpsq *ptpsq, int numa)
372 {
373 	struct mlx5e_ptp_metadata_fifo *metadata_freelist = &ptpsq->metadata_freelist;
374 	struct mlx5e_ptp_metadata_map *metadata_map = &ptpsq->metadata_map;
375 	struct mlx5e_ptp_port_ts_cqe_list *cqe_list;
376 	int db_sz;
377 	int md;
378 
379 	cqe_list = kvzalloc_node(sizeof(*ptpsq->ts_cqe_pending_list), GFP_KERNEL, numa);
380 	if (!cqe_list)
381 		return -ENOMEM;
382 	ptpsq->ts_cqe_pending_list = cqe_list;
383 
384 	db_sz = min_t(u32, mlx5_wq_cyc_get_size(&ptpsq->txqsq.wq),
385 		      1 << MLX5_CAP_GEN_2(ptpsq->txqsq.mdev,
386 					  ts_cqe_metadata_size2wqe_counter));
387 	ptpsq->ts_cqe_ctr_mask = db_sz - 1;
388 
389 	cqe_list->nodes = kvzalloc_node(array_size(db_sz, sizeof(*cqe_list->nodes)),
390 					GFP_KERNEL, numa);
391 	if (!cqe_list->nodes)
392 		goto free_cqe_list;
393 	INIT_LIST_HEAD(&cqe_list->tracker_list_head);
394 	spin_lock_init(&cqe_list->tracker_list_lock);
395 
396 	metadata_freelist->data =
397 		kvzalloc_node(array_size(db_sz, sizeof(*metadata_freelist->data)),
398 			      GFP_KERNEL, numa);
399 	if (!metadata_freelist->data)
400 		goto free_cqe_list_nodes;
401 	metadata_freelist->mask = ptpsq->ts_cqe_ctr_mask;
402 
403 	for (md = 0; md < db_sz; ++md) {
404 		cqe_list->nodes[md].metadata_id = md;
405 		metadata_freelist->data[md] = md;
406 	}
407 	metadata_freelist->pc = db_sz;
408 
409 	metadata_map->data =
410 		kvzalloc_node(array_size(db_sz, sizeof(*metadata_map->data)),
411 			      GFP_KERNEL, numa);
412 	if (!metadata_map->data)
413 		goto free_metadata_freelist;
414 	metadata_map->capacity = db_sz;
415 
416 	return 0;
417 
418 free_metadata_freelist:
419 	kvfree(metadata_freelist->data);
420 free_cqe_list_nodes:
421 	kvfree(cqe_list->nodes);
422 free_cqe_list:
423 	kvfree(cqe_list);
424 	return -ENOMEM;
425 }
426 
mlx5e_ptp_drain_metadata_map(struct mlx5e_ptp_metadata_map * map)427 static void mlx5e_ptp_drain_metadata_map(struct mlx5e_ptp_metadata_map *map)
428 {
429 	int idx;
430 
431 	for (idx = 0; idx < map->capacity; ++idx) {
432 		struct sk_buff *skb = map->data[idx];
433 
434 		dev_kfree_skb_any(skb);
435 	}
436 }
437 
mlx5e_ptp_free_traffic_db(struct mlx5e_ptpsq * ptpsq)438 static void mlx5e_ptp_free_traffic_db(struct mlx5e_ptpsq *ptpsq)
439 {
440 	mlx5e_ptp_drain_metadata_map(&ptpsq->metadata_map);
441 	kvfree(ptpsq->metadata_map.data);
442 	kvfree(ptpsq->metadata_freelist.data);
443 	kvfree(ptpsq->ts_cqe_pending_list->nodes);
444 	kvfree(ptpsq->ts_cqe_pending_list);
445 }
446 
mlx5e_ptpsq_unhealthy_work(struct work_struct * work)447 static void mlx5e_ptpsq_unhealthy_work(struct work_struct *work)
448 {
449 	struct mlx5e_ptpsq *ptpsq =
450 		container_of(work, struct mlx5e_ptpsq, report_unhealthy_work);
451 
452 	mlx5e_reporter_tx_ptpsq_unhealthy(ptpsq);
453 }
454 
mlx5e_ptp_open_txqsq(struct mlx5e_ptp * c,u32 tisn,int txq_ix,struct mlx5e_ptp_params * cparams,int tc,struct mlx5e_ptpsq * ptpsq)455 static int mlx5e_ptp_open_txqsq(struct mlx5e_ptp *c, u32 tisn,
456 				int txq_ix, struct mlx5e_ptp_params *cparams,
457 				int tc, struct mlx5e_ptpsq *ptpsq)
458 {
459 	struct mlx5e_sq_param *sqp = &cparams->txq_sq_param;
460 	struct mlx5e_txqsq *txqsq = &ptpsq->txqsq;
461 	struct mlx5e_create_sq_param csp = {};
462 	int err;
463 
464 	err = mlx5e_ptp_alloc_txqsq(c, txq_ix, &cparams->params, sqp,
465 				    txqsq, tc, ptpsq);
466 	if (err)
467 		return err;
468 
469 	csp.tisn            = tisn;
470 	csp.tis_lst_sz      = 1;
471 	csp.cqn             = txqsq->cq.mcq.cqn;
472 	csp.wq_ctrl         = &txqsq->wq_ctrl;
473 	csp.min_inline_mode = txqsq->min_inline_mode;
474 	csp.ts_cqe_to_dest_cqn = ptpsq->ts_cq.mcq.cqn;
475 
476 	err = mlx5e_create_sq_rdy(c->mdev, sqp, &csp, 0, &txqsq->sqn);
477 	if (err)
478 		goto err_free_txqsq;
479 
480 	err = mlx5e_ptp_alloc_traffic_db(ptpsq, dev_to_node(mlx5_core_dma_dev(c->mdev)));
481 	if (err)
482 		goto err_free_txqsq;
483 
484 	INIT_WORK(&ptpsq->report_unhealthy_work, mlx5e_ptpsq_unhealthy_work);
485 
486 	return 0;
487 
488 err_free_txqsq:
489 	mlx5e_free_txqsq(txqsq);
490 
491 	return err;
492 }
493 
mlx5e_ptp_close_txqsq(struct mlx5e_ptpsq * ptpsq)494 static void mlx5e_ptp_close_txqsq(struct mlx5e_ptpsq *ptpsq)
495 {
496 	struct mlx5e_txqsq *sq = &ptpsq->txqsq;
497 	struct mlx5_core_dev *mdev = sq->mdev;
498 
499 	if (current_work() != &ptpsq->report_unhealthy_work)
500 		cancel_work_sync(&ptpsq->report_unhealthy_work);
501 	mlx5e_ptp_free_traffic_db(ptpsq);
502 	cancel_work_sync(&sq->recover_work);
503 	mlx5e_ptp_destroy_sq(mdev, sq->sqn);
504 	mlx5e_free_txqsq_descs(sq);
505 	mlx5e_free_txqsq(sq);
506 }
507 
mlx5e_ptp_open_txqsqs(struct mlx5e_ptp * c,struct mlx5e_ptp_params * cparams)508 static int mlx5e_ptp_open_txqsqs(struct mlx5e_ptp *c,
509 				 struct mlx5e_ptp_params *cparams)
510 {
511 	struct mlx5e_params *params = &cparams->params;
512 	u8 num_tc = mlx5e_get_dcb_num_tc(params);
513 	int ix_base;
514 	int err;
515 	int tc;
516 
517 	ix_base = num_tc * params->num_channels;
518 
519 	for (tc = 0; tc < num_tc; tc++) {
520 		int txq_ix = ix_base + tc;
521 
522 		err = mlx5e_ptp_open_txqsq(c, c->priv->tisn[c->lag_port][tc], txq_ix,
523 					   cparams, tc, &c->ptpsq[tc]);
524 		if (err)
525 			goto close_txqsq;
526 	}
527 
528 	return 0;
529 
530 close_txqsq:
531 	for (--tc; tc >= 0; tc--)
532 		mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
533 
534 	return err;
535 }
536 
mlx5e_ptp_close_txqsqs(struct mlx5e_ptp * c)537 static void mlx5e_ptp_close_txqsqs(struct mlx5e_ptp *c)
538 {
539 	int tc;
540 
541 	for (tc = 0; tc < c->num_tc; tc++)
542 		mlx5e_ptp_close_txqsq(&c->ptpsq[tc]);
543 }
544 
mlx5e_ptp_open_tx_cqs(struct mlx5e_ptp * c,struct mlx5e_ptp_params * cparams)545 static int mlx5e_ptp_open_tx_cqs(struct mlx5e_ptp *c,
546 				 struct mlx5e_ptp_params *cparams)
547 {
548 	struct mlx5e_params *params = &cparams->params;
549 	struct mlx5e_create_cq_param ccp = {};
550 	struct dim_cq_moder ptp_moder = {};
551 	struct mlx5e_cq_param *cq_param;
552 	u8 num_tc;
553 	int err;
554 	int tc;
555 
556 	num_tc = mlx5e_get_dcb_num_tc(params);
557 
558 	ccp.node     = dev_to_node(mlx5_core_dma_dev(c->mdev));
559 	ccp.ch_stats = c->stats;
560 	ccp.napi     = &c->napi;
561 	ccp.ix       = MLX5E_PTP_CHANNEL_IX;
562 
563 	cq_param = &cparams->txq_sq_param.cqp;
564 
565 	for (tc = 0; tc < num_tc; tc++) {
566 		struct mlx5e_cq *cq = &c->ptpsq[tc].txqsq.cq;
567 
568 		err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
569 		if (err)
570 			goto out_err_txqsq_cq;
571 	}
572 
573 	for (tc = 0; tc < num_tc; tc++) {
574 		struct mlx5e_cq *cq = &c->ptpsq[tc].ts_cq;
575 		struct mlx5e_ptpsq *ptpsq = &c->ptpsq[tc];
576 
577 		err = mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
578 		if (err)
579 			goto out_err_ts_cq;
580 
581 		ptpsq->cq_stats = &c->priv->ptp_stats.cq[tc];
582 	}
583 
584 	return 0;
585 
586 out_err_ts_cq:
587 	for (--tc; tc >= 0; tc--)
588 		mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
589 	tc = num_tc;
590 out_err_txqsq_cq:
591 	for (--tc; tc >= 0; tc--)
592 		mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
593 
594 	return err;
595 }
596 
mlx5e_ptp_open_rx_cq(struct mlx5e_ptp * c,struct mlx5e_ptp_params * cparams)597 static int mlx5e_ptp_open_rx_cq(struct mlx5e_ptp *c,
598 				struct mlx5e_ptp_params *cparams)
599 {
600 	struct mlx5e_create_cq_param ccp = {};
601 	struct dim_cq_moder ptp_moder = {};
602 	struct mlx5e_cq_param *cq_param;
603 	struct mlx5e_cq *cq = &c->rq.cq;
604 
605 	ccp.node     = dev_to_node(mlx5_core_dma_dev(c->mdev));
606 	ccp.ch_stats = c->stats;
607 	ccp.napi     = &c->napi;
608 	ccp.ix       = MLX5E_PTP_CHANNEL_IX;
609 
610 	cq_param = &cparams->rq_param.cqp;
611 
612 	return mlx5e_open_cq(c->priv, ptp_moder, cq_param, &ccp, cq);
613 }
614 
mlx5e_ptp_close_tx_cqs(struct mlx5e_ptp * c)615 static void mlx5e_ptp_close_tx_cqs(struct mlx5e_ptp *c)
616 {
617 	int tc;
618 
619 	for (tc = 0; tc < c->num_tc; tc++)
620 		mlx5e_close_cq(&c->ptpsq[tc].ts_cq);
621 
622 	for (tc = 0; tc < c->num_tc; tc++)
623 		mlx5e_close_cq(&c->ptpsq[tc].txqsq.cq);
624 }
625 
mlx5e_ptp_build_sq_param(struct mlx5_core_dev * mdev,struct mlx5e_params * params,struct mlx5e_sq_param * param)626 static void mlx5e_ptp_build_sq_param(struct mlx5_core_dev *mdev,
627 				     struct mlx5e_params *params,
628 				     struct mlx5e_sq_param *param)
629 {
630 	void *sqc = param->sqc;
631 	void *wq;
632 
633 	mlx5e_build_sq_param_common(mdev, param);
634 
635 	wq = MLX5_ADDR_OF(sqc, sqc, wq);
636 	MLX5_SET(wq, wq, log_wq_sz, params->log_sq_size);
637 	param->stop_room = mlx5e_stop_room_for_max_wqe(mdev);
638 	mlx5e_build_tx_cq_param(mdev, params, &param->cqp);
639 }
640 
mlx5e_ptp_build_rq_param(struct mlx5_core_dev * mdev,struct net_device * netdev,u16 q_counter,struct mlx5e_ptp_params * ptp_params)641 static void mlx5e_ptp_build_rq_param(struct mlx5_core_dev *mdev,
642 				     struct net_device *netdev,
643 				     u16 q_counter,
644 				     struct mlx5e_ptp_params *ptp_params)
645 {
646 	struct mlx5e_rq_param *rq_params = &ptp_params->rq_param;
647 	struct mlx5e_params *params = &ptp_params->params;
648 
649 	params->rq_wq_type = MLX5_WQ_TYPE_CYCLIC;
650 	mlx5e_init_rq_type_params(mdev, params);
651 	params->sw_mtu = netdev->max_mtu;
652 	mlx5e_build_rq_param(mdev, params, NULL, q_counter, rq_params);
653 }
654 
mlx5e_ptp_build_params(struct mlx5e_ptp * c,struct mlx5e_ptp_params * cparams,struct mlx5e_params * orig)655 static void mlx5e_ptp_build_params(struct mlx5e_ptp *c,
656 				   struct mlx5e_ptp_params *cparams,
657 				   struct mlx5e_params *orig)
658 {
659 	struct mlx5e_params *params = &cparams->params;
660 
661 	params->tx_min_inline_mode = orig->tx_min_inline_mode;
662 	params->num_channels = orig->num_channels;
663 	params->hard_mtu = orig->hard_mtu;
664 	params->sw_mtu = orig->sw_mtu;
665 	params->mqprio = orig->mqprio;
666 
667 	/* SQ */
668 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
669 		params->log_sq_size =
670 			min(MLX5_CAP_GEN_2(c->mdev, ts_cqe_metadata_size2wqe_counter),
671 			    MLX5E_PTP_MAX_LOG_SQ_SIZE);
672 		params->log_sq_size = min(params->log_sq_size, orig->log_sq_size);
673 		mlx5e_ptp_build_sq_param(c->mdev, params, &cparams->txq_sq_param);
674 	}
675 	/* RQ */
676 	if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
677 		params->vlan_strip_disable = orig->vlan_strip_disable;
678 		mlx5e_ptp_build_rq_param(c->mdev, c->netdev, c->priv->q_counter, cparams);
679 	}
680 }
681 
mlx5e_init_ptp_rq(struct mlx5e_ptp * c,struct mlx5e_params * params,struct mlx5e_rq * rq)682 static int mlx5e_init_ptp_rq(struct mlx5e_ptp *c, struct mlx5e_params *params,
683 			     struct mlx5e_rq *rq)
684 {
685 	struct mlx5_core_dev *mdev = c->mdev;
686 	struct mlx5e_priv *priv = c->priv;
687 	int err;
688 
689 	rq->wq_type      = params->rq_wq_type;
690 	rq->pdev         = c->pdev;
691 	rq->netdev       = priv->netdev;
692 	rq->priv         = priv;
693 	rq->clock        = &mdev->clock;
694 	rq->tstamp       = &priv->tstamp;
695 	rq->mdev         = mdev;
696 	rq->hw_mtu       = MLX5E_SW2HW_MTU(params, params->sw_mtu);
697 	rq->stats        = &c->priv->ptp_stats.rq;
698 	rq->ix           = MLX5E_PTP_CHANNEL_IX;
699 	rq->ptp_cyc2time = mlx5_rq_ts_translator(mdev);
700 	err = mlx5e_rq_set_handlers(rq, params, false);
701 	if (err)
702 		return err;
703 
704 	return xdp_rxq_info_reg(&rq->xdp_rxq, rq->netdev, rq->ix, 0);
705 }
706 
mlx5e_ptp_open_rq(struct mlx5e_ptp * c,struct mlx5e_params * params,struct mlx5e_rq_param * rq_param)707 static int mlx5e_ptp_open_rq(struct mlx5e_ptp *c, struct mlx5e_params *params,
708 			     struct mlx5e_rq_param *rq_param)
709 {
710 	int node = dev_to_node(c->mdev->device);
711 	int err;
712 
713 	err = mlx5e_init_ptp_rq(c, params, &c->rq);
714 	if (err)
715 		return err;
716 
717 	return mlx5e_open_rq(params, rq_param, NULL, node, &c->rq);
718 }
719 
mlx5e_ptp_open_queues(struct mlx5e_ptp * c,struct mlx5e_ptp_params * cparams)720 static int mlx5e_ptp_open_queues(struct mlx5e_ptp *c,
721 				 struct mlx5e_ptp_params *cparams)
722 {
723 	int err;
724 
725 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
726 		err = mlx5e_ptp_open_tx_cqs(c, cparams);
727 		if (err)
728 			return err;
729 
730 		err = mlx5e_ptp_open_txqsqs(c, cparams);
731 		if (err)
732 			goto close_tx_cqs;
733 	}
734 	if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
735 		err = mlx5e_ptp_open_rx_cq(c, cparams);
736 		if (err)
737 			goto close_txqsq;
738 
739 		err = mlx5e_ptp_open_rq(c, &cparams->params, &cparams->rq_param);
740 		if (err)
741 			goto close_rx_cq;
742 	}
743 	return 0;
744 
745 close_rx_cq:
746 	if (test_bit(MLX5E_PTP_STATE_RX, c->state))
747 		mlx5e_close_cq(&c->rq.cq);
748 close_txqsq:
749 	if (test_bit(MLX5E_PTP_STATE_TX, c->state))
750 		mlx5e_ptp_close_txqsqs(c);
751 close_tx_cqs:
752 	if (test_bit(MLX5E_PTP_STATE_TX, c->state))
753 		mlx5e_ptp_close_tx_cqs(c);
754 
755 	return err;
756 }
757 
mlx5e_ptp_close_queues(struct mlx5e_ptp * c)758 static void mlx5e_ptp_close_queues(struct mlx5e_ptp *c)
759 {
760 	if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
761 		mlx5e_close_rq(&c->rq);
762 		mlx5e_close_cq(&c->rq.cq);
763 	}
764 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
765 		mlx5e_ptp_close_txqsqs(c);
766 		mlx5e_ptp_close_tx_cqs(c);
767 	}
768 }
769 
mlx5e_ptp_set_state(struct mlx5e_ptp * c,struct mlx5e_params * params)770 static int mlx5e_ptp_set_state(struct mlx5e_ptp *c, struct mlx5e_params *params)
771 {
772 	if (MLX5E_GET_PFLAG(params, MLX5E_PFLAG_TX_PORT_TS))
773 		__set_bit(MLX5E_PTP_STATE_TX, c->state);
774 
775 	if (params->ptp_rx)
776 		__set_bit(MLX5E_PTP_STATE_RX, c->state);
777 
778 	return bitmap_empty(c->state, MLX5E_PTP_STATE_NUM_STATES) ? -EINVAL : 0;
779 }
780 
mlx5e_ptp_rx_unset_fs(struct mlx5e_flow_steering * fs)781 static void mlx5e_ptp_rx_unset_fs(struct mlx5e_flow_steering *fs)
782 {
783 	struct mlx5e_ptp_fs *ptp_fs = mlx5e_fs_get_ptp(fs);
784 
785 	if (!ptp_fs->valid)
786 		return;
787 
788 	mlx5e_fs_tt_redirect_del_rule(ptp_fs->l2_rule);
789 	mlx5e_fs_tt_redirect_any_destroy(fs);
790 
791 	mlx5e_fs_tt_redirect_del_rule(ptp_fs->udp_v6_rule);
792 	mlx5e_fs_tt_redirect_del_rule(ptp_fs->udp_v4_rule);
793 	mlx5e_fs_tt_redirect_udp_destroy(fs);
794 	ptp_fs->valid = false;
795 }
796 
mlx5e_ptp_rx_set_fs(struct mlx5e_priv * priv)797 static int mlx5e_ptp_rx_set_fs(struct mlx5e_priv *priv)
798 {
799 	u32 tirn = mlx5e_rx_res_get_tirn_ptp(priv->rx_res);
800 	struct mlx5e_flow_steering *fs = priv->fs;
801 	struct mlx5_flow_handle *rule;
802 	struct mlx5e_ptp_fs *ptp_fs;
803 	int err;
804 
805 	ptp_fs = mlx5e_fs_get_ptp(fs);
806 	if (ptp_fs->valid)
807 		return 0;
808 
809 	err = mlx5e_fs_tt_redirect_udp_create(fs);
810 	if (err)
811 		goto out_free;
812 
813 	rule = mlx5e_fs_tt_redirect_udp_add_rule(fs, MLX5_TT_IPV4_UDP,
814 						 tirn, PTP_EV_PORT);
815 	if (IS_ERR(rule)) {
816 		err = PTR_ERR(rule);
817 		goto out_destroy_fs_udp;
818 	}
819 	ptp_fs->udp_v4_rule = rule;
820 
821 	rule = mlx5e_fs_tt_redirect_udp_add_rule(fs, MLX5_TT_IPV6_UDP,
822 						 tirn, PTP_EV_PORT);
823 	if (IS_ERR(rule)) {
824 		err = PTR_ERR(rule);
825 		goto out_destroy_udp_v4_rule;
826 	}
827 	ptp_fs->udp_v6_rule = rule;
828 
829 	err = mlx5e_fs_tt_redirect_any_create(fs);
830 	if (err)
831 		goto out_destroy_udp_v6_rule;
832 
833 	rule = mlx5e_fs_tt_redirect_any_add_rule(fs, tirn, ETH_P_1588);
834 	if (IS_ERR(rule)) {
835 		err = PTR_ERR(rule);
836 		goto out_destroy_fs_any;
837 	}
838 	ptp_fs->l2_rule = rule;
839 	ptp_fs->valid = true;
840 
841 	return 0;
842 
843 out_destroy_fs_any:
844 	mlx5e_fs_tt_redirect_any_destroy(fs);
845 out_destroy_udp_v6_rule:
846 	mlx5e_fs_tt_redirect_del_rule(ptp_fs->udp_v6_rule);
847 out_destroy_udp_v4_rule:
848 	mlx5e_fs_tt_redirect_del_rule(ptp_fs->udp_v4_rule);
849 out_destroy_fs_udp:
850 	mlx5e_fs_tt_redirect_udp_destroy(fs);
851 out_free:
852 	return err;
853 }
854 
mlx5e_ptp_open(struct mlx5e_priv * priv,struct mlx5e_params * params,u8 lag_port,struct mlx5e_ptp ** cp)855 int mlx5e_ptp_open(struct mlx5e_priv *priv, struct mlx5e_params *params,
856 		   u8 lag_port, struct mlx5e_ptp **cp)
857 {
858 	struct net_device *netdev = priv->netdev;
859 	struct mlx5_core_dev *mdev = priv->mdev;
860 	struct mlx5e_ptp_params *cparams;
861 	struct mlx5e_ptp *c;
862 	int err;
863 
864 
865 	c = kvzalloc_node(sizeof(*c), GFP_KERNEL, dev_to_node(mlx5_core_dma_dev(mdev)));
866 	cparams = kvzalloc(sizeof(*cparams), GFP_KERNEL);
867 	if (!c || !cparams) {
868 		err = -ENOMEM;
869 		goto err_free;
870 	}
871 
872 	c->priv     = priv;
873 	c->mdev     = priv->mdev;
874 	c->tstamp   = &priv->tstamp;
875 	c->pdev     = mlx5_core_dma_dev(priv->mdev);
876 	c->netdev   = priv->netdev;
877 	c->mkey_be  = cpu_to_be32(priv->mdev->mlx5e_res.hw_objs.mkey);
878 	c->num_tc   = mlx5e_get_dcb_num_tc(params);
879 	c->stats    = &priv->ptp_stats.ch;
880 	c->lag_port = lag_port;
881 
882 	err = mlx5e_ptp_set_state(c, params);
883 	if (err)
884 		goto err_free;
885 
886 	netif_napi_add(netdev, &c->napi, mlx5e_ptp_napi_poll);
887 
888 	mlx5e_ptp_build_params(c, cparams, params);
889 
890 	err = mlx5e_ptp_open_queues(c, cparams);
891 	if (unlikely(err))
892 		goto err_napi_del;
893 
894 	if (test_bit(MLX5E_PTP_STATE_RX, c->state))
895 		priv->rx_ptp_opened = true;
896 
897 	*cp = c;
898 
899 	kvfree(cparams);
900 
901 	return 0;
902 
903 err_napi_del:
904 	netif_napi_del(&c->napi);
905 err_free:
906 	kvfree(cparams);
907 	kvfree(c);
908 	return err;
909 }
910 
mlx5e_ptp_close(struct mlx5e_ptp * c)911 void mlx5e_ptp_close(struct mlx5e_ptp *c)
912 {
913 	mlx5e_ptp_close_queues(c);
914 	netif_napi_del(&c->napi);
915 
916 	kvfree(c);
917 }
918 
mlx5e_ptp_activate_channel(struct mlx5e_ptp * c)919 void mlx5e_ptp_activate_channel(struct mlx5e_ptp *c)
920 {
921 	int tc;
922 
923 	napi_enable(&c->napi);
924 
925 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
926 		for (tc = 0; tc < c->num_tc; tc++)
927 			mlx5e_activate_txqsq(&c->ptpsq[tc].txqsq);
928 	}
929 	if (test_bit(MLX5E_PTP_STATE_RX, c->state)) {
930 		mlx5e_ptp_rx_set_fs(c->priv);
931 		mlx5e_activate_rq(&c->rq);
932 	}
933 	mlx5e_trigger_napi_sched(&c->napi);
934 }
935 
mlx5e_ptp_deactivate_channel(struct mlx5e_ptp * c)936 void mlx5e_ptp_deactivate_channel(struct mlx5e_ptp *c)
937 {
938 	int tc;
939 
940 	if (test_bit(MLX5E_PTP_STATE_RX, c->state))
941 		mlx5e_deactivate_rq(&c->rq);
942 
943 	if (test_bit(MLX5E_PTP_STATE_TX, c->state)) {
944 		for (tc = 0; tc < c->num_tc; tc++)
945 			mlx5e_deactivate_txqsq(&c->ptpsq[tc].txqsq);
946 	}
947 
948 	napi_disable(&c->napi);
949 }
950 
mlx5e_ptp_get_rqn(struct mlx5e_ptp * c,u32 * rqn)951 int mlx5e_ptp_get_rqn(struct mlx5e_ptp *c, u32 *rqn)
952 {
953 	if (!c || !test_bit(MLX5E_PTP_STATE_RX, c->state))
954 		return -EINVAL;
955 
956 	*rqn = c->rq.rqn;
957 	return 0;
958 }
959 
mlx5e_ptp_alloc_rx_fs(struct mlx5e_flow_steering * fs,const struct mlx5e_profile * profile)960 int mlx5e_ptp_alloc_rx_fs(struct mlx5e_flow_steering *fs,
961 			  const struct mlx5e_profile *profile)
962 {
963 	struct mlx5e_ptp_fs *ptp_fs;
964 
965 	if (!mlx5e_profile_feature_cap(profile, PTP_RX))
966 		return 0;
967 
968 	ptp_fs = kzalloc(sizeof(*ptp_fs), GFP_KERNEL);
969 	if (!ptp_fs)
970 		return -ENOMEM;
971 	mlx5e_fs_set_ptp(fs, ptp_fs);
972 
973 	return 0;
974 }
975 
mlx5e_ptp_free_rx_fs(struct mlx5e_flow_steering * fs,const struct mlx5e_profile * profile)976 void mlx5e_ptp_free_rx_fs(struct mlx5e_flow_steering *fs,
977 			  const struct mlx5e_profile *profile)
978 {
979 	struct mlx5e_ptp_fs *ptp_fs = mlx5e_fs_get_ptp(fs);
980 
981 	if (!mlx5e_profile_feature_cap(profile, PTP_RX))
982 		return;
983 
984 	mlx5e_ptp_rx_unset_fs(fs);
985 	kfree(ptp_fs);
986 }
987 
mlx5e_ptp_rx_manage_fs(struct mlx5e_priv * priv,bool set)988 int mlx5e_ptp_rx_manage_fs(struct mlx5e_priv *priv, bool set)
989 {
990 	struct mlx5e_ptp *c = priv->channels.ptp;
991 
992 	if (!mlx5e_profile_feature_cap(priv->profile, PTP_RX))
993 		return 0;
994 
995 	if (!test_bit(MLX5E_STATE_OPENED, &priv->state))
996 		return 0;
997 
998 	if (set) {
999 		if (!c || !test_bit(MLX5E_PTP_STATE_RX, c->state)) {
1000 			netdev_WARN_ONCE(priv->netdev, "Don't try to add PTP RX-FS rules");
1001 			return -EINVAL;
1002 		}
1003 		return mlx5e_ptp_rx_set_fs(priv);
1004 	}
1005 	/* set == false */
1006 	if (c && test_bit(MLX5E_PTP_STATE_RX, c->state)) {
1007 		netdev_WARN_ONCE(priv->netdev, "Don't try to remove PTP RX-FS rules");
1008 		return -EINVAL;
1009 	}
1010 	mlx5e_ptp_rx_unset_fs(priv->fs);
1011 	return 0;
1012 }
1013