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