1	// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2	/* Copyright (c) 2017-2018 Mellanox Technologies. All rights reserved */
3
4	#include <linux/kernel.h>
5	#include <linux/errno.h>
6	#include <linux/netdevice.h>
7	#include <net/pkt_cls.h>
8	#include <net/red.h>
9
10	#include "spectrum.h"
11	#include "spectrum_span.h"
12	#include "reg.h"
13
14	#define MLXSW_SP_PRIO_BAND_TO_TCLASS(band) (IEEE_8021QAZ_MAX_TCS - band - 1)
15	#define MLXSW_SP_PRIO_CHILD_TO_TCLASS(child) \
16	MLXSW_SP_PRIO_BAND_TO_TCLASS((child - 1))
17
18	enum mlxsw_sp_qdisc_type {
19	MLXSW_SP_QDISC_NO_QDISC,
20	MLXSW_SP_QDISC_RED,
21	MLXSW_SP_QDISC_PRIO,
22	MLXSW_SP_QDISC_ETS,
23	MLXSW_SP_QDISC_TBF,
24	MLXSW_SP_QDISC_FIFO,
25	};
26
27	struct mlxsw_sp_qdisc;
28
29	struct mlxsw_sp_qdisc_ops {
30	enum mlxsw_sp_qdisc_type type;
31	int (*check_params)(struct mlxsw_sp_port *mlxsw_sp_port,
32	void *params);
33	int (*replace)(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
34	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params);
35	int (*destroy)(struct mlxsw_sp_port *mlxsw_sp_port,
36	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc);
37	int (*get_stats)(struct mlxsw_sp_port *mlxsw_sp_port,
38	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
39	struct tc_qopt_offload_stats *stats_ptr);
40	int (*get_xstats)(struct mlxsw_sp_port *mlxsw_sp_port,
41	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
42	void *xstats_ptr);
43	void (*clean_stats)(struct mlxsw_sp_port *mlxsw_sp_port,
44	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc);
45	/* unoffload - to be used for a qdisc that stops being offloaded without
46	* being destroyed.
47	*/
48	void (*unoffload)(struct mlxsw_sp_port *mlxsw_sp_port,
49	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params);
50	struct mlxsw_sp_qdisc *(*find_class)(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
51	u32 parent);
52	unsigned int num_classes;
53
54	u8 (*get_prio_bitmap)(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
55	struct mlxsw_sp_qdisc *child);
56	int (*get_tclass_num)(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
57	struct mlxsw_sp_qdisc *child);
58	};
59
60	struct mlxsw_sp_qdisc_ets_band {
61	u8 prio_bitmap;
62	int tclass_num;
63	};
64
65	struct mlxsw_sp_qdisc_ets_data {
66	struct mlxsw_sp_qdisc_ets_band bands[IEEE_8021QAZ_MAX_TCS];
67	};
68
69	struct mlxsw_sp_qdisc {
70	u32 handle;
71	union {
72	struct red_stats red;
73	} xstats_base;
74	struct mlxsw_sp_qdisc_stats {
75	u64 tx_bytes;
76	u64 tx_packets;
77	u64 drops;
78	u64 overlimits;
79	u64 backlog;
80	} stats_base;
81
82	union {
83	struct mlxsw_sp_qdisc_ets_data *ets_data;
84	};
85
86	struct mlxsw_sp_qdisc_ops *ops;
87	struct mlxsw_sp_qdisc *parent;
88	struct mlxsw_sp_qdisc *qdiscs;
89	unsigned int num_classes;
90	};
91
92	struct mlxsw_sp_qdisc_state {
93	struct mlxsw_sp_qdisc root_qdisc;
94
95	/* When a PRIO or ETS are added, the invisible FIFOs in their bands are
96	* created first. When notifications for these FIFOs arrive, it is not
97	* known what qdisc their parent handle refers to. It could be a
98	* newly-created PRIO that will replace the currently-offloaded one, or
99	* it could be e.g. a RED that will be attached below it.
100	*
101	* As the notifications start to arrive, use them to note what the
102	* future parent handle is, and keep track of which child FIFOs were
103	* seen. Then when the parent is known, retroactively offload those
104	* FIFOs.
105	*/
106	u32 future_handle;
107	bool future_fifos[IEEE_8021QAZ_MAX_TCS];
108	struct mutex lock; /* Protects qdisc state. */
109	};
110
111	static bool
112	mlxsw_sp_qdisc_compare(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, u32 handle)
113	{
114	return mlxsw_sp_qdisc->ops && mlxsw_sp_qdisc->handle == handle;
115	}
116
117	static struct mlxsw_sp_qdisc *
118	mlxsw_sp_qdisc_walk(struct mlxsw_sp_qdisc *qdisc,
119	struct mlxsw_sp_qdisc *(*pre)(struct mlxsw_sp_qdisc *,
120	void *),
121	void *data)
122	{
123	struct mlxsw_sp_qdisc *tmp;
124	unsigned int i;
125
126	if (pre) {
127	tmp = pre(qdisc, data);
128	if (tmp)
129	return tmp;
130	}
131
132	if (qdisc->ops) {
133	for (i = 0; i < qdisc->num_classes; i++) {
134	tmp = &qdisc->qdiscs[i];
135	if (qdisc->ops) {
136	tmp = mlxsw_sp_qdisc_walk(qdisc: tmp, pre, data);
137	if (tmp)
138	return tmp;
139	}
140	}
141	}
142
143	return NULL;
144	}
145
146	static struct mlxsw_sp_qdisc *
147	mlxsw_sp_qdisc_walk_cb_find(struct mlxsw_sp_qdisc *qdisc, void *data)
148	{
149	u32 parent = *(u32 *)data;
150
151	if (qdisc->ops && TC_H_MAJ(qdisc->handle) == TC_H_MAJ(parent)) {
152	if (qdisc->ops->find_class)
153	return qdisc->ops->find_class(qdisc, parent);
154	}
155
156	return NULL;
157	}
158
159	static struct mlxsw_sp_qdisc *
160	mlxsw_sp_qdisc_find(struct mlxsw_sp_port *mlxsw_sp_port, u32 parent)
161	{
162	struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
163
164	if (!qdisc_state)
165	return NULL;
166	if (parent == TC_H_ROOT)
167	return &qdisc_state->root_qdisc;
168	return mlxsw_sp_qdisc_walk(qdisc: &qdisc_state->root_qdisc,
169	pre: mlxsw_sp_qdisc_walk_cb_find, data: &parent);
170	}
171
172	static struct mlxsw_sp_qdisc *
173	mlxsw_sp_qdisc_walk_cb_find_by_handle(struct mlxsw_sp_qdisc *qdisc, void *data)
174	{
175	u32 handle = *(u32 *)data;
176
177	if (qdisc->ops && qdisc->handle == handle)
178	return qdisc;
179	return NULL;
180	}
181
182	static struct mlxsw_sp_qdisc *
183	mlxsw_sp_qdisc_find_by_handle(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle)
184	{
185	struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
186
187	if (!qdisc_state)
188	return NULL;
189	return mlxsw_sp_qdisc_walk(qdisc: &qdisc_state->root_qdisc,
190	pre: mlxsw_sp_qdisc_walk_cb_find_by_handle,
191	data: &handle);
192	}
193
194	static void
195	mlxsw_sp_qdisc_reduce_parent_backlog(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
196	{
197	struct mlxsw_sp_qdisc *tmp;
198
199	for (tmp = mlxsw_sp_qdisc->parent; tmp; tmp = tmp->parent)
200	tmp->stats_base.backlog -= mlxsw_sp_qdisc->stats_base.backlog;
201	}
202
203	static u8 mlxsw_sp_qdisc_get_prio_bitmap(struct mlxsw_sp_port *mlxsw_sp_port,
204	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
205	{
206	struct mlxsw_sp_qdisc *parent = mlxsw_sp_qdisc->parent;
207
208	if (!parent)
209	return 0xff;
210	if (!parent->ops->get_prio_bitmap)
211	return mlxsw_sp_qdisc_get_prio_bitmap(mlxsw_sp_port, mlxsw_sp_qdisc: parent);
212	return parent->ops->get_prio_bitmap(parent, mlxsw_sp_qdisc);
213	}
214
215	#define MLXSW_SP_PORT_DEFAULT_TCLASS 0
216
217	static int mlxsw_sp_qdisc_get_tclass_num(struct mlxsw_sp_port *mlxsw_sp_port,
218	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
219	{
220	struct mlxsw_sp_qdisc *parent = mlxsw_sp_qdisc->parent;
221
222	if (!parent)
223	return MLXSW_SP_PORT_DEFAULT_TCLASS;
224	if (!parent->ops->get_tclass_num)
225	return mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port, mlxsw_sp_qdisc: parent);
226	return parent->ops->get_tclass_num(parent, mlxsw_sp_qdisc);
227	}
228
229	static int
230	mlxsw_sp_qdisc_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
231	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
232	{
233	struct mlxsw_sp_qdisc *root_qdisc = &mlxsw_sp_port->qdisc->root_qdisc;
234	int err_hdroom = 0;
235	int err = 0;
236	int i;
237
238	if (!mlxsw_sp_qdisc)
239	return 0;
240
241	if (root_qdisc == mlxsw_sp_qdisc) {
242	struct mlxsw_sp_hdroom hdroom = *mlxsw_sp_port->hdroom;
243
244	hdroom.mode = MLXSW_SP_HDROOM_MODE_DCB;
245	mlxsw_sp_hdroom_prios_reset_buf_idx(hdroom: &hdroom);
246	mlxsw_sp_hdroom_bufs_reset_lossiness(hdroom: &hdroom);
247	mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, hdroom: &hdroom);
248	err_hdroom = mlxsw_sp_hdroom_configure(mlxsw_sp_port, hdroom: &hdroom);
249	}
250
251	if (!mlxsw_sp_qdisc->ops)
252	return 0;
253
254	for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++)
255	mlxsw_sp_qdisc_destroy(mlxsw_sp_port,
256	mlxsw_sp_qdisc: &mlxsw_sp_qdisc->qdiscs[i]);
257	mlxsw_sp_qdisc_reduce_parent_backlog(mlxsw_sp_qdisc);
258	if (mlxsw_sp_qdisc->ops->destroy)
259	err = mlxsw_sp_qdisc->ops->destroy(mlxsw_sp_port,
260	mlxsw_sp_qdisc);
261	if (mlxsw_sp_qdisc->ops->clean_stats)
262	mlxsw_sp_qdisc->ops->clean_stats(mlxsw_sp_port, mlxsw_sp_qdisc);
263
264	mlxsw_sp_qdisc->handle = TC_H_UNSPEC;
265	mlxsw_sp_qdisc->ops = NULL;
266	mlxsw_sp_qdisc->num_classes = 0;
267	kfree(objp: mlxsw_sp_qdisc->qdiscs);
268	mlxsw_sp_qdisc->qdiscs = NULL;
269	return err_hdroom ?: err;
270	}
271
272	struct mlxsw_sp_qdisc_tree_validate {
273	bool forbid_ets;
274	bool forbid_root_tbf;
275	bool forbid_tbf;
276	bool forbid_red;
277	};
278
279	static int
280	__mlxsw_sp_qdisc_tree_validate(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
281	struct mlxsw_sp_qdisc_tree_validate validate);
282
283	static int
284	mlxsw_sp_qdisc_tree_validate_children(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
285	struct mlxsw_sp_qdisc_tree_validate validate)
286	{
287	unsigned int i;
288	int err;
289
290	for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++) {
291	err = __mlxsw_sp_qdisc_tree_validate(mlxsw_sp_qdisc: &mlxsw_sp_qdisc->qdiscs[i],
292	validate);
293	if (err)
294	return err;
295	}
296
297	return 0;
298	}
299
300	static int
301	__mlxsw_sp_qdisc_tree_validate(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
302	struct mlxsw_sp_qdisc_tree_validate validate)
303	{
304	if (!mlxsw_sp_qdisc->ops)
305	return 0;
306
307	switch (mlxsw_sp_qdisc->ops->type) {
308	case MLXSW_SP_QDISC_FIFO:
309	break;
310	case MLXSW_SP_QDISC_RED:
311	if (validate.forbid_red)
312	return -EINVAL;
313	validate.forbid_red = true;
314	validate.forbid_root_tbf = true;
315	validate.forbid_ets = true;
316	break;
317	case MLXSW_SP_QDISC_TBF:
318	if (validate.forbid_root_tbf) {
319	if (validate.forbid_tbf)
320	return -EINVAL;
321	/* This is a TC TBF. */
322	validate.forbid_tbf = true;
323	validate.forbid_ets = true;
324	} else {
325	/* This is root TBF. */
326	validate.forbid_root_tbf = true;
327	}
328	break;
329	case MLXSW_SP_QDISC_PRIO:
330	case MLXSW_SP_QDISC_ETS:
331	if (validate.forbid_ets)
332	return -EINVAL;
333	validate.forbid_root_tbf = true;
334	validate.forbid_ets = true;
335	break;
336	default:
337	WARN_ON(1);
338	return -EINVAL;
339	}
340
341	return mlxsw_sp_qdisc_tree_validate_children(mlxsw_sp_qdisc, validate);
342	}
343
344	static int mlxsw_sp_qdisc_tree_validate(struct mlxsw_sp_port *mlxsw_sp_port)
345	{
346	struct mlxsw_sp_qdisc_tree_validate validate = {};
347	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
348
349	mlxsw_sp_qdisc = &mlxsw_sp_port->qdisc->root_qdisc;
350	return __mlxsw_sp_qdisc_tree_validate(mlxsw_sp_qdisc, validate);
351	}
352
353	static int mlxsw_sp_qdisc_create(struct mlxsw_sp_port *mlxsw_sp_port,
354	u32 handle,
355	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
356	struct mlxsw_sp_qdisc_ops *ops, void *params)
357	{
358	struct mlxsw_sp_qdisc *root_qdisc = &mlxsw_sp_port->qdisc->root_qdisc;
359	struct mlxsw_sp_hdroom orig_hdroom;
360	unsigned int i;
361	int err;
362
363	err = ops->check_params(mlxsw_sp_port, params);
364	if (err)
365	return err;
366
367	if (ops->num_classes) {
368	mlxsw_sp_qdisc->qdiscs = kcalloc(n: ops->num_classes,
369	size: sizeof(*mlxsw_sp_qdisc->qdiscs),
370	GFP_KERNEL);
371	if (!mlxsw_sp_qdisc->qdiscs)
372	return -ENOMEM;
373
374	for (i = 0; i < ops->num_classes; i++)
375	mlxsw_sp_qdisc->qdiscs[i].parent = mlxsw_sp_qdisc;
376	}
377
378	orig_hdroom = *mlxsw_sp_port->hdroom;
379	if (root_qdisc == mlxsw_sp_qdisc) {
380	struct mlxsw_sp_hdroom hdroom = orig_hdroom;
381
382	hdroom.mode = MLXSW_SP_HDROOM_MODE_TC;
383	mlxsw_sp_hdroom_prios_reset_buf_idx(hdroom: &hdroom);
384	mlxsw_sp_hdroom_bufs_reset_lossiness(hdroom: &hdroom);
385	mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, hdroom: &hdroom);
386
387	err = mlxsw_sp_hdroom_configure(mlxsw_sp_port, hdroom: &hdroom);
388	if (err)
389	goto err_hdroom_configure;
390	}
391
392	mlxsw_sp_qdisc->num_classes = ops->num_classes;
393	mlxsw_sp_qdisc->ops = ops;
394	mlxsw_sp_qdisc->handle = handle;
395	err = mlxsw_sp_qdisc_tree_validate(mlxsw_sp_port);
396	if (err)
397	goto err_replace;
398
399	err = ops->replace(mlxsw_sp_port, handle, mlxsw_sp_qdisc, params);
400	if (err)
401	goto err_replace;
402
403	return 0;
404
405	err_replace:
406	mlxsw_sp_qdisc->handle = TC_H_UNSPEC;
407	mlxsw_sp_qdisc->ops = NULL;
408	mlxsw_sp_qdisc->num_classes = 0;
409	mlxsw_sp_hdroom_configure(mlxsw_sp_port, hdroom: &orig_hdroom);
410	err_hdroom_configure:
411	kfree(objp: mlxsw_sp_qdisc->qdiscs);
412	mlxsw_sp_qdisc->qdiscs = NULL;
413	return err;
414	}
415
416	static int
417	mlxsw_sp_qdisc_change(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
418	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params)
419	{
420	struct mlxsw_sp_qdisc_ops *ops = mlxsw_sp_qdisc->ops;
421	int err;
422
423	err = ops->check_params(mlxsw_sp_port, params);
424	if (err)
425	goto unoffload;
426
427	err = ops->replace(mlxsw_sp_port, handle, mlxsw_sp_qdisc, params);
428	if (err)
429	goto unoffload;
430
431	/* Check if the Qdisc changed. That includes a situation where an
432	* invisible Qdisc replaces another one, or is being added for the
433	* first time.
434	*/
435	if (mlxsw_sp_qdisc->handle != handle) {
436	if (ops->clean_stats)
437	ops->clean_stats(mlxsw_sp_port, mlxsw_sp_qdisc);
438	}
439
440	mlxsw_sp_qdisc->handle = handle;
441	return 0;
442
443	unoffload:
444	if (ops->unoffload)
445	ops->unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, params);
446
447	mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
448	return err;
449	}
450
451	static int
452	mlxsw_sp_qdisc_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
453	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
454	struct mlxsw_sp_qdisc_ops *ops, void *params)
455	{
456	if (mlxsw_sp_qdisc->ops && mlxsw_sp_qdisc->ops->type != ops->type)
457	/* In case this location contained a different qdisc of the
458	* same type we can override the old qdisc configuration.
459	* Otherwise, we need to remove the old qdisc before setting the
460	* new one.
461	*/
462	mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
463
464	if (!mlxsw_sp_qdisc->ops)
465	return mlxsw_sp_qdisc_create(mlxsw_sp_port, handle,
466	mlxsw_sp_qdisc, ops, params);
467	else
468	return mlxsw_sp_qdisc_change(mlxsw_sp_port, handle,
469	mlxsw_sp_qdisc, params);
470	}
471
472	static int
473	mlxsw_sp_qdisc_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
474	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
475	struct tc_qopt_offload_stats *stats_ptr)
476	{
477	if (mlxsw_sp_qdisc && mlxsw_sp_qdisc->ops &&
478	mlxsw_sp_qdisc->ops->get_stats)
479	return mlxsw_sp_qdisc->ops->get_stats(mlxsw_sp_port,
480	mlxsw_sp_qdisc,
481	stats_ptr);
482
483	return -EOPNOTSUPP;
484	}
485
486	static int
487	mlxsw_sp_qdisc_get_xstats(struct mlxsw_sp_port *mlxsw_sp_port,
488	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
489	void *xstats_ptr)
490	{
491	if (mlxsw_sp_qdisc && mlxsw_sp_qdisc->ops &&
492	mlxsw_sp_qdisc->ops->get_xstats)
493	return mlxsw_sp_qdisc->ops->get_xstats(mlxsw_sp_port,
494	mlxsw_sp_qdisc,
495	xstats_ptr);
496
497	return -EOPNOTSUPP;
498	}
499
500	static u64
501	mlxsw_sp_xstats_backlog(struct mlxsw_sp_port_xstats *xstats, int tclass_num)
502	{
503	return xstats->backlog[tclass_num] +
504	xstats->backlog[tclass_num + 8];
505	}
506
507	static u64
508	mlxsw_sp_xstats_tail_drop(struct mlxsw_sp_port_xstats *xstats, int tclass_num)
509	{
510	return xstats->tail_drop[tclass_num] +
511	xstats->tail_drop[tclass_num + 8];
512	}
513
514	static void
515	mlxsw_sp_qdisc_bstats_per_priority_get(struct mlxsw_sp_port_xstats *xstats,
516	u8 prio_bitmap, u64 *tx_packets,
517	u64 *tx_bytes)
518	{
519	int i;
520
521	*tx_packets = 0;
522	*tx_bytes = 0;
523	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
524	if (prio_bitmap & BIT(i)) {
525	*tx_packets += xstats->tx_packets[i];
526	*tx_bytes += xstats->tx_bytes[i];
527	}
528	}
529	}
530
531	static void
532	mlxsw_sp_qdisc_collect_tc_stats(struct mlxsw_sp_port *mlxsw_sp_port,
533	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
534	u64 *p_tx_bytes, u64 *p_tx_packets,
535	u64 *p_drops, u64 *p_backlog)
536	{
537	struct mlxsw_sp_port_xstats *xstats;
538	u64 tx_bytes, tx_packets;
539	u8 prio_bitmap;
540	int tclass_num;
541
542	prio_bitmap = mlxsw_sp_qdisc_get_prio_bitmap(mlxsw_sp_port,
543	mlxsw_sp_qdisc);
544	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
545	mlxsw_sp_qdisc);
546	xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
547	mlxsw_sp_qdisc_bstats_per_priority_get(xstats, prio_bitmap,
548	tx_packets: &tx_packets, tx_bytes: &tx_bytes);
549
550	*p_tx_packets += tx_packets;
551	*p_tx_bytes += tx_bytes;
552	*p_drops += xstats->wred_drop[tclass_num] +
553	mlxsw_sp_xstats_tail_drop(xstats, tclass_num);
554	*p_backlog += mlxsw_sp_xstats_backlog(xstats, tclass_num);
555	}
556
557	static void
558	mlxsw_sp_qdisc_update_stats(struct mlxsw_sp *mlxsw_sp,
559	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
560	u64 tx_bytes, u64 tx_packets,
561	u64 drops, u64 backlog,
562	struct tc_qopt_offload_stats *stats_ptr)
563	{
564	struct mlxsw_sp_qdisc_stats *stats_base = &mlxsw_sp_qdisc->stats_base;
565
566	tx_bytes -= stats_base->tx_bytes;
567	tx_packets -= stats_base->tx_packets;
568	drops -= stats_base->drops;
569	backlog -= stats_base->backlog;
570
571	_bstats_update(bstats: stats_ptr->bstats, bytes: tx_bytes, packets: tx_packets);
572	stats_ptr->qstats->drops += drops;
573	stats_ptr->qstats->backlog += mlxsw_sp_cells_bytes(mlxsw_sp, cells: backlog);
574
575	stats_base->backlog += backlog;
576	stats_base->drops += drops;
577	stats_base->tx_bytes += tx_bytes;
578	stats_base->tx_packets += tx_packets;
579	}
580
581	static void
582	mlxsw_sp_qdisc_get_tc_stats(struct mlxsw_sp_port *mlxsw_sp_port,
583	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
584	struct tc_qopt_offload_stats *stats_ptr)
585	{
586	u64 tx_packets = 0;
587	u64 tx_bytes = 0;
588	u64 backlog = 0;
589	u64 drops = 0;
590
591	mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
592	p_tx_bytes: &tx_bytes, p_tx_packets: &tx_packets,
593	p_drops: &drops, p_backlog: &backlog);
594	mlxsw_sp_qdisc_update_stats(mlxsw_sp: mlxsw_sp_port->mlxsw_sp, mlxsw_sp_qdisc,
595	tx_bytes, tx_packets, drops, backlog,
596	stats_ptr);
597	}
598
599	static int
600	mlxsw_sp_tclass_congestion_enable(struct mlxsw_sp_port *mlxsw_sp_port,
601	int tclass_num, u32 min, u32 max,
602	u32 probability, bool is_wred, bool is_ecn)
603	{
604	char cwtpm_cmd[MLXSW_REG_CWTPM_LEN];
605	char cwtp_cmd[MLXSW_REG_CWTP_LEN];
606	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
607	int err;
608
609	mlxsw_reg_cwtp_pack(payload: cwtp_cmd, local_port: mlxsw_sp_port->local_port, traffic_class: tclass_num);
610	mlxsw_reg_cwtp_profile_pack(payload: cwtp_cmd, MLXSW_REG_CWTP_DEFAULT_PROFILE,
611	roundup(min, MLXSW_REG_CWTP_MIN_VALUE),
612	roundup(max, MLXSW_REG_CWTP_MIN_VALUE),
613	probability);
614
615	err = mlxsw_reg_write(mlxsw_core: mlxsw_sp->core, MLXSW_REG(cwtp), payload: cwtp_cmd);
616	if (err)
617	return err;
618
619	mlxsw_reg_cwtpm_pack(payload: cwtpm_cmd, local_port: mlxsw_sp_port->local_port, traffic_class: tclass_num,
620	MLXSW_REG_CWTP_DEFAULT_PROFILE, wred: is_wred, ecn: is_ecn);
621
622	return mlxsw_reg_write(mlxsw_core: mlxsw_sp->core, MLXSW_REG(cwtpm), payload: cwtpm_cmd);
623	}
624
625	static int
626	mlxsw_sp_tclass_congestion_disable(struct mlxsw_sp_port *mlxsw_sp_port,
627	int tclass_num)
628	{
629	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
630	char cwtpm_cmd[MLXSW_REG_CWTPM_LEN];
631
632	mlxsw_reg_cwtpm_pack(payload: cwtpm_cmd, local_port: mlxsw_sp_port->local_port, traffic_class: tclass_num,
633	MLXSW_REG_CWTPM_RESET_PROFILE, wred: false, ecn: false);
634	return mlxsw_reg_write(mlxsw_core: mlxsw_sp->core, MLXSW_REG(cwtpm), payload: cwtpm_cmd);
635	}
636
637	static void
638	mlxsw_sp_setup_tc_qdisc_red_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
639	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
640	{
641	struct mlxsw_sp_qdisc_stats *stats_base;
642	struct mlxsw_sp_port_xstats *xstats;
643	struct red_stats *red_base;
644	u8 prio_bitmap;
645	int tclass_num;
646
647	prio_bitmap = mlxsw_sp_qdisc_get_prio_bitmap(mlxsw_sp_port,
648	mlxsw_sp_qdisc);
649	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
650	mlxsw_sp_qdisc);
651	xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
652	stats_base = &mlxsw_sp_qdisc->stats_base;
653	red_base = &mlxsw_sp_qdisc->xstats_base.red;
654
655	mlxsw_sp_qdisc_bstats_per_priority_get(xstats, prio_bitmap,
656	tx_packets: &stats_base->tx_packets,
657	tx_bytes: &stats_base->tx_bytes);
658	red_base->prob_mark = xstats->tc_ecn[tclass_num];
659	red_base->prob_drop = xstats->wred_drop[tclass_num];
660	red_base->pdrop = mlxsw_sp_xstats_tail_drop(xstats, tclass_num);
661
662	stats_base->overlimits = red_base->prob_drop + red_base->prob_mark;
663	stats_base->drops = red_base->prob_drop + red_base->pdrop;
664
665	stats_base->backlog = 0;
666	}
667
668	static int
669	mlxsw_sp_qdisc_red_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
670	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
671	{
672	int tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
673	mlxsw_sp_qdisc);
674
675	return mlxsw_sp_tclass_congestion_disable(mlxsw_sp_port, tclass_num);
676	}
677
678	static int
679	mlxsw_sp_qdisc_red_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
680	void *params)
681	{
682	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
683	struct tc_red_qopt_offload_params *p = params;
684
685	if (p->min > p->max) {
686	dev_err(mlxsw_sp->bus_info->dev,
687	"spectrum: RED: min %u is bigger then max %u\n", p->min,
688	p->max);
689	return -EINVAL;
690	}
691	if (p->max > MLXSW_CORE_RES_GET(mlxsw_sp->core,
692	GUARANTEED_SHARED_BUFFER)) {
693	dev_err(mlxsw_sp->bus_info->dev,
694	"spectrum: RED: max value %u is too big\n", p->max);
695	return -EINVAL;
696	}
697	if (p->min == 0 || p->max == 0) {
698	dev_err(mlxsw_sp->bus_info->dev,
699	"spectrum: RED: 0 value is illegal for min and max\n");
700	return -EINVAL;
701	}
702	return 0;
703	}
704
705	static int
706	mlxsw_sp_qdisc_future_fifo_replace(struct mlxsw_sp_port *mlxsw_sp_port,
707	u32 handle, unsigned int band,
708	struct mlxsw_sp_qdisc *child_qdisc);
709	static void
710	mlxsw_sp_qdisc_future_fifos_init(struct mlxsw_sp_port *mlxsw_sp_port,
711	u32 handle);
712
713	static int
714	mlxsw_sp_qdisc_red_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
715	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
716	void *params)
717	{
718	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
719	struct tc_red_qopt_offload_params *p = params;
720	int tclass_num;
721	u32 min, max;
722	u64 prob;
723	int err;
724
725	err = mlxsw_sp_qdisc_future_fifo_replace(mlxsw_sp_port, handle, band: 0,
726	child_qdisc: &mlxsw_sp_qdisc->qdiscs[0]);
727	if (err)
728	return err;
729	mlxsw_sp_qdisc_future_fifos_init(mlxsw_sp_port, TC_H_UNSPEC);
730
731	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
732	mlxsw_sp_qdisc);
733
734	/* calculate probability in percentage */
735	prob = p->probability;
736	prob *= 100;
737	prob = DIV_ROUND_UP(prob, 1 << 16);
738	prob = DIV_ROUND_UP(prob, 1 << 16);
739	min = mlxsw_sp_bytes_cells(mlxsw_sp, bytes: p->min);
740	max = mlxsw_sp_bytes_cells(mlxsw_sp, bytes: p->max);
741	return mlxsw_sp_tclass_congestion_enable(mlxsw_sp_port, tclass_num,
742	min, max, probability: prob,
743	is_wred: !p->is_nodrop, is_ecn: p->is_ecn);
744	}
745
746	static void
747	mlxsw_sp_qdisc_leaf_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
748	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
749	struct gnet_stats_queue *qstats)
750	{
751	u64 backlog;
752
753	backlog = mlxsw_sp_cells_bytes(mlxsw_sp: mlxsw_sp_port->mlxsw_sp,
754	cells: mlxsw_sp_qdisc->stats_base.backlog);
755	qstats->backlog -= backlog;
756	mlxsw_sp_qdisc->stats_base.backlog = 0;
757	}
758
759	static void
760	mlxsw_sp_qdisc_red_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
761	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
762	void *params)
763	{
764	struct tc_red_qopt_offload_params *p = params;
765
766	mlxsw_sp_qdisc_leaf_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, qstats: p->qstats);
767	}
768
769	static int
770	mlxsw_sp_qdisc_get_red_xstats(struct mlxsw_sp_port *mlxsw_sp_port,
771	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
772	void *xstats_ptr)
773	{
774	struct red_stats *xstats_base = &mlxsw_sp_qdisc->xstats_base.red;
775	struct mlxsw_sp_port_xstats *xstats;
776	struct red_stats *res = xstats_ptr;
777	int early_drops, marks, pdrops;
778	int tclass_num;
779
780	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
781	mlxsw_sp_qdisc);
782	xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
783
784	early_drops = xstats->wred_drop[tclass_num] - xstats_base->prob_drop;
785	marks = xstats->tc_ecn[tclass_num] - xstats_base->prob_mark;
786	pdrops = mlxsw_sp_xstats_tail_drop(xstats, tclass_num) -
787	xstats_base->pdrop;
788
789	res->pdrop += pdrops;
790	res->prob_drop += early_drops;
791	res->prob_mark += marks;
792
793	xstats_base->pdrop += pdrops;
794	xstats_base->prob_drop += early_drops;
795	xstats_base->prob_mark += marks;
796	return 0;
797	}
798
799	static int
800	mlxsw_sp_qdisc_get_red_stats(struct mlxsw_sp_port *mlxsw_sp_port,
801	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
802	struct tc_qopt_offload_stats *stats_ptr)
803	{
804	struct mlxsw_sp_qdisc_stats *stats_base;
805	struct mlxsw_sp_port_xstats *xstats;
806	u64 overlimits;
807	int tclass_num;
808
809	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
810	mlxsw_sp_qdisc);
811	xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
812	stats_base = &mlxsw_sp_qdisc->stats_base;
813
814	mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc, stats_ptr);
815	overlimits = xstats->wred_drop[tclass_num] +
816	xstats->tc_ecn[tclass_num] - stats_base->overlimits;
817
818	stats_ptr->qstats->overlimits += overlimits;
819	stats_base->overlimits += overlimits;
820
821	return 0;
822	}
823
824	static struct mlxsw_sp_qdisc *
825	mlxsw_sp_qdisc_leaf_find_class(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
826	u32 parent)
827	{
828	/* RED and TBF are formally classful qdiscs, but all class references,
829	* including X:0, just refer to the same one class.
830	*/
831	return &mlxsw_sp_qdisc->qdiscs[0];
832	}
833
834	static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_red = {
835	.type = MLXSW_SP_QDISC_RED,
836	.check_params = mlxsw_sp_qdisc_red_check_params,
837	.replace = mlxsw_sp_qdisc_red_replace,
838	.unoffload = mlxsw_sp_qdisc_red_unoffload,
839	.destroy = mlxsw_sp_qdisc_red_destroy,
840	.get_stats = mlxsw_sp_qdisc_get_red_stats,
841	.get_xstats = mlxsw_sp_qdisc_get_red_xstats,
842	.clean_stats = mlxsw_sp_setup_tc_qdisc_red_clean_stats,
843	.find_class = mlxsw_sp_qdisc_leaf_find_class,
844	.num_classes = 1,
845	};
846
847	static int mlxsw_sp_qdisc_graft(struct mlxsw_sp_port *mlxsw_sp_port,
848	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
849	u8 band, u32 child_handle);
850
851	static int __mlxsw_sp_setup_tc_red(struct mlxsw_sp_port *mlxsw_sp_port,
852	struct tc_red_qopt_offload *p)
853	{
854	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
855
856	mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, parent: p->parent);
857	if (!mlxsw_sp_qdisc)
858	return -EOPNOTSUPP;
859
860	if (p->command == TC_RED_REPLACE)
861	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, handle: p->handle,
862	mlxsw_sp_qdisc,
863	ops: &mlxsw_sp_qdisc_ops_red,
864	params: &p->set);
865
866	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, handle: p->handle))
867	return -EOPNOTSUPP;
868
869	switch (p->command) {
870	case TC_RED_DESTROY:
871	return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
872	case TC_RED_XSTATS:
873	return mlxsw_sp_qdisc_get_xstats(mlxsw_sp_port, mlxsw_sp_qdisc,
874	xstats_ptr: p->xstats);
875	case TC_RED_STATS:
876	return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
877	stats_ptr: &p->stats);
878	case TC_RED_GRAFT:
879	return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc, band: 0,
880	child_handle: p->child_handle);
881	default:
882	return -EOPNOTSUPP;
883	}
884	}
885
886	int mlxsw_sp_setup_tc_red(struct mlxsw_sp_port *mlxsw_sp_port,
887	struct tc_red_qopt_offload *p)
888	{
889	int err;
890
891	mutex_lock(&mlxsw_sp_port->qdisc->lock);
892	err = __mlxsw_sp_setup_tc_red(mlxsw_sp_port, p);
893	mutex_unlock(lock: &mlxsw_sp_port->qdisc->lock);
894
895	return err;
896	}
897
898	static void
899	mlxsw_sp_setup_tc_qdisc_leaf_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
900	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
901	{
902	u64 backlog_cells = 0;
903	u64 tx_packets = 0;
904	u64 tx_bytes = 0;
905	u64 drops = 0;
906
907	mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
908	p_tx_bytes: &tx_bytes, p_tx_packets: &tx_packets,
909	p_drops: &drops, p_backlog: &backlog_cells);
910
911	mlxsw_sp_qdisc->stats_base.tx_packets = tx_packets;
912	mlxsw_sp_qdisc->stats_base.tx_bytes = tx_bytes;
913	mlxsw_sp_qdisc->stats_base.drops = drops;
914	mlxsw_sp_qdisc->stats_base.backlog = 0;
915	}
916
917	static enum mlxsw_reg_qeec_hr
918	mlxsw_sp_qdisc_tbf_hr(struct mlxsw_sp_port *mlxsw_sp_port,
919	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
920	{
921	if (mlxsw_sp_qdisc == &mlxsw_sp_port->qdisc->root_qdisc)
922	return MLXSW_REG_QEEC_HR_PORT;
923
924	/* Configure subgroup shaper, so that both UC and MC traffic is subject
925	* to shaping. That is unlike RED, however UC queue lengths are going to
926	* be different than MC ones due to different pool and quota
927	* configurations, so the configuration is not applicable. For shaper on
928	* the other hand, subjecting the overall stream to the configured
929	* shaper makes sense. Also note that that is what we do for
930	* ieee_setmaxrate().
931	*/
932	return MLXSW_REG_QEEC_HR_SUBGROUP;
933	}
934
935	static int
936	mlxsw_sp_qdisc_tbf_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
937	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
938	{
939	enum mlxsw_reg_qeec_hr hr = mlxsw_sp_qdisc_tbf_hr(mlxsw_sp_port,
940	mlxsw_sp_qdisc);
941	int tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
942	mlxsw_sp_qdisc);
943
944	return mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port, hr, index: tclass_num, next_index: 0,
945	MLXSW_REG_QEEC_MAS_DIS, burst_size: 0);
946	}
947
948	static int
949	mlxsw_sp_qdisc_tbf_bs(struct mlxsw_sp_port *mlxsw_sp_port,
950	u32 max_size, u8 *p_burst_size)
951	{
952	/* TBF burst size is configured in bytes. The ASIC burst size value is
953	* ((2 ^ bs) * 512 bits. Convert the TBF bytes to 512-bit units.
954	*/
955	u32 bs512 = max_size / 64;
956	u8 bs = fls(x: bs512);
957
958	if (!bs)
959	return -EINVAL;
960	--bs;
961
962	/* Demand a power of two. */
963	if ((1 << bs) != bs512)
964	return -EINVAL;
965
966	if (bs < mlxsw_sp_port->mlxsw_sp->lowest_shaper_bs ||
967	bs > MLXSW_REG_QEEC_HIGHEST_SHAPER_BS)
968	return -EINVAL;
969
970	*p_burst_size = bs;
971	return 0;
972	}
973
974	static u32
975	mlxsw_sp_qdisc_tbf_max_size(u8 bs)
976	{
977	return (1U << bs) * 64;
978	}
979
980	static u64
981	mlxsw_sp_qdisc_tbf_rate_kbps(struct tc_tbf_qopt_offload_replace_params *p)
982	{
983	/* TBF interface is in bytes/s, whereas Spectrum ASIC is configured in
984	* Kbits/s.
985	*/
986	return div_u64(dividend: p->rate.rate_bytes_ps, divisor: 1000) * 8;
987	}
988
989	static int
990	mlxsw_sp_qdisc_tbf_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
991	void *params)
992	{
993	struct tc_tbf_qopt_offload_replace_params *p = params;
994	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
995	u64 rate_kbps = mlxsw_sp_qdisc_tbf_rate_kbps(p);
996	u8 burst_size;
997	int err;
998
999	if (rate_kbps >= MLXSW_REG_QEEC_MAS_DIS) {
1000	dev_err(mlxsw_sp_port->mlxsw_sp->bus_info->dev,
1001	"spectrum: TBF: rate of %lluKbps must be below %u\n",
1002	rate_kbps, MLXSW_REG_QEEC_MAS_DIS);
1003	return -EINVAL;
1004	}
1005
1006	err = mlxsw_sp_qdisc_tbf_bs(mlxsw_sp_port, max_size: p->max_size, p_burst_size: &burst_size);
1007	if (err) {
1008	u8 highest_shaper_bs = MLXSW_REG_QEEC_HIGHEST_SHAPER_BS;
1009
1010	dev_err(mlxsw_sp->bus_info->dev,
1011	"spectrum: TBF: invalid burst size of %u, must be a power of two between %u and %u",
1012	p->max_size,
1013	mlxsw_sp_qdisc_tbf_max_size(mlxsw_sp->lowest_shaper_bs),
1014	mlxsw_sp_qdisc_tbf_max_size(highest_shaper_bs));
1015	return -EINVAL;
1016	}
1017
1018	return 0;
1019	}
1020
1021	static int
1022	mlxsw_sp_qdisc_tbf_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1023	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1024	void *params)
1025	{
1026	enum mlxsw_reg_qeec_hr hr = mlxsw_sp_qdisc_tbf_hr(mlxsw_sp_port,
1027	mlxsw_sp_qdisc);
1028	struct tc_tbf_qopt_offload_replace_params *p = params;
1029	u64 rate_kbps = mlxsw_sp_qdisc_tbf_rate_kbps(p);
1030	int tclass_num;
1031	u8 burst_size;
1032	int err;
1033
1034	err = mlxsw_sp_qdisc_future_fifo_replace(mlxsw_sp_port, handle, band: 0,
1035	child_qdisc: &mlxsw_sp_qdisc->qdiscs[0]);
1036	if (err)
1037	return err;
1038	mlxsw_sp_qdisc_future_fifos_init(mlxsw_sp_port, TC_H_UNSPEC);
1039
1040	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port,
1041	mlxsw_sp_qdisc);
1042
1043	err = mlxsw_sp_qdisc_tbf_bs(mlxsw_sp_port, max_size: p->max_size, p_burst_size: &burst_size);
1044	if (WARN_ON_ONCE(err))
1045	/* check_params above was supposed to reject this value. */
1046	return -EINVAL;
1047
1048	return mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port, hr, index: tclass_num, next_index: 0,
1049	maxrate: rate_kbps, burst_size);
1050	}
1051
1052	static void
1053	mlxsw_sp_qdisc_tbf_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1054	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1055	void *params)
1056	{
1057	struct tc_tbf_qopt_offload_replace_params *p = params;
1058
1059	mlxsw_sp_qdisc_leaf_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, qstats: p->qstats);
1060	}
1061
1062	static int
1063	mlxsw_sp_qdisc_get_tbf_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1064	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1065	struct tc_qopt_offload_stats *stats_ptr)
1066	{
1067	mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1068	stats_ptr);
1069	return 0;
1070	}
1071
1072	static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_tbf = {
1073	.type = MLXSW_SP_QDISC_TBF,
1074	.check_params = mlxsw_sp_qdisc_tbf_check_params,
1075	.replace = mlxsw_sp_qdisc_tbf_replace,
1076	.unoffload = mlxsw_sp_qdisc_tbf_unoffload,
1077	.destroy = mlxsw_sp_qdisc_tbf_destroy,
1078	.get_stats = mlxsw_sp_qdisc_get_tbf_stats,
1079	.clean_stats = mlxsw_sp_setup_tc_qdisc_leaf_clean_stats,
1080	.find_class = mlxsw_sp_qdisc_leaf_find_class,
1081	.num_classes = 1,
1082	};
1083
1084	static int __mlxsw_sp_setup_tc_tbf(struct mlxsw_sp_port *mlxsw_sp_port,
1085	struct tc_tbf_qopt_offload *p)
1086	{
1087	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1088
1089	mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, parent: p->parent);
1090	if (!mlxsw_sp_qdisc)
1091	return -EOPNOTSUPP;
1092
1093	if (p->command == TC_TBF_REPLACE)
1094	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, handle: p->handle,
1095	mlxsw_sp_qdisc,
1096	ops: &mlxsw_sp_qdisc_ops_tbf,
1097	params: &p->replace_params);
1098
1099	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, handle: p->handle))
1100	return -EOPNOTSUPP;
1101
1102	switch (p->command) {
1103	case TC_TBF_DESTROY:
1104	return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1105	case TC_TBF_STATS:
1106	return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1107	stats_ptr: &p->stats);
1108	case TC_TBF_GRAFT:
1109	return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc, band: 0,
1110	child_handle: p->child_handle);
1111	default:
1112	return -EOPNOTSUPP;
1113	}
1114	}
1115
1116	int mlxsw_sp_setup_tc_tbf(struct mlxsw_sp_port *mlxsw_sp_port,
1117	struct tc_tbf_qopt_offload *p)
1118	{
1119	int err;
1120
1121	mutex_lock(&mlxsw_sp_port->qdisc->lock);
1122	err = __mlxsw_sp_setup_tc_tbf(mlxsw_sp_port, p);
1123	mutex_unlock(lock: &mlxsw_sp_port->qdisc->lock);
1124
1125	return err;
1126	}
1127
1128	static int
1129	mlxsw_sp_qdisc_fifo_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
1130	void *params)
1131	{
1132	return 0;
1133	}
1134
1135	static int
1136	mlxsw_sp_qdisc_fifo_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1137	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1138	void *params)
1139	{
1140	return 0;
1141	}
1142
1143	static int
1144	mlxsw_sp_qdisc_get_fifo_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1145	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1146	struct tc_qopt_offload_stats *stats_ptr)
1147	{
1148	mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1149	stats_ptr);
1150	return 0;
1151	}
1152
1153	static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_fifo = {
1154	.type = MLXSW_SP_QDISC_FIFO,
1155	.check_params = mlxsw_sp_qdisc_fifo_check_params,
1156	.replace = mlxsw_sp_qdisc_fifo_replace,
1157	.get_stats = mlxsw_sp_qdisc_get_fifo_stats,
1158	.clean_stats = mlxsw_sp_setup_tc_qdisc_leaf_clean_stats,
1159	};
1160
1161	static int
1162	mlxsw_sp_qdisc_future_fifo_replace(struct mlxsw_sp_port *mlxsw_sp_port,
1163	u32 handle, unsigned int band,
1164	struct mlxsw_sp_qdisc *child_qdisc)
1165	{
1166	struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
1167
1168	if (handle == qdisc_state->future_handle &&
1169	qdisc_state->future_fifos[band])
1170	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, TC_H_UNSPEC,
1171	mlxsw_sp_qdisc: child_qdisc,
1172	ops: &mlxsw_sp_qdisc_ops_fifo,
1173	NULL);
1174	return 0;
1175	}
1176
1177	static void
1178	mlxsw_sp_qdisc_future_fifos_init(struct mlxsw_sp_port *mlxsw_sp_port,
1179	u32 handle)
1180	{
1181	struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
1182
1183	qdisc_state->future_handle = handle;
1184	memset(qdisc_state->future_fifos, 0, sizeof(qdisc_state->future_fifos));
1185	}
1186
1187	static int __mlxsw_sp_setup_tc_fifo(struct mlxsw_sp_port *mlxsw_sp_port,
1188	struct tc_fifo_qopt_offload *p)
1189	{
1190	struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
1191	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1192	unsigned int band;
1193	u32 parent_handle;
1194
1195	mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, parent: p->parent);
1196	if (!mlxsw_sp_qdisc && p->handle == TC_H_UNSPEC) {
1197	parent_handle = TC_H_MAJ(p->parent);
1198	if (parent_handle != qdisc_state->future_handle) {
1199	/* This notifications is for a different Qdisc than
1200	* previously. Wipe the future cache.
1201	*/
1202	mlxsw_sp_qdisc_future_fifos_init(mlxsw_sp_port,
1203	handle: parent_handle);
1204	}
1205
1206	band = TC_H_MIN(p->parent) - 1;
1207	if (band < IEEE_8021QAZ_MAX_TCS) {
1208	if (p->command == TC_FIFO_REPLACE)
1209	qdisc_state->future_fifos[band] = true;
1210	else if (p->command == TC_FIFO_DESTROY)
1211	qdisc_state->future_fifos[band] = false;
1212	}
1213	}
1214	if (!mlxsw_sp_qdisc)
1215	return -EOPNOTSUPP;
1216
1217	if (p->command == TC_FIFO_REPLACE) {
1218	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, handle: p->handle,
1219	mlxsw_sp_qdisc,
1220	ops: &mlxsw_sp_qdisc_ops_fifo, NULL);
1221	}
1222
1223	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, handle: p->handle))
1224	return -EOPNOTSUPP;
1225
1226	switch (p->command) {
1227	case TC_FIFO_DESTROY:
1228	return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1229	case TC_FIFO_STATS:
1230	return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1231	stats_ptr: &p->stats);
1232	case TC_FIFO_REPLACE: /* Handled above. */
1233	break;
1234	}
1235
1236	return -EOPNOTSUPP;
1237	}
1238
1239	int mlxsw_sp_setup_tc_fifo(struct mlxsw_sp_port *mlxsw_sp_port,
1240	struct tc_fifo_qopt_offload *p)
1241	{
1242	int err;
1243
1244	mutex_lock(&mlxsw_sp_port->qdisc->lock);
1245	err = __mlxsw_sp_setup_tc_fifo(mlxsw_sp_port, p);
1246	mutex_unlock(lock: &mlxsw_sp_port->qdisc->lock);
1247
1248	return err;
1249	}
1250
1251	static int __mlxsw_sp_qdisc_ets_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1252	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1253	{
1254	int i;
1255
1256	for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++) {
1257	mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, switch_prio: i,
1258	MLXSW_SP_PORT_DEFAULT_TCLASS);
1259	mlxsw_sp_port_ets_set(mlxsw_sp_port,
1260	hr: MLXSW_REG_QEEC_HR_SUBGROUP,
1261	index: i, next_index: 0, dwrr: false, dwrr_weight: 0);
1262	}
1263
1264	kfree(objp: mlxsw_sp_qdisc->ets_data);
1265	mlxsw_sp_qdisc->ets_data = NULL;
1266	return 0;
1267	}
1268
1269	static int
1270	mlxsw_sp_qdisc_prio_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1271	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1272	{
1273	return __mlxsw_sp_qdisc_ets_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1274	}
1275
1276	static int
1277	__mlxsw_sp_qdisc_ets_check_params(unsigned int nbands)
1278	{
1279	if (nbands > IEEE_8021QAZ_MAX_TCS)
1280	return -EOPNOTSUPP;
1281
1282	return 0;
1283	}
1284
1285	static int
1286	mlxsw_sp_qdisc_prio_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
1287	void *params)
1288	{
1289	struct tc_prio_qopt_offload_params *p = params;
1290
1291	return __mlxsw_sp_qdisc_ets_check_params(nbands: p->bands);
1292	}
1293
1294	static struct mlxsw_sp_qdisc *
1295	mlxsw_sp_qdisc_walk_cb_clean_stats(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1296	void *mlxsw_sp_port)
1297	{
1298	u64 backlog;
1299
1300	if (mlxsw_sp_qdisc->ops) {
1301	backlog = mlxsw_sp_qdisc->stats_base.backlog;
1302	if (mlxsw_sp_qdisc->ops->clean_stats)
1303	mlxsw_sp_qdisc->ops->clean_stats(mlxsw_sp_port,
1304	mlxsw_sp_qdisc);
1305	mlxsw_sp_qdisc->stats_base.backlog = backlog;
1306	}
1307
1308	return NULL;
1309	}
1310
1311	static void
1312	mlxsw_sp_qdisc_tree_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1313	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1314	{
1315	mlxsw_sp_qdisc_walk(qdisc: mlxsw_sp_qdisc, pre: mlxsw_sp_qdisc_walk_cb_clean_stats,
1316	data: mlxsw_sp_port);
1317	}
1318
1319	static int
1320	__mlxsw_sp_qdisc_ets_replace(struct mlxsw_sp_port *mlxsw_sp_port,
1321	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1322	u32 handle, unsigned int nbands,
1323	const unsigned int *quanta,
1324	const unsigned int *weights,
1325	const u8 *priomap)
1326	{
1327	struct mlxsw_sp_qdisc_ets_data *ets_data = mlxsw_sp_qdisc->ets_data;
1328	struct mlxsw_sp_qdisc_ets_band *ets_band;
1329	struct mlxsw_sp_qdisc *child_qdisc;
1330	u8 old_priomap, new_priomap;
1331	int i, band;
1332	int err;
1333
1334	if (!ets_data) {
1335	ets_data = kzalloc(size: sizeof(*ets_data), GFP_KERNEL);
1336	if (!ets_data)
1337	return -ENOMEM;
1338	mlxsw_sp_qdisc->ets_data = ets_data;
1339
1340	for (band = 0; band < mlxsw_sp_qdisc->num_classes; band++) {
1341	int tclass_num = MLXSW_SP_PRIO_BAND_TO_TCLASS(band);
1342
1343	ets_band = &ets_data->bands[band];
1344	ets_band->tclass_num = tclass_num;
1345	}
1346	}
1347
1348	for (band = 0; band < nbands; band++) {
1349	int tclass_num;
1350
1351	child_qdisc = &mlxsw_sp_qdisc->qdiscs[band];
1352	ets_band = &ets_data->bands[band];
1353
1354	tclass_num = ets_band->tclass_num;
1355	old_priomap = ets_band->prio_bitmap;
1356	new_priomap = 0;
1357
1358	err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
1359	hr: MLXSW_REG_QEEC_HR_SUBGROUP,
1360	index: tclass_num, next_index: 0, dwrr: !!quanta[band],
1361	dwrr_weight: weights[band]);
1362	if (err)
1363	return err;
1364
1365	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
1366	if (priomap[i] == band) {
1367	new_priomap |= BIT(i);
1368	if (BIT(i) & old_priomap)
1369	continue;
1370	err = mlxsw_sp_port_prio_tc_set(mlxsw_sp_port,
1371	switch_prio: i, tclass: tclass_num);
1372	if (err)
1373	return err;
1374	}
1375	}
1376
1377	ets_band->prio_bitmap = new_priomap;
1378
1379	if (old_priomap != new_priomap)
1380	mlxsw_sp_qdisc_tree_clean_stats(mlxsw_sp_port,
1381	mlxsw_sp_qdisc: child_qdisc);
1382
1383	err = mlxsw_sp_qdisc_future_fifo_replace(mlxsw_sp_port, handle,
1384	band, child_qdisc);
1385	if (err)
1386	return err;
1387	}
1388	for (; band < IEEE_8021QAZ_MAX_TCS; band++) {
1389	ets_band = &ets_data->bands[band];
1390	ets_band->prio_bitmap = 0;
1391
1392	child_qdisc = &mlxsw_sp_qdisc->qdiscs[band];
1393	mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc: child_qdisc);
1394
1395	mlxsw_sp_port_ets_set(mlxsw_sp_port,
1396	hr: MLXSW_REG_QEEC_HR_SUBGROUP,
1397	index: ets_band->tclass_num, next_index: 0, dwrr: false, dwrr_weight: 0);
1398	}
1399
1400	mlxsw_sp_qdisc_future_fifos_init(mlxsw_sp_port, TC_H_UNSPEC);
1401	return 0;
1402	}
1403
1404	static int
1405	mlxsw_sp_qdisc_prio_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1406	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1407	void *params)
1408	{
1409	struct tc_prio_qopt_offload_params *p = params;
1410	unsigned int zeroes[TCQ_ETS_MAX_BANDS] = {0};
1411
1412	return __mlxsw_sp_qdisc_ets_replace(mlxsw_sp_port, mlxsw_sp_qdisc,
1413	handle, nbands: p->bands, quanta: zeroes,
1414	weights: zeroes, priomap: p->priomap);
1415	}
1416
1417	static void
1418	__mlxsw_sp_qdisc_ets_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1419	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1420	struct gnet_stats_queue *qstats)
1421	{
1422	u64 backlog;
1423
1424	backlog = mlxsw_sp_cells_bytes(mlxsw_sp: mlxsw_sp_port->mlxsw_sp,
1425	cells: mlxsw_sp_qdisc->stats_base.backlog);
1426	qstats->backlog -= backlog;
1427	}
1428
1429	static void
1430	mlxsw_sp_qdisc_prio_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1431	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1432	void *params)
1433	{
1434	struct tc_prio_qopt_offload_params *p = params;
1435
1436	__mlxsw_sp_qdisc_ets_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc,
1437	qstats: p->qstats);
1438	}
1439
1440	static int
1441	mlxsw_sp_qdisc_get_prio_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1442	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1443	struct tc_qopt_offload_stats *stats_ptr)
1444	{
1445	struct mlxsw_sp_qdisc *tc_qdisc;
1446	u64 tx_packets = 0;
1447	u64 tx_bytes = 0;
1448	u64 backlog = 0;
1449	u64 drops = 0;
1450	int i;
1451
1452	for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++) {
1453	tc_qdisc = &mlxsw_sp_qdisc->qdiscs[i];
1454	mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc: tc_qdisc,
1455	p_tx_bytes: &tx_bytes, p_tx_packets: &tx_packets,
1456	p_drops: &drops, p_backlog: &backlog);
1457	}
1458
1459	mlxsw_sp_qdisc_update_stats(mlxsw_sp: mlxsw_sp_port->mlxsw_sp, mlxsw_sp_qdisc,
1460	tx_bytes, tx_packets, drops, backlog,
1461	stats_ptr);
1462	return 0;
1463	}
1464
1465	static void
1466	mlxsw_sp_setup_tc_qdisc_prio_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1467	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1468	{
1469	struct mlxsw_sp_qdisc_stats *stats_base;
1470	struct mlxsw_sp_port_xstats *xstats;
1471	struct rtnl_link_stats64 *stats;
1472	int i;
1473
1474	xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
1475	stats = &mlxsw_sp_port->periodic_hw_stats.stats;
1476	stats_base = &mlxsw_sp_qdisc->stats_base;
1477
1478	stats_base->tx_packets = stats->tx_packets;
1479	stats_base->tx_bytes = stats->tx_bytes;
1480
1481	stats_base->drops = 0;
1482	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
1483	stats_base->drops += mlxsw_sp_xstats_tail_drop(xstats, tclass_num: i);
1484	stats_base->drops += xstats->wred_drop[i];
1485	}
1486
1487	mlxsw_sp_qdisc->stats_base.backlog = 0;
1488	}
1489
1490	static struct mlxsw_sp_qdisc *
1491	mlxsw_sp_qdisc_prio_find_class(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1492	u32 parent)
1493	{
1494	int child_index = TC_H_MIN(parent);
1495	int band = child_index - 1;
1496
1497	if (band < 0 || band >= mlxsw_sp_qdisc->num_classes)
1498	return NULL;
1499	return &mlxsw_sp_qdisc->qdiscs[band];
1500	}
1501
1502	static struct mlxsw_sp_qdisc_ets_band *
1503	mlxsw_sp_qdisc_ets_get_band(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1504	struct mlxsw_sp_qdisc *child)
1505	{
1506	unsigned int band = child - mlxsw_sp_qdisc->qdiscs;
1507
1508	if (WARN_ON(band >= IEEE_8021QAZ_MAX_TCS))
1509	band = 0;
1510	return &mlxsw_sp_qdisc->ets_data->bands[band];
1511	}
1512
1513	static u8
1514	mlxsw_sp_qdisc_ets_get_prio_bitmap(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1515	struct mlxsw_sp_qdisc *child)
1516	{
1517	return mlxsw_sp_qdisc_ets_get_band(mlxsw_sp_qdisc, child)->prio_bitmap;
1518	}
1519
1520	static int
1521	mlxsw_sp_qdisc_ets_get_tclass_num(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1522	struct mlxsw_sp_qdisc *child)
1523	{
1524	return mlxsw_sp_qdisc_ets_get_band(mlxsw_sp_qdisc, child)->tclass_num;
1525	}
1526
1527	static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_prio = {
1528	.type = MLXSW_SP_QDISC_PRIO,
1529	.check_params = mlxsw_sp_qdisc_prio_check_params,
1530	.replace = mlxsw_sp_qdisc_prio_replace,
1531	.unoffload = mlxsw_sp_qdisc_prio_unoffload,
1532	.destroy = mlxsw_sp_qdisc_prio_destroy,
1533	.get_stats = mlxsw_sp_qdisc_get_prio_stats,
1534	.clean_stats = mlxsw_sp_setup_tc_qdisc_prio_clean_stats,
1535	.find_class = mlxsw_sp_qdisc_prio_find_class,
1536	.num_classes = IEEE_8021QAZ_MAX_TCS,
1537	.get_prio_bitmap = mlxsw_sp_qdisc_ets_get_prio_bitmap,
1538	.get_tclass_num = mlxsw_sp_qdisc_ets_get_tclass_num,
1539	};
1540
1541	static int
1542	mlxsw_sp_qdisc_ets_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
1543	void *params)
1544	{
1545	struct tc_ets_qopt_offload_replace_params *p = params;
1546
1547	return __mlxsw_sp_qdisc_ets_check_params(nbands: p->bands);
1548	}
1549
1550	static int
1551	mlxsw_sp_qdisc_ets_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1552	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1553	void *params)
1554	{
1555	struct tc_ets_qopt_offload_replace_params *p = params;
1556
1557	return __mlxsw_sp_qdisc_ets_replace(mlxsw_sp_port, mlxsw_sp_qdisc,
1558	handle, nbands: p->bands, quanta: p->quanta,
1559	weights: p->weights, priomap: p->priomap);
1560	}
1561
1562	static void
1563	mlxsw_sp_qdisc_ets_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1564	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1565	void *params)
1566	{
1567	struct tc_ets_qopt_offload_replace_params *p = params;
1568
1569	__mlxsw_sp_qdisc_ets_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc,
1570	qstats: p->qstats);
1571	}
1572
1573	static int
1574	mlxsw_sp_qdisc_ets_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1575	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1576	{
1577	return __mlxsw_sp_qdisc_ets_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1578	}
1579
1580	static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_ets = {
1581	.type = MLXSW_SP_QDISC_ETS,
1582	.check_params = mlxsw_sp_qdisc_ets_check_params,
1583	.replace = mlxsw_sp_qdisc_ets_replace,
1584	.unoffload = mlxsw_sp_qdisc_ets_unoffload,
1585	.destroy = mlxsw_sp_qdisc_ets_destroy,
1586	.get_stats = mlxsw_sp_qdisc_get_prio_stats,
1587	.clean_stats = mlxsw_sp_setup_tc_qdisc_prio_clean_stats,
1588	.find_class = mlxsw_sp_qdisc_prio_find_class,
1589	.num_classes = IEEE_8021QAZ_MAX_TCS,
1590	.get_prio_bitmap = mlxsw_sp_qdisc_ets_get_prio_bitmap,
1591	.get_tclass_num = mlxsw_sp_qdisc_ets_get_tclass_num,
1592	};
1593
1594	/* Linux allows linking of Qdiscs to arbitrary classes (so long as the resulting
1595	* graph is free of cycles). These operations do not change the parent handle
1596	* though, which means it can be incomplete (if there is more than one class
1597	* where the Qdisc in question is grafted) or outright wrong (if the Qdisc was
1598	* linked to a different class and then removed from the original class).
1599	*
1600	* E.g. consider this sequence of operations:
1601	*
1602	* # tc qdisc add dev swp1 root handle 1: prio
1603	* # tc qdisc add dev swp1 parent 1:3 handle 13: red limit 1000000 avpkt 10000
1604	* RED: set bandwidth to 10Mbit
1605	* # tc qdisc link dev swp1 handle 13: parent 1:2
1606	*
1607	* At this point, both 1:2 and 1:3 have the same RED Qdisc instance as their
1608	* child. But RED will still only claim that 1:3 is its parent. If it's removed
1609	* from that band, its only parent will be 1:2, but it will continue to claim
1610	* that it is in fact 1:3.
1611	*
1612	* The notification for child Qdisc replace (e.g. TC_RED_REPLACE) comes before
1613	* the notification for parent graft (e.g. TC_PRIO_GRAFT). We take the replace
1614	* notification to offload the child Qdisc, based on its parent handle, and use
1615	* the graft operation to validate that the class where the child is actually
1616	* grafted corresponds to the parent handle. If the two don't match, we
1617	* unoffload the child.
1618	*/
1619	static int mlxsw_sp_qdisc_graft(struct mlxsw_sp_port *mlxsw_sp_port,
1620	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1621	u8 band, u32 child_handle)
1622	{
1623	struct mlxsw_sp_qdisc *old_qdisc;
1624	u32 parent;
1625
1626	if (band < mlxsw_sp_qdisc->num_classes &&
1627	mlxsw_sp_qdisc->qdiscs[band].handle == child_handle)
1628	return 0;
1629
1630	if (!child_handle) {
1631	/* This is an invisible FIFO replacing the original Qdisc.
1632	* Ignore it--the original Qdisc's destroy will follow.
1633	*/
1634	return 0;
1635	}
1636
1637	/* See if the grafted qdisc is already offloaded on any tclass. If so,
1638	* unoffload it.
1639	*/
1640	old_qdisc = mlxsw_sp_qdisc_find_by_handle(mlxsw_sp_port,
1641	handle: child_handle);
1642	if (old_qdisc)
1643	mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc: old_qdisc);
1644
1645	parent = TC_H_MAKE(mlxsw_sp_qdisc->handle, band + 1);
1646	mlxsw_sp_qdisc = mlxsw_sp_qdisc->ops->find_class(mlxsw_sp_qdisc,
1647	parent);
1648	if (!WARN_ON(!mlxsw_sp_qdisc))
1649	mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1650
1651	return -EOPNOTSUPP;
1652	}
1653
1654	static int __mlxsw_sp_setup_tc_prio(struct mlxsw_sp_port *mlxsw_sp_port,
1655	struct tc_prio_qopt_offload *p)
1656	{
1657	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1658
1659	mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, parent: p->parent);
1660	if (!mlxsw_sp_qdisc)
1661	return -EOPNOTSUPP;
1662
1663	if (p->command == TC_PRIO_REPLACE)
1664	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, handle: p->handle,
1665	mlxsw_sp_qdisc,
1666	ops: &mlxsw_sp_qdisc_ops_prio,
1667	params: &p->replace_params);
1668
1669	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, handle: p->handle))
1670	return -EOPNOTSUPP;
1671
1672	switch (p->command) {
1673	case TC_PRIO_DESTROY:
1674	return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1675	case TC_PRIO_STATS:
1676	return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1677	stats_ptr: &p->stats);
1678	case TC_PRIO_GRAFT:
1679	return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1680	band: p->graft_params.band,
1681	child_handle: p->graft_params.child_handle);
1682	default:
1683	return -EOPNOTSUPP;
1684	}
1685	}
1686
1687	int mlxsw_sp_setup_tc_prio(struct mlxsw_sp_port *mlxsw_sp_port,
1688	struct tc_prio_qopt_offload *p)
1689	{
1690	int err;
1691
1692	mutex_lock(&mlxsw_sp_port->qdisc->lock);
1693	err = __mlxsw_sp_setup_tc_prio(mlxsw_sp_port, p);
1694	mutex_unlock(lock: &mlxsw_sp_port->qdisc->lock);
1695
1696	return err;
1697	}
1698
1699	static int __mlxsw_sp_setup_tc_ets(struct mlxsw_sp_port *mlxsw_sp_port,
1700	struct tc_ets_qopt_offload *p)
1701	{
1702	struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1703
1704	mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, parent: p->parent);
1705	if (!mlxsw_sp_qdisc)
1706	return -EOPNOTSUPP;
1707
1708	if (p->command == TC_ETS_REPLACE)
1709	return mlxsw_sp_qdisc_replace(mlxsw_sp_port, handle: p->handle,
1710	mlxsw_sp_qdisc,
1711	ops: &mlxsw_sp_qdisc_ops_ets,
1712	params: &p->replace_params);
1713
1714	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, handle: p->handle))
1715	return -EOPNOTSUPP;
1716
1717	switch (p->command) {
1718	case TC_ETS_DESTROY:
1719	return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1720	case TC_ETS_STATS:
1721	return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1722	stats_ptr: &p->stats);
1723	case TC_ETS_GRAFT:
1724	return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1725	band: p->graft_params.band,
1726	child_handle: p->graft_params.child_handle);
1727	default:
1728	return -EOPNOTSUPP;
1729	}
1730	}
1731
1732	int mlxsw_sp_setup_tc_ets(struct mlxsw_sp_port *mlxsw_sp_port,
1733	struct tc_ets_qopt_offload *p)
1734	{
1735	int err;
1736
1737	mutex_lock(&mlxsw_sp_port->qdisc->lock);
1738	err = __mlxsw_sp_setup_tc_ets(mlxsw_sp_port, p);
1739	mutex_unlock(lock: &mlxsw_sp_port->qdisc->lock);
1740
1741	return err;
1742	}
1743
1744	struct mlxsw_sp_qevent_block {
1745	struct list_head binding_list;
1746	struct list_head mall_entry_list;
1747	struct mlxsw_sp *mlxsw_sp;
1748	};
1749
1750	struct mlxsw_sp_qevent_binding {
1751	struct list_head list;
1752	struct mlxsw_sp_port *mlxsw_sp_port;
1753	u32 handle;
1754	int tclass_num;
1755	enum mlxsw_sp_span_trigger span_trigger;
1756	unsigned int action_mask;
1757	};
1758
1759	static LIST_HEAD(mlxsw_sp_qevent_block_cb_list);
1760
1761	static int mlxsw_sp_qevent_span_configure(struct mlxsw_sp *mlxsw_sp,
1762	struct mlxsw_sp_mall_entry *mall_entry,
1763	struct mlxsw_sp_qevent_binding *qevent_binding,
1764	const struct mlxsw_sp_span_agent_parms *agent_parms,
1765	int *p_span_id)
1766	{
1767	enum mlxsw_sp_span_trigger span_trigger = qevent_binding->span_trigger;
1768	struct mlxsw_sp_port *mlxsw_sp_port = qevent_binding->mlxsw_sp_port;
1769	struct mlxsw_sp_span_trigger_parms trigger_parms = {};
1770	bool ingress;
1771	int span_id;
1772	int err;
1773
1774	err = mlxsw_sp_span_agent_get(mlxsw_sp, p_span_id: &span_id, parms: agent_parms);
1775	if (err)
1776	return err;
1777
1778	ingress = mlxsw_sp_span_trigger_is_ingress(trigger: span_trigger);
1779	err = mlxsw_sp_span_analyzed_port_get(mlxsw_sp_port, ingress);
1780	if (err)
1781	goto err_analyzed_port_get;
1782
1783	trigger_parms.span_id = span_id;
1784	trigger_parms.probability_rate = 1;
1785	err = mlxsw_sp_span_agent_bind(mlxsw_sp, trigger: span_trigger, mlxsw_sp_port,
1786	parms: &trigger_parms);
1787	if (err)
1788	goto err_agent_bind;
1789
1790	err = mlxsw_sp_span_trigger_enable(mlxsw_sp_port, trigger: span_trigger,
1791	tc: qevent_binding->tclass_num);
1792	if (err)
1793	goto err_trigger_enable;
1794
1795	*p_span_id = span_id;
1796	return 0;
1797
1798	err_trigger_enable:
1799	mlxsw_sp_span_agent_unbind(mlxsw_sp, trigger: span_trigger, mlxsw_sp_port,
1800	parms: &trigger_parms);
1801	err_agent_bind:
1802	mlxsw_sp_span_analyzed_port_put(mlxsw_sp_port, ingress);
1803	err_analyzed_port_get:
1804	mlxsw_sp_span_agent_put(mlxsw_sp, span_id);
1805	return err;
1806	}
1807
1808	static void mlxsw_sp_qevent_span_deconfigure(struct mlxsw_sp *mlxsw_sp,
1809	struct mlxsw_sp_qevent_binding *qevent_binding,
1810	int span_id)
1811	{
1812	enum mlxsw_sp_span_trigger span_trigger = qevent_binding->span_trigger;
1813	struct mlxsw_sp_port *mlxsw_sp_port = qevent_binding->mlxsw_sp_port;
1814	struct mlxsw_sp_span_trigger_parms trigger_parms = {
1815	.span_id = span_id,
1816	};
1817	bool ingress;
1818
1819	ingress = mlxsw_sp_span_trigger_is_ingress(trigger: span_trigger);
1820
1821	mlxsw_sp_span_trigger_disable(mlxsw_sp_port, trigger: span_trigger,
1822	tc: qevent_binding->tclass_num);
1823	mlxsw_sp_span_agent_unbind(mlxsw_sp, trigger: span_trigger, mlxsw_sp_port,
1824	parms: &trigger_parms);
1825	mlxsw_sp_span_analyzed_port_put(mlxsw_sp_port, ingress);
1826	mlxsw_sp_span_agent_put(mlxsw_sp, span_id);
1827	}
1828
1829	static int mlxsw_sp_qevent_mirror_configure(struct mlxsw_sp *mlxsw_sp,
1830	struct mlxsw_sp_mall_entry *mall_entry,
1831	struct mlxsw_sp_qevent_binding *qevent_binding)
1832	{
1833	struct mlxsw_sp_span_agent_parms agent_parms = {
1834	.to_dev = mall_entry->mirror.to_dev,
1835	};
1836
1837	return mlxsw_sp_qevent_span_configure(mlxsw_sp, mall_entry, qevent_binding,
1838	agent_parms: &agent_parms, p_span_id: &mall_entry->mirror.span_id);
1839	}
1840
1841	static void mlxsw_sp_qevent_mirror_deconfigure(struct mlxsw_sp *mlxsw_sp,
1842	struct mlxsw_sp_mall_entry *mall_entry,
1843	struct mlxsw_sp_qevent_binding *qevent_binding)
1844	{
1845	mlxsw_sp_qevent_span_deconfigure(mlxsw_sp, qevent_binding, span_id: mall_entry->mirror.span_id);
1846	}
1847
1848	static int mlxsw_sp_qevent_trap_configure(struct mlxsw_sp *mlxsw_sp,
1849	struct mlxsw_sp_mall_entry *mall_entry,
1850	struct mlxsw_sp_qevent_binding *qevent_binding)
1851	{
1852	struct mlxsw_sp_span_agent_parms agent_parms = {
1853	.session_id = MLXSW_SP_SPAN_SESSION_ID_BUFFER,
1854	};
1855	int err;
1856
1857	err = mlxsw_sp_trap_group_policer_hw_id_get(mlxsw_sp,
1858	id: DEVLINK_TRAP_GROUP_GENERIC_ID_BUFFER_DROPS,
1859	p_enabled: &agent_parms.policer_enable,
1860	p_hw_id: &agent_parms.policer_id);
1861	if (err)
1862	return err;
1863
1864	return mlxsw_sp_qevent_span_configure(mlxsw_sp, mall_entry, qevent_binding,
1865	agent_parms: &agent_parms, p_span_id: &mall_entry->trap.span_id);
1866	}
1867
1868	static void mlxsw_sp_qevent_trap_deconfigure(struct mlxsw_sp *mlxsw_sp,
1869	struct mlxsw_sp_mall_entry *mall_entry,
1870	struct mlxsw_sp_qevent_binding *qevent_binding)
1871	{
1872	mlxsw_sp_qevent_span_deconfigure(mlxsw_sp, qevent_binding, span_id: mall_entry->trap.span_id);
1873	}
1874
1875	static int
1876	mlxsw_sp_qevent_entry_configure(struct mlxsw_sp *mlxsw_sp,
1877	struct mlxsw_sp_mall_entry *mall_entry,
1878	struct mlxsw_sp_qevent_binding *qevent_binding,
1879	struct netlink_ext_ack *extack)
1880	{
1881	if (!(BIT(mall_entry->type) & qevent_binding->action_mask)) {
1882	NL_SET_ERR_MSG(extack, "Action not supported at this qevent");
1883	return -EOPNOTSUPP;
1884	}
1885
1886	switch (mall_entry->type) {
1887	case MLXSW_SP_MALL_ACTION_TYPE_MIRROR:
1888	return mlxsw_sp_qevent_mirror_configure(mlxsw_sp, mall_entry, qevent_binding);
1889	case MLXSW_SP_MALL_ACTION_TYPE_TRAP:
1890	return mlxsw_sp_qevent_trap_configure(mlxsw_sp, mall_entry, qevent_binding);
1891	default:
1892	/* This should have been validated away. */
1893	WARN_ON(1);
1894	return -EOPNOTSUPP;
1895	}
1896	}
1897
1898	static void mlxsw_sp_qevent_entry_deconfigure(struct mlxsw_sp *mlxsw_sp,
1899	struct mlxsw_sp_mall_entry *mall_entry,
1900	struct mlxsw_sp_qevent_binding *qevent_binding)
1901	{
1902	switch (mall_entry->type) {
1903	case MLXSW_SP_MALL_ACTION_TYPE_MIRROR:
1904	return mlxsw_sp_qevent_mirror_deconfigure(mlxsw_sp, mall_entry, qevent_binding);
1905	case MLXSW_SP_MALL_ACTION_TYPE_TRAP:
1906	return mlxsw_sp_qevent_trap_deconfigure(mlxsw_sp, mall_entry, qevent_binding);
1907	default:
1908	WARN_ON(1);
1909	return;
1910	}
1911	}
1912
1913	static int
1914	mlxsw_sp_qevent_binding_configure(struct mlxsw_sp_qevent_block *qevent_block,
1915	struct mlxsw_sp_qevent_binding *qevent_binding,
1916	struct netlink_ext_ack *extack)
1917	{
1918	struct mlxsw_sp_mall_entry *mall_entry;
1919	int err;
1920
1921	list_for_each_entry(mall_entry, &qevent_block->mall_entry_list, list) {
1922	err = mlxsw_sp_qevent_entry_configure(mlxsw_sp: qevent_block->mlxsw_sp, mall_entry,
1923	qevent_binding, extack);
1924	if (err)
1925	goto err_entry_configure;
1926	}
1927
1928	return 0;
1929
1930	err_entry_configure:
1931	list_for_each_entry_continue_reverse(mall_entry, &qevent_block->mall_entry_list, list)
1932	mlxsw_sp_qevent_entry_deconfigure(mlxsw_sp: qevent_block->mlxsw_sp, mall_entry,
1933	qevent_binding);
1934	return err;
1935	}
1936
1937	static void mlxsw_sp_qevent_binding_deconfigure(struct mlxsw_sp_qevent_block *qevent_block,
1938	struct mlxsw_sp_qevent_binding *qevent_binding)
1939	{
1940	struct mlxsw_sp_mall_entry *mall_entry;
1941
1942	list_for_each_entry(mall_entry, &qevent_block->mall_entry_list, list)
1943	mlxsw_sp_qevent_entry_deconfigure(mlxsw_sp: qevent_block->mlxsw_sp, mall_entry,
1944	qevent_binding);
1945	}
1946
1947	static int
1948	mlxsw_sp_qevent_block_configure(struct mlxsw_sp_qevent_block *qevent_block,
1949	struct netlink_ext_ack *extack)
1950	{
1951	struct mlxsw_sp_qevent_binding *qevent_binding;
1952	int err;
1953
1954	list_for_each_entry(qevent_binding, &qevent_block->binding_list, list) {
1955	err = mlxsw_sp_qevent_binding_configure(qevent_block,
1956	qevent_binding,
1957	extack);
1958	if (err)
1959	goto err_binding_configure;
1960	}
1961
1962	return 0;
1963
1964	err_binding_configure:
1965	list_for_each_entry_continue_reverse(qevent_binding, &qevent_block->binding_list, list)
1966	mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
1967	return err;
1968	}
1969
1970	static void mlxsw_sp_qevent_block_deconfigure(struct mlxsw_sp_qevent_block *qevent_block)
1971	{
1972	struct mlxsw_sp_qevent_binding *qevent_binding;
1973
1974	list_for_each_entry(qevent_binding, &qevent_block->binding_list, list)
1975	mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
1976	}
1977
1978	static struct mlxsw_sp_mall_entry *
1979	mlxsw_sp_qevent_mall_entry_find(struct mlxsw_sp_qevent_block *block, unsigned long cookie)
1980	{
1981	struct mlxsw_sp_mall_entry *mall_entry;
1982
1983	list_for_each_entry(mall_entry, &block->mall_entry_list, list)
1984	if (mall_entry->cookie == cookie)
1985	return mall_entry;
1986
1987	return NULL;
1988	}
1989
1990	static int mlxsw_sp_qevent_mall_replace(struct mlxsw_sp *mlxsw_sp,
1991	struct mlxsw_sp_qevent_block *qevent_block,
1992	struct tc_cls_matchall_offload *f)
1993	{
1994	struct mlxsw_sp_mall_entry *mall_entry;
1995	struct flow_action_entry *act;
1996	int err;
1997
1998	/* It should not currently be possible to replace a matchall rule. So
1999	* this must be a new rule.
2000	*/
2001	if (!list_empty(head: &qevent_block->mall_entry_list)) {
2002	NL_SET_ERR_MSG(f->common.extack, "At most one filter supported");
2003	return -EOPNOTSUPP;
2004	}
2005	if (f->rule->action.num_entries != 1) {
2006	NL_SET_ERR_MSG(f->common.extack, "Only singular actions supported");
2007	return -EOPNOTSUPP;
2008	}
2009	if (f->common.chain_index) {
2010	NL_SET_ERR_MSG(f->common.extack, "Only chain 0 is supported");
2011	return -EOPNOTSUPP;
2012	}
2013	if (f->common.protocol != htons(ETH_P_ALL)) {
2014	NL_SET_ERR_MSG(f->common.extack, "Protocol matching not supported");
2015	return -EOPNOTSUPP;
2016	}
2017
2018	act = &f->rule->action.entries[0];
2019	if (!(act->hw_stats & FLOW_ACTION_HW_STATS_DISABLED)) {
2020	NL_SET_ERR_MSG(f->common.extack, "HW counters not supported on qevents");
2021	return -EOPNOTSUPP;
2022	}
2023
2024	mall_entry = kzalloc(size: sizeof(*mall_entry), GFP_KERNEL);
2025	if (!mall_entry)
2026	return -ENOMEM;
2027	mall_entry->cookie = f->cookie;
2028
2029	if (act->id == FLOW_ACTION_MIRRED) {
2030	mall_entry->type = MLXSW_SP_MALL_ACTION_TYPE_MIRROR;
2031	mall_entry->mirror.to_dev = act->dev;
2032	} else if (act->id == FLOW_ACTION_TRAP) {
2033	mall_entry->type = MLXSW_SP_MALL_ACTION_TYPE_TRAP;
2034	} else {
2035	NL_SET_ERR_MSG(f->common.extack, "Unsupported action");
2036	err = -EOPNOTSUPP;
2037	goto err_unsupported_action;
2038	}
2039
2040	list_add_tail(new: &mall_entry->list, head: &qevent_block->mall_entry_list);
2041
2042	err = mlxsw_sp_qevent_block_configure(qevent_block, extack: f->common.extack);
2043	if (err)
2044	goto err_block_configure;
2045
2046	return 0;
2047
2048	err_block_configure:
2049	list_del(entry: &mall_entry->list);
2050	err_unsupported_action:
2051	kfree(objp: mall_entry);
2052	return err;
2053	}
2054
2055	static void mlxsw_sp_qevent_mall_destroy(struct mlxsw_sp_qevent_block *qevent_block,
2056	struct tc_cls_matchall_offload *f)
2057	{
2058	struct mlxsw_sp_mall_entry *mall_entry;
2059
2060	mall_entry = mlxsw_sp_qevent_mall_entry_find(block: qevent_block, cookie: f->cookie);
2061	if (!mall_entry)
2062	return;
2063
2064	mlxsw_sp_qevent_block_deconfigure(qevent_block);
2065
2066	list_del(entry: &mall_entry->list);
2067	kfree(objp: mall_entry);
2068	}
2069
2070	static int mlxsw_sp_qevent_block_mall_cb(struct mlxsw_sp_qevent_block *qevent_block,
2071	struct tc_cls_matchall_offload *f)
2072	{
2073	struct mlxsw_sp *mlxsw_sp = qevent_block->mlxsw_sp;
2074
2075	switch (f->command) {
2076	case TC_CLSMATCHALL_REPLACE:
2077	return mlxsw_sp_qevent_mall_replace(mlxsw_sp, qevent_block, f);
2078	case TC_CLSMATCHALL_DESTROY:
2079	mlxsw_sp_qevent_mall_destroy(qevent_block, f);
2080	return 0;
2081	default:
2082	return -EOPNOTSUPP;
2083	}
2084	}
2085
2086	static int mlxsw_sp_qevent_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
2087	{
2088	struct mlxsw_sp_qevent_block *qevent_block = cb_priv;
2089
2090	switch (type) {
2091	case TC_SETUP_CLSMATCHALL:
2092	return mlxsw_sp_qevent_block_mall_cb(qevent_block, f: type_data);
2093	default:
2094	return -EOPNOTSUPP;
2095	}
2096	}
2097
2098	static struct mlxsw_sp_qevent_block *mlxsw_sp_qevent_block_create(struct mlxsw_sp *mlxsw_sp,
2099	struct net *net)
2100	{
2101	struct mlxsw_sp_qevent_block *qevent_block;
2102
2103	qevent_block = kzalloc(size: sizeof(*qevent_block), GFP_KERNEL);
2104	if (!qevent_block)
2105	return NULL;
2106
2107	INIT_LIST_HEAD(list: &qevent_block->binding_list);
2108	INIT_LIST_HEAD(list: &qevent_block->mall_entry_list);
2109	qevent_block->mlxsw_sp = mlxsw_sp;
2110	return qevent_block;
2111	}
2112
2113	static void
2114	mlxsw_sp_qevent_block_destroy(struct mlxsw_sp_qevent_block *qevent_block)
2115	{
2116	WARN_ON(!list_empty(&qevent_block->binding_list));
2117	WARN_ON(!list_empty(&qevent_block->mall_entry_list));
2118	kfree(objp: qevent_block);
2119	}
2120
2121	static void mlxsw_sp_qevent_block_release(void *cb_priv)
2122	{
2123	struct mlxsw_sp_qevent_block *qevent_block = cb_priv;
2124
2125	mlxsw_sp_qevent_block_destroy(qevent_block);
2126	}
2127
2128	static struct mlxsw_sp_qevent_binding *
2129	mlxsw_sp_qevent_binding_create(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle, int tclass_num,
2130	enum mlxsw_sp_span_trigger span_trigger,
2131	unsigned int action_mask)
2132	{
2133	struct mlxsw_sp_qevent_binding *binding;
2134
2135	binding = kzalloc(size: sizeof(*binding), GFP_KERNEL);
2136	if (!binding)
2137	return ERR_PTR(error: -ENOMEM);
2138
2139	binding->mlxsw_sp_port = mlxsw_sp_port;
2140	binding->handle = handle;
2141	binding->tclass_num = tclass_num;
2142	binding->span_trigger = span_trigger;
2143	binding->action_mask = action_mask;
2144	return binding;
2145	}
2146
2147	static void
2148	mlxsw_sp_qevent_binding_destroy(struct mlxsw_sp_qevent_binding *binding)
2149	{
2150	kfree(objp: binding);
2151	}
2152
2153	static struct mlxsw_sp_qevent_binding *
2154	mlxsw_sp_qevent_binding_lookup(struct mlxsw_sp_qevent_block *block,
2155	struct mlxsw_sp_port *mlxsw_sp_port,
2156	u32 handle,
2157	enum mlxsw_sp_span_trigger span_trigger)
2158	{
2159	struct mlxsw_sp_qevent_binding *qevent_binding;
2160
2161	list_for_each_entry(qevent_binding, &block->binding_list, list)
2162	if (qevent_binding->mlxsw_sp_port == mlxsw_sp_port &&
2163	qevent_binding->handle == handle &&
2164	qevent_binding->span_trigger == span_trigger)
2165	return qevent_binding;
2166	return NULL;
2167	}
2168
2169	static int
2170	mlxsw_sp_setup_tc_block_qevent_bind(struct mlxsw_sp_port *mlxsw_sp_port,
2171	struct flow_block_offload *f,
2172	enum mlxsw_sp_span_trigger span_trigger,
2173	unsigned int action_mask)
2174	{
2175	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
2176	struct mlxsw_sp_qevent_binding *qevent_binding;
2177	struct mlxsw_sp_qevent_block *qevent_block;
2178	struct flow_block_cb *block_cb;
2179	struct mlxsw_sp_qdisc *qdisc;
2180	bool register_block = false;
2181	int tclass_num;
2182	int err;
2183
2184	block_cb = flow_block_cb_lookup(block: f->block, cb: mlxsw_sp_qevent_block_cb, cb_ident: mlxsw_sp);
2185	if (!block_cb) {
2186	qevent_block = mlxsw_sp_qevent_block_create(mlxsw_sp, net: f->net);
2187	if (!qevent_block)
2188	return -ENOMEM;
2189	block_cb = flow_block_cb_alloc(cb: mlxsw_sp_qevent_block_cb, cb_ident: mlxsw_sp, cb_priv: qevent_block,
2190	release: mlxsw_sp_qevent_block_release);
2191	if (IS_ERR(ptr: block_cb)) {
2192	mlxsw_sp_qevent_block_destroy(qevent_block);
2193	return PTR_ERR(ptr: block_cb);
2194	}
2195	register_block = true;
2196	} else {
2197	qevent_block = flow_block_cb_priv(block_cb);
2198	}
2199	flow_block_cb_incref(block_cb);
2200
2201	qdisc = mlxsw_sp_qdisc_find_by_handle(mlxsw_sp_port, handle: f->sch->handle);
2202	if (!qdisc) {
2203	NL_SET_ERR_MSG(f->extack, "Qdisc not offloaded");
2204	err = -ENOENT;
2205	goto err_find_qdisc;
2206	}
2207
2208	if (WARN_ON(mlxsw_sp_qevent_binding_lookup(qevent_block, mlxsw_sp_port, f->sch->handle,
2209	span_trigger))) {
2210	err = -EEXIST;
2211	goto err_binding_exists;
2212	}
2213
2214	tclass_num = mlxsw_sp_qdisc_get_tclass_num(mlxsw_sp_port, mlxsw_sp_qdisc: qdisc);
2215	qevent_binding = mlxsw_sp_qevent_binding_create(mlxsw_sp_port,
2216	handle: f->sch->handle,
2217	tclass_num,
2218	span_trigger,
2219	action_mask);
2220	if (IS_ERR(ptr: qevent_binding)) {
2221	err = PTR_ERR(ptr: qevent_binding);
2222	goto err_binding_create;
2223	}
2224
2225	err = mlxsw_sp_qevent_binding_configure(qevent_block, qevent_binding,
2226	extack: f->extack);
2227	if (err)
2228	goto err_binding_configure;
2229
2230	list_add(new: &qevent_binding->list, head: &qevent_block->binding_list);
2231
2232	if (register_block) {
2233	flow_block_cb_add(block_cb, offload: f);
2234	list_add_tail(new: &block_cb->driver_list, head: &mlxsw_sp_qevent_block_cb_list);
2235	}
2236
2237	return 0;
2238
2239	err_binding_configure:
2240	mlxsw_sp_qevent_binding_destroy(binding: qevent_binding);
2241	err_binding_create:
2242	err_binding_exists:
2243	err_find_qdisc:
2244	if (!flow_block_cb_decref(block_cb))
2245	flow_block_cb_free(block_cb);
2246	return err;
2247	}
2248
2249	static void mlxsw_sp_setup_tc_block_qevent_unbind(struct mlxsw_sp_port *mlxsw_sp_port,
2250	struct flow_block_offload *f,
2251	enum mlxsw_sp_span_trigger span_trigger)
2252	{
2253	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
2254	struct mlxsw_sp_qevent_binding *qevent_binding;
2255	struct mlxsw_sp_qevent_block *qevent_block;
2256	struct flow_block_cb *block_cb;
2257
2258	block_cb = flow_block_cb_lookup(block: f->block, cb: mlxsw_sp_qevent_block_cb, cb_ident: mlxsw_sp);
2259	if (!block_cb)
2260	return;
2261	qevent_block = flow_block_cb_priv(block_cb);
2262
2263	qevent_binding = mlxsw_sp_qevent_binding_lookup(block: qevent_block, mlxsw_sp_port, handle: f->sch->handle,
2264	span_trigger);
2265	if (!qevent_binding)
2266	return;
2267
2268	list_del(entry: &qevent_binding->list);
2269	mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
2270	mlxsw_sp_qevent_binding_destroy(binding: qevent_binding);
2271
2272	if (!flow_block_cb_decref(block_cb)) {
2273	flow_block_cb_remove(block_cb, offload: f);
2274	list_del(entry: &block_cb->driver_list);
2275	}
2276	}
2277
2278	static int
2279	mlxsw_sp_setup_tc_block_qevent(struct mlxsw_sp_port *mlxsw_sp_port,
2280	struct flow_block_offload *f,
2281	enum mlxsw_sp_span_trigger span_trigger,
2282	unsigned int action_mask)
2283	{
2284	f->driver_block_list = &mlxsw_sp_qevent_block_cb_list;
2285
2286	switch (f->command) {
2287	case FLOW_BLOCK_BIND:
2288	return mlxsw_sp_setup_tc_block_qevent_bind(mlxsw_sp_port, f,
2289	span_trigger,
2290	action_mask);
2291	case FLOW_BLOCK_UNBIND:
2292	mlxsw_sp_setup_tc_block_qevent_unbind(mlxsw_sp_port, f, span_trigger);
2293	return 0;
2294	default:
2295	return -EOPNOTSUPP;
2296	}
2297	}
2298
2299	int mlxsw_sp_setup_tc_block_qevent_early_drop(struct mlxsw_sp_port *mlxsw_sp_port,
2300	struct flow_block_offload *f)
2301	{
2302	unsigned int action_mask = BIT(MLXSW_SP_MALL_ACTION_TYPE_MIRROR) |
2303	BIT(MLXSW_SP_MALL_ACTION_TYPE_TRAP);
2304
2305	return mlxsw_sp_setup_tc_block_qevent(mlxsw_sp_port, f,
2306	span_trigger: MLXSW_SP_SPAN_TRIGGER_EARLY_DROP,
2307	action_mask);
2308	}
2309
2310	int mlxsw_sp_setup_tc_block_qevent_mark(struct mlxsw_sp_port *mlxsw_sp_port,
2311	struct flow_block_offload *f)
2312	{
2313	unsigned int action_mask = BIT(MLXSW_SP_MALL_ACTION_TYPE_MIRROR);
2314
2315	return mlxsw_sp_setup_tc_block_qevent(mlxsw_sp_port, f,
2316	span_trigger: MLXSW_SP_SPAN_TRIGGER_ECN,
2317	action_mask);
2318	}
2319
2320	int mlxsw_sp_tc_qdisc_init(struct mlxsw_sp_port *mlxsw_sp_port)
2321	{
2322	struct mlxsw_sp_qdisc_state *qdisc_state;
2323
2324	qdisc_state = kzalloc(size: sizeof(*qdisc_state), GFP_KERNEL);
2325	if (!qdisc_state)
2326	return -ENOMEM;
2327
2328	mutex_init(&qdisc_state->lock);
2329	mlxsw_sp_port->qdisc = qdisc_state;
2330	return 0;
2331	}
2332
2333	void mlxsw_sp_tc_qdisc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
2334	{
2335	mutex_destroy(lock: &mlxsw_sp_port->qdisc->lock);
2336	kfree(objp: mlxsw_sp_port->qdisc);
2337	}
2338