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(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_state->root_qdisc,
169 				   mlxsw_sp_qdisc_walk_cb_find, &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_state->root_qdisc,
190 				   mlxsw_sp_qdisc_walk_cb_find_by_handle,
191 				   &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, 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, 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);
246 		mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
247 		mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
248 		err_hdroom = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &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->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(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->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(ops->num_classes,
369 						 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);
384 		mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
385 		mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
386 
387 		err = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &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, &orig_hdroom);
410 err_hdroom_configure:
411 	kfree(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_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(stats_ptr->bstats, tx_bytes, tx_packets);
572 	stats_ptr->qstats->drops += drops;
573 	stats_ptr->qstats->backlog += mlxsw_sp_cells_bytes(mlxsw_sp, 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 					&tx_bytes, &tx_packets,
593 					&drops, &backlog);
594 	mlxsw_sp_qdisc_update_stats(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(cwtp_cmd, mlxsw_sp_port->local_port, tclass_num);
610 	mlxsw_reg_cwtp_profile_pack(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_sp->core, MLXSW_REG(cwtp), cwtp_cmd);
616 	if (err)
617 		return err;
618 
619 	mlxsw_reg_cwtpm_pack(cwtpm_cmd, mlxsw_sp_port->local_port, tclass_num,
620 			     MLXSW_REG_CWTP_DEFAULT_PROFILE, is_wred, is_ecn);
621 
622 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(cwtpm), 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(cwtpm_cmd, mlxsw_sp_port->local_port, tclass_num,
633 			     MLXSW_REG_CWTPM_RESET_PROFILE, false, false);
634 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(cwtpm), 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 					       &stats_base->tx_packets,
657 					       &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, 0,
726 						 &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, p->min);
740 	max = mlxsw_sp_bytes_cells(mlxsw_sp, p->max);
741 	return mlxsw_sp_tclass_congestion_enable(mlxsw_sp_port, tclass_num,
742 						 min, max, prob,
743 						 !p->is_nodrop, 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_port->mlxsw_sp,
754 				       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, 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, 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, p->handle,
862 					      mlxsw_sp_qdisc,
863 					      &mlxsw_sp_qdisc_ops_red,
864 					      &p->set);
865 
866 	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, 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 						 p->xstats);
875 	case TC_RED_STATS:
876 		return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
877 						&p->stats);
878 	case TC_RED_GRAFT:
879 		return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc, 0,
880 					    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(&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 					&tx_bytes, &tx_packets,
909 					&drops, &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, tclass_num, 0,
945 					     MLXSW_REG_QEEC_MAS_DIS, 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(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(p->rate.rate_bytes_ps, 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, p->max_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, 0,
1035 						 &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, p->max_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, tclass_num, 0,
1049 					     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, 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, 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, p->handle,
1095 					      mlxsw_sp_qdisc,
1096 					      &mlxsw_sp_qdisc_ops_tbf,
1097 					      &p->replace_params);
1098 
1099 	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, 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 						&p->stats);
1108 	case TC_TBF_GRAFT:
1109 		return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc, 0,
1110 					    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(&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 					      child_qdisc,
1172 					      &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, 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 							 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, p->handle,
1219 					      mlxsw_sp_qdisc,
1220 					      &mlxsw_sp_qdisc_ops_fifo, NULL);
1221 	}
1222 
1223 	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, 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 						&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(&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, i,
1258 					  MLXSW_SP_PORT_DEFAULT_TCLASS);
1259 		mlxsw_sp_port_ets_set(mlxsw_sp_port,
1260 				      MLXSW_REG_QEEC_HR_SUBGROUP,
1261 				      i, 0, false, 0);
1262 	}
1263 
1264 	kfree(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(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(mlxsw_sp_qdisc, mlxsw_sp_qdisc_walk_cb_clean_stats,
1316 			    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(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 					    MLXSW_REG_QEEC_HR_SUBGROUP,
1360 					    tclass_num, 0, !!quanta[band],
1361 					    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 								i, 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 							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, child_qdisc);
1394 
1395 		mlxsw_sp_port_ets_set(mlxsw_sp_port,
1396 				      MLXSW_REG_QEEC_HR_SUBGROUP,
1397 				      ets_band->tclass_num, 0, false, 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, p->bands, zeroes,
1414 					    zeroes, 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_port->mlxsw_sp,
1425 				       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 				       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, tc_qdisc,
1455 						&tx_bytes, &tx_packets,
1456 						&drops, &backlog);
1457 	}
1458 
1459 	mlxsw_sp_qdisc_update_stats(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, 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(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, p->bands, p->quanta,
1559 					    p->weights, 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 				       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 						  child_handle);
1642 	if (old_qdisc)
1643 		mlxsw_sp_qdisc_destroy(mlxsw_sp_port, 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, 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, p->handle,
1665 					      mlxsw_sp_qdisc,
1666 					      &mlxsw_sp_qdisc_ops_prio,
1667 					      &p->replace_params);
1668 
1669 	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, 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 						&p->stats);
1678 	case TC_PRIO_GRAFT:
1679 		return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1680 					    p->graft_params.band,
1681 					    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(&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, 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, p->handle,
1710 					      mlxsw_sp_qdisc,
1711 					      &mlxsw_sp_qdisc_ops_ets,
1712 					      &p->replace_params);
1713 
1714 	if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, 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 						&p->stats);
1723 	case TC_ETS_GRAFT:
1724 		return mlxsw_sp_qdisc_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1725 					    p->graft_params.band,
1726 					    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(&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, &span_id, agent_parms);
1775 	if (err)
1776 		return err;
1777 
1778 	ingress = mlxsw_sp_span_trigger_is_ingress(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, span_trigger, mlxsw_sp_port,
1786 				       &trigger_parms);
1787 	if (err)
1788 		goto err_agent_bind;
1789 
1790 	err = mlxsw_sp_span_trigger_enable(mlxsw_sp_port, span_trigger,
1791 					   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, span_trigger, mlxsw_sp_port,
1800 				   &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(span_trigger);
1820 
1821 	mlxsw_sp_span_trigger_disable(mlxsw_sp_port, span_trigger,
1822 				      qevent_binding->tclass_num);
1823 	mlxsw_sp_span_agent_unbind(mlxsw_sp, span_trigger, mlxsw_sp_port,
1824 				   &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, &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, 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 						    DEVLINK_TRAP_GROUP_GENERIC_ID_BUFFER_DROPS,
1859 						    &agent_parms.policer_enable,
1860 						    &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, &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, 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(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(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(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(&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(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(&mall_entry->list, &qevent_block->mall_entry_list);
2041 
2042 	err = mlxsw_sp_qevent_block_configure(qevent_block, f->common.extack);
2043 	if (err)
2044 		goto err_block_configure;
2045 
2046 	return 0;
2047 
2048 err_block_configure:
2049 	list_del(&mall_entry->list);
2050 err_unsupported_action:
2051 	kfree(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(qevent_block, f->cookie);
2061 	if (!mall_entry)
2062 		return;
2063 
2064 	mlxsw_sp_qevent_block_deconfigure(qevent_block);
2065 
2066 	list_del(&mall_entry->list);
2067 	kfree(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, 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(sizeof(*qevent_block), GFP_KERNEL);
2104 	if (!qevent_block)
2105 		return NULL;
2106 
2107 	INIT_LIST_HEAD(&qevent_block->binding_list);
2108 	INIT_LIST_HEAD(&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(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(sizeof(*binding), GFP_KERNEL);
2136 	if (!binding)
2137 		return ERR_PTR(-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(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(f->block, mlxsw_sp_qevent_block_cb, mlxsw_sp);
2185 	if (!block_cb) {
2186 		qevent_block = mlxsw_sp_qevent_block_create(mlxsw_sp, f->net);
2187 		if (!qevent_block)
2188 			return -ENOMEM;
2189 		block_cb = flow_block_cb_alloc(mlxsw_sp_qevent_block_cb, mlxsw_sp, qevent_block,
2190 					       mlxsw_sp_qevent_block_release);
2191 		if (IS_ERR(block_cb)) {
2192 			mlxsw_sp_qevent_block_destroy(qevent_block);
2193 			return PTR_ERR(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, 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, qdisc);
2215 	qevent_binding = mlxsw_sp_qevent_binding_create(mlxsw_sp_port,
2216 							f->sch->handle,
2217 							tclass_num,
2218 							span_trigger,
2219 							action_mask);
2220 	if (IS_ERR(qevent_binding)) {
2221 		err = PTR_ERR(qevent_binding);
2222 		goto err_binding_create;
2223 	}
2224 
2225 	err = mlxsw_sp_qevent_binding_configure(qevent_block, qevent_binding,
2226 						f->extack);
2227 	if (err)
2228 		goto err_binding_configure;
2229 
2230 	list_add(&qevent_binding->list, &qevent_block->binding_list);
2231 
2232 	if (register_block) {
2233 		flow_block_cb_add(block_cb, f);
2234 		list_add_tail(&block_cb->driver_list, &mlxsw_sp_qevent_block_cb_list);
2235 	}
2236 
2237 	return 0;
2238 
2239 err_binding_configure:
2240 	mlxsw_sp_qevent_binding_destroy(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(f->block, mlxsw_sp_qevent_block_cb, 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(qevent_block, mlxsw_sp_port, f->sch->handle,
2264 							span_trigger);
2265 	if (!qevent_binding)
2266 		return;
2267 
2268 	list_del(&qevent_binding->list);
2269 	mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
2270 	mlxsw_sp_qevent_binding_destroy(qevent_binding);
2271 
2272 	if (!flow_block_cb_decref(block_cb)) {
2273 		flow_block_cb_remove(block_cb, f);
2274 		list_del(&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 					      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 					      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(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(&mlxsw_sp_port->qdisc->lock);
2336 	kfree(mlxsw_sp_port->qdisc);
2337 }
2338