1 // SPDX-License-Identifier: GPL-2.0
2 /* Texas Instruments K3 AM65 Ethernet QoS submodule
3  * Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/
4  *
5  * quality of service module includes:
6  * Enhanced Scheduler Traffic (EST - P802.1Qbv/D2.2)
7  */
8 
9 #include <linux/pm_runtime.h>
10 #include <linux/time.h>
11 #include <net/pkt_cls.h>
12 
13 #include "am65-cpsw-nuss.h"
14 #include "am65-cpsw-qos.h"
15 #include "am65-cpts.h"
16 #include "cpsw_ale.h"
17 
18 #define AM65_CPSW_REG_CTL			0x004
19 #define AM65_CPSW_PN_REG_CTL			0x004
20 #define AM65_CPSW_PN_REG_FIFO_STATUS		0x050
21 #define AM65_CPSW_PN_REG_EST_CTL		0x060
22 
23 /* AM65_CPSW_REG_CTL register fields */
24 #define AM65_CPSW_CTL_EST_EN			BIT(18)
25 
26 /* AM65_CPSW_PN_REG_CTL register fields */
27 #define AM65_CPSW_PN_CTL_EST_PORT_EN		BIT(17)
28 
29 /* AM65_CPSW_PN_REG_EST_CTL register fields */
30 #define AM65_CPSW_PN_EST_ONEBUF			BIT(0)
31 #define AM65_CPSW_PN_EST_BUFSEL			BIT(1)
32 #define AM65_CPSW_PN_EST_TS_EN			BIT(2)
33 #define AM65_CPSW_PN_EST_TS_FIRST		BIT(3)
34 #define AM65_CPSW_PN_EST_ONEPRI			BIT(4)
35 #define AM65_CPSW_PN_EST_TS_PRI_MSK		GENMASK(7, 5)
36 
37 /* AM65_CPSW_PN_REG_FIFO_STATUS register fields */
38 #define AM65_CPSW_PN_FST_TX_PRI_ACTIVE_MSK	GENMASK(7, 0)
39 #define AM65_CPSW_PN_FST_TX_E_MAC_ALLOW_MSK	GENMASK(15, 8)
40 #define AM65_CPSW_PN_FST_EST_CNT_ERR		BIT(16)
41 #define AM65_CPSW_PN_FST_EST_ADD_ERR		BIT(17)
42 #define AM65_CPSW_PN_FST_EST_BUFACT		BIT(18)
43 
44 /* EST FETCH COMMAND RAM */
45 #define AM65_CPSW_FETCH_RAM_CMD_NUM		0x80
46 #define AM65_CPSW_FETCH_CNT_MSK			GENMASK(21, 8)
47 #define AM65_CPSW_FETCH_CNT_MAX			(AM65_CPSW_FETCH_CNT_MSK >> 8)
48 #define AM65_CPSW_FETCH_CNT_OFFSET		8
49 #define AM65_CPSW_FETCH_ALLOW_MSK		GENMASK(7, 0)
50 #define AM65_CPSW_FETCH_ALLOW_MAX		AM65_CPSW_FETCH_ALLOW_MSK
51 
52 enum timer_act {
53 	TACT_PROG,		/* need program timer */
54 	TACT_NEED_STOP,		/* need stop first */
55 	TACT_SKIP_PROG,		/* just buffer can be updated */
56 };
57 
58 static int am65_cpsw_port_est_enabled(struct am65_cpsw_port *port)
59 {
60 	return port->qos.est_oper || port->qos.est_admin;
61 }
62 
63 static void am65_cpsw_est_enable(struct am65_cpsw_common *common, int enable)
64 {
65 	u32 val;
66 
67 	val = readl(common->cpsw_base + AM65_CPSW_REG_CTL);
68 
69 	if (enable)
70 		val |= AM65_CPSW_CTL_EST_EN;
71 	else
72 		val &= ~AM65_CPSW_CTL_EST_EN;
73 
74 	writel(val, common->cpsw_base + AM65_CPSW_REG_CTL);
75 	common->est_enabled = enable;
76 }
77 
78 static void am65_cpsw_port_est_enable(struct am65_cpsw_port *port, int enable)
79 {
80 	u32 val;
81 
82 	val = readl(port->port_base + AM65_CPSW_PN_REG_CTL);
83 	if (enable)
84 		val |= AM65_CPSW_PN_CTL_EST_PORT_EN;
85 	else
86 		val &= ~AM65_CPSW_PN_CTL_EST_PORT_EN;
87 
88 	writel(val, port->port_base + AM65_CPSW_PN_REG_CTL);
89 }
90 
91 /* target new EST RAM buffer, actual toggle happens after cycle completion */
92 static void am65_cpsw_port_est_assign_buf_num(struct net_device *ndev,
93 					      int buf_num)
94 {
95 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
96 	u32 val;
97 
98 	val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL);
99 	if (buf_num)
100 		val |= AM65_CPSW_PN_EST_BUFSEL;
101 	else
102 		val &= ~AM65_CPSW_PN_EST_BUFSEL;
103 
104 	writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL);
105 }
106 
107 /* am65_cpsw_port_est_is_swapped() - Indicate if h/w is transitioned
108  * admin -> oper or not
109  *
110  * Return true if already transitioned. i.e oper is equal to admin and buf
111  * numbers match (est_oper->buf match with est_admin->buf).
112  * false if before transition. i.e oper is not equal to admin, (i.e a
113  * previous admin command is waiting to be transitioned to oper state
114  * and est_oper->buf not match with est_oper->buf).
115  */
116 static int am65_cpsw_port_est_is_swapped(struct net_device *ndev, int *oper,
117 					 int *admin)
118 {
119 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
120 	u32 val;
121 
122 	val = readl(port->port_base + AM65_CPSW_PN_REG_FIFO_STATUS);
123 	*oper = !!(val & AM65_CPSW_PN_FST_EST_BUFACT);
124 
125 	val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL);
126 	*admin = !!(val & AM65_CPSW_PN_EST_BUFSEL);
127 
128 	return *admin == *oper;
129 }
130 
131 /* am65_cpsw_port_est_get_free_buf_num() - Get free buffer number for
132  * Admin to program the new schedule.
133  *
134  * Logic as follows:-
135  * If oper is same as admin, return the other buffer (!oper) as the admin
136  * buffer.  If oper is not the same, driver let the current oper to continue
137  * as it is in the process of transitioning from admin -> oper. So keep the
138  * oper by selecting the same oper buffer by writing to EST_BUFSEL bit in
139  * EST CTL register. In the second iteration they will match and code returns.
140  * The actual buffer to write command is selected later before it is ready
141  * to update the schedule.
142  */
143 static int am65_cpsw_port_est_get_free_buf_num(struct net_device *ndev)
144 {
145 	int oper, admin;
146 	int roll = 2;
147 
148 	while (roll--) {
149 		if (am65_cpsw_port_est_is_swapped(ndev, &oper, &admin))
150 			return !oper;
151 
152 		/* admin is not set, so hinder transition as it's not allowed
153 		 * to touch memory in-flight, by targeting same oper buf.
154 		 */
155 		am65_cpsw_port_est_assign_buf_num(ndev, oper);
156 
157 		dev_info(&ndev->dev,
158 			 "Prev. EST admin cycle is in transit %d -> %d\n",
159 			 oper, admin);
160 	}
161 
162 	return admin;
163 }
164 
165 static void am65_cpsw_admin_to_oper(struct net_device *ndev)
166 {
167 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
168 
169 	devm_kfree(&ndev->dev, port->qos.est_oper);
170 
171 	port->qos.est_oper = port->qos.est_admin;
172 	port->qos.est_admin = NULL;
173 }
174 
175 static void am65_cpsw_port_est_get_buf_num(struct net_device *ndev,
176 					   struct am65_cpsw_est *est_new)
177 {
178 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
179 	u32 val;
180 
181 	val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL);
182 	val &= ~AM65_CPSW_PN_EST_ONEBUF;
183 	writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL);
184 
185 	est_new->buf = am65_cpsw_port_est_get_free_buf_num(ndev);
186 
187 	/* rolled buf num means changed buf while configuring */
188 	if (port->qos.est_oper && port->qos.est_admin &&
189 	    est_new->buf == port->qos.est_oper->buf)
190 		am65_cpsw_admin_to_oper(ndev);
191 }
192 
193 static void am65_cpsw_est_set(struct net_device *ndev, int enable)
194 {
195 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
196 	struct am65_cpsw_common *common = port->common;
197 	int common_enable = 0;
198 	int i;
199 
200 	am65_cpsw_port_est_enable(port, enable);
201 
202 	for (i = 0; i < common->port_num; i++)
203 		common_enable |= am65_cpsw_port_est_enabled(&common->ports[i]);
204 
205 	common_enable |= enable;
206 	am65_cpsw_est_enable(common, common_enable);
207 }
208 
209 /* This update is supposed to be used in any routine before getting real state
210  * of admin -> oper transition, particularly it's supposed to be used in some
211  * generic routine for providing real state to Taprio Qdisc.
212  */
213 static void am65_cpsw_est_update_state(struct net_device *ndev)
214 {
215 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
216 	int oper, admin;
217 
218 	if (!port->qos.est_admin)
219 		return;
220 
221 	if (!am65_cpsw_port_est_is_swapped(ndev, &oper, &admin))
222 		return;
223 
224 	am65_cpsw_admin_to_oper(ndev);
225 }
226 
227 /* Fetch command count it's number of bytes in Gigabit mode or nibbles in
228  * 10/100Mb mode. So, having speed and time in ns, recalculate ns to number of
229  * bytes/nibbles that can be sent while transmission on given speed.
230  */
231 static int am65_est_cmd_ns_to_cnt(u64 ns, int link_speed)
232 {
233 	u64 temp;
234 
235 	temp = ns * link_speed;
236 	if (link_speed < SPEED_1000)
237 		temp <<= 1;
238 
239 	return DIV_ROUND_UP(temp, 8 * 1000);
240 }
241 
242 static void __iomem *am65_cpsw_est_set_sched_cmds(void __iomem *addr,
243 						  int fetch_cnt,
244 						  int fetch_allow)
245 {
246 	u32 prio_mask, cmd_fetch_cnt, cmd;
247 
248 	do {
249 		if (fetch_cnt > AM65_CPSW_FETCH_CNT_MAX) {
250 			fetch_cnt -= AM65_CPSW_FETCH_CNT_MAX;
251 			cmd_fetch_cnt = AM65_CPSW_FETCH_CNT_MAX;
252 		} else {
253 			cmd_fetch_cnt = fetch_cnt;
254 			/* fetch count can't be less than 16? */
255 			if (cmd_fetch_cnt && cmd_fetch_cnt < 16)
256 				cmd_fetch_cnt = 16;
257 
258 			fetch_cnt = 0;
259 		}
260 
261 		prio_mask = fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK;
262 		cmd = (cmd_fetch_cnt << AM65_CPSW_FETCH_CNT_OFFSET) | prio_mask;
263 
264 		writel(cmd, addr);
265 		addr += 4;
266 	} while (fetch_cnt);
267 
268 	return addr;
269 }
270 
271 static int am65_cpsw_est_calc_cmd_num(struct net_device *ndev,
272 				      struct tc_taprio_qopt_offload *taprio,
273 				      int link_speed)
274 {
275 	int i, cmd_cnt, cmd_sum = 0;
276 	u32 fetch_cnt;
277 
278 	for (i = 0; i < taprio->num_entries; i++) {
279 		if (taprio->entries[i].command != TC_TAPRIO_CMD_SET_GATES) {
280 			dev_err(&ndev->dev, "Only SET command is supported");
281 			return -EINVAL;
282 		}
283 
284 		fetch_cnt = am65_est_cmd_ns_to_cnt(taprio->entries[i].interval,
285 						   link_speed);
286 
287 		cmd_cnt = DIV_ROUND_UP(fetch_cnt, AM65_CPSW_FETCH_CNT_MAX);
288 		if (!cmd_cnt)
289 			cmd_cnt++;
290 
291 		cmd_sum += cmd_cnt;
292 
293 		if (!fetch_cnt)
294 			break;
295 	}
296 
297 	return cmd_sum;
298 }
299 
300 static int am65_cpsw_est_check_scheds(struct net_device *ndev,
301 				      struct am65_cpsw_est *est_new)
302 {
303 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
304 	int cmd_num;
305 
306 	cmd_num = am65_cpsw_est_calc_cmd_num(ndev, &est_new->taprio,
307 					     port->qos.link_speed);
308 	if (cmd_num < 0)
309 		return cmd_num;
310 
311 	if (cmd_num > AM65_CPSW_FETCH_RAM_CMD_NUM / 2) {
312 		dev_err(&ndev->dev, "No fetch RAM");
313 		return -ENOMEM;
314 	}
315 
316 	return 0;
317 }
318 
319 static void am65_cpsw_est_set_sched_list(struct net_device *ndev,
320 					 struct am65_cpsw_est *est_new)
321 {
322 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
323 	u32 fetch_cnt, fetch_allow, all_fetch_allow = 0;
324 	void __iomem *ram_addr, *max_ram_addr;
325 	struct tc_taprio_sched_entry *entry;
326 	int i, ram_size;
327 
328 	ram_addr = port->fetch_ram_base;
329 	ram_size = AM65_CPSW_FETCH_RAM_CMD_NUM * 2;
330 	ram_addr += est_new->buf * ram_size;
331 
332 	max_ram_addr = ram_size + ram_addr;
333 	for (i = 0; i < est_new->taprio.num_entries; i++) {
334 		entry = &est_new->taprio.entries[i];
335 
336 		fetch_cnt = am65_est_cmd_ns_to_cnt(entry->interval,
337 						   port->qos.link_speed);
338 		fetch_allow = entry->gate_mask;
339 		if (fetch_allow > AM65_CPSW_FETCH_ALLOW_MAX)
340 			dev_dbg(&ndev->dev, "fetch_allow > 8 bits: %d\n",
341 				fetch_allow);
342 
343 		ram_addr = am65_cpsw_est_set_sched_cmds(ram_addr, fetch_cnt,
344 							fetch_allow);
345 
346 		if (!fetch_cnt && i < est_new->taprio.num_entries - 1) {
347 			dev_info(&ndev->dev,
348 				 "next scheds after %d have no impact", i + 1);
349 			break;
350 		}
351 
352 		all_fetch_allow |= fetch_allow;
353 	}
354 
355 	/* end cmd, enabling non-timed queues for potential over cycle time */
356 	if (ram_addr < max_ram_addr)
357 		writel(~all_fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK, ram_addr);
358 }
359 
360 /*
361  * Enable ESTf periodic output, set cycle start time and interval.
362  */
363 static int am65_cpsw_timer_set(struct net_device *ndev,
364 			       struct am65_cpsw_est *est_new)
365 {
366 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
367 	struct am65_cpsw_common *common = port->common;
368 	struct am65_cpts *cpts = common->cpts;
369 	struct am65_cpts_estf_cfg cfg;
370 
371 	cfg.ns_period = est_new->taprio.cycle_time;
372 	cfg.ns_start = est_new->taprio.base_time;
373 
374 	return am65_cpts_estf_enable(cpts, port->port_id - 1, &cfg);
375 }
376 
377 static void am65_cpsw_timer_stop(struct net_device *ndev)
378 {
379 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
380 	struct am65_cpts *cpts = port->common->cpts;
381 
382 	am65_cpts_estf_disable(cpts, port->port_id - 1);
383 }
384 
385 static enum timer_act am65_cpsw_timer_act(struct net_device *ndev,
386 					  struct am65_cpsw_est *est_new)
387 {
388 	struct tc_taprio_qopt_offload *taprio_oper, *taprio_new;
389 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
390 	struct am65_cpts *cpts = port->common->cpts;
391 	u64 cur_time;
392 	s64 diff;
393 
394 	if (!port->qos.est_oper)
395 		return TACT_PROG;
396 
397 	taprio_new = &est_new->taprio;
398 	taprio_oper = &port->qos.est_oper->taprio;
399 
400 	if (taprio_new->cycle_time != taprio_oper->cycle_time)
401 		return TACT_NEED_STOP;
402 
403 	/* in order to avoid timer reset get base_time form oper taprio */
404 	if (!taprio_new->base_time && taprio_oper)
405 		taprio_new->base_time = taprio_oper->base_time;
406 
407 	if (taprio_new->base_time == taprio_oper->base_time)
408 		return TACT_SKIP_PROG;
409 
410 	/* base times are cycle synchronized */
411 	diff = taprio_new->base_time - taprio_oper->base_time;
412 	diff = diff < 0 ? -diff : diff;
413 	if (diff % taprio_new->cycle_time)
414 		return TACT_NEED_STOP;
415 
416 	cur_time = am65_cpts_ns_gettime(cpts);
417 	if (taprio_new->base_time <= cur_time + taprio_new->cycle_time)
418 		return TACT_SKIP_PROG;
419 
420 	/* TODO: Admin schedule at future time is not currently supported */
421 	return TACT_NEED_STOP;
422 }
423 
424 static void am65_cpsw_stop_est(struct net_device *ndev)
425 {
426 	am65_cpsw_est_set(ndev, 0);
427 	am65_cpsw_timer_stop(ndev);
428 }
429 
430 static void am65_cpsw_purge_est(struct net_device *ndev)
431 {
432 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
433 
434 	am65_cpsw_stop_est(ndev);
435 
436 	devm_kfree(&ndev->dev, port->qos.est_admin);
437 	devm_kfree(&ndev->dev, port->qos.est_oper);
438 
439 	port->qos.est_oper = NULL;
440 	port->qos.est_admin = NULL;
441 }
442 
443 static int am65_cpsw_configure_taprio(struct net_device *ndev,
444 				      struct am65_cpsw_est *est_new)
445 {
446 	struct am65_cpsw_common *common = am65_ndev_to_common(ndev);
447 	struct am65_cpts *cpts = common->cpts;
448 	int ret = 0, tact = TACT_PROG;
449 
450 	am65_cpsw_est_update_state(ndev);
451 
452 	if (!est_new->taprio.enable) {
453 		am65_cpsw_stop_est(ndev);
454 		return ret;
455 	}
456 
457 	ret = am65_cpsw_est_check_scheds(ndev, est_new);
458 	if (ret < 0)
459 		return ret;
460 
461 	tact = am65_cpsw_timer_act(ndev, est_new);
462 	if (tact == TACT_NEED_STOP) {
463 		dev_err(&ndev->dev,
464 			"Can't toggle estf timer, stop taprio first");
465 		return -EINVAL;
466 	}
467 
468 	if (tact == TACT_PROG)
469 		am65_cpsw_timer_stop(ndev);
470 
471 	if (!est_new->taprio.base_time)
472 		est_new->taprio.base_time = am65_cpts_ns_gettime(cpts);
473 
474 	am65_cpsw_port_est_get_buf_num(ndev, est_new);
475 	am65_cpsw_est_set_sched_list(ndev, est_new);
476 	am65_cpsw_port_est_assign_buf_num(ndev, est_new->buf);
477 
478 	am65_cpsw_est_set(ndev, est_new->taprio.enable);
479 
480 	if (tact == TACT_PROG) {
481 		ret = am65_cpsw_timer_set(ndev, est_new);
482 		if (ret) {
483 			dev_err(&ndev->dev, "Failed to set cycle time");
484 			return ret;
485 		}
486 	}
487 
488 	return ret;
489 }
490 
491 static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from,
492 				struct tc_taprio_qopt_offload *to)
493 {
494 	int i;
495 
496 	*to = *from;
497 	for (i = 0; i < from->num_entries; i++)
498 		to->entries[i] = from->entries[i];
499 }
500 
501 static int am65_cpsw_set_taprio(struct net_device *ndev, void *type_data)
502 {
503 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
504 	struct tc_taprio_qopt_offload *taprio = type_data;
505 	struct am65_cpsw_est *est_new;
506 	int ret = 0;
507 
508 	if (taprio->cycle_time_extension) {
509 		dev_err(&ndev->dev, "Failed to set cycle time extension");
510 		return -EOPNOTSUPP;
511 	}
512 
513 	est_new = devm_kzalloc(&ndev->dev,
514 			       struct_size(est_new, taprio.entries, taprio->num_entries),
515 			       GFP_KERNEL);
516 	if (!est_new)
517 		return -ENOMEM;
518 
519 	am65_cpsw_cp_taprio(taprio, &est_new->taprio);
520 	ret = am65_cpsw_configure_taprio(ndev, est_new);
521 	if (!ret) {
522 		if (taprio->enable) {
523 			devm_kfree(&ndev->dev, port->qos.est_admin);
524 
525 			port->qos.est_admin = est_new;
526 		} else {
527 			devm_kfree(&ndev->dev, est_new);
528 			am65_cpsw_purge_est(ndev);
529 		}
530 	} else {
531 		devm_kfree(&ndev->dev, est_new);
532 	}
533 
534 	return ret;
535 }
536 
537 static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed)
538 {
539 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
540 	ktime_t cur_time;
541 	s64 delta;
542 
543 	port->qos.link_speed = link_speed;
544 	if (!am65_cpsw_port_est_enabled(port))
545 		return;
546 
547 	if (port->qos.link_down_time) {
548 		cur_time = ktime_get();
549 		delta = ktime_us_delta(cur_time, port->qos.link_down_time);
550 		if (delta > USEC_PER_SEC) {
551 			dev_err(&ndev->dev,
552 				"Link has been lost too long, stopping TAS");
553 			goto purge_est;
554 		}
555 	}
556 
557 	return;
558 
559 purge_est:
560 	am65_cpsw_purge_est(ndev);
561 }
562 
563 static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data)
564 {
565 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
566 	struct am65_cpsw_common *common = port->common;
567 
568 	if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS))
569 		return -ENODEV;
570 
571 	if (!netif_running(ndev)) {
572 		dev_err(&ndev->dev, "interface is down, link speed unknown\n");
573 		return -ENETDOWN;
574 	}
575 
576 	if (common->pf_p0_rx_ptype_rrobin) {
577 		dev_err(&ndev->dev,
578 			"p0-rx-ptype-rrobin flag conflicts with taprio qdisc\n");
579 		return -EINVAL;
580 	}
581 
582 	if (port->qos.link_speed == SPEED_UNKNOWN)
583 		return -ENOLINK;
584 
585 	return am65_cpsw_set_taprio(ndev, type_data);
586 }
587 
588 static int am65_cpsw_tc_query_caps(struct net_device *ndev, void *type_data)
589 {
590 	struct tc_query_caps_base *base = type_data;
591 
592 	switch (base->type) {
593 	case TC_SETUP_QDISC_TAPRIO: {
594 		struct tc_taprio_caps *caps = base->caps;
595 
596 		if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS))
597 			return -EOPNOTSUPP;
598 
599 		caps->gate_mask_per_txq = true;
600 
601 		return 0;
602 	}
603 	default:
604 		return -EOPNOTSUPP;
605 	}
606 }
607 
608 static int am65_cpsw_qos_clsflower_add_policer(struct am65_cpsw_port *port,
609 					       struct netlink_ext_ack *extack,
610 					       struct flow_cls_offload *cls,
611 					       u64 rate_pkt_ps)
612 {
613 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
614 	struct flow_dissector *dissector = rule->match.dissector;
615 	static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00};
616 	struct am65_cpsw_qos *qos = &port->qos;
617 	struct flow_match_eth_addrs match;
618 	int ret;
619 
620 	if (dissector->used_keys &
621 	    ~(BIT(FLOW_DISSECTOR_KEY_BASIC) |
622 	      BIT(FLOW_DISSECTOR_KEY_CONTROL) |
623 	      BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {
624 		NL_SET_ERR_MSG_MOD(extack,
625 				   "Unsupported keys used");
626 		return -EOPNOTSUPP;
627 	}
628 
629 	if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
630 		NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address");
631 		return -EOPNOTSUPP;
632 	}
633 
634 	flow_rule_match_eth_addrs(rule, &match);
635 
636 	if (!is_zero_ether_addr(match.mask->src)) {
637 		NL_SET_ERR_MSG_MOD(extack,
638 				   "Matching on source MAC not supported");
639 		return -EOPNOTSUPP;
640 	}
641 
642 	if (is_broadcast_ether_addr(match.key->dst) &&
643 	    is_broadcast_ether_addr(match.mask->dst)) {
644 		ret = cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, rate_pkt_ps);
645 		if (ret)
646 			return ret;
647 
648 		qos->ale_bc_ratelimit.cookie = cls->cookie;
649 		qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps;
650 	} else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) &&
651 		   ether_addr_equal_unaligned(match.mask->dst, mc_mac)) {
652 		ret = cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, rate_pkt_ps);
653 		if (ret)
654 			return ret;
655 
656 		qos->ale_mc_ratelimit.cookie = cls->cookie;
657 		qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps;
658 	} else {
659 		NL_SET_ERR_MSG_MOD(extack, "Not supported matching key");
660 		return -EOPNOTSUPP;
661 	}
662 
663 	return 0;
664 }
665 
666 static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action,
667 						    const struct flow_action_entry *act,
668 						    struct netlink_ext_ack *extack)
669 {
670 	if (act->police.exceed.act_id != FLOW_ACTION_DROP) {
671 		NL_SET_ERR_MSG_MOD(extack,
672 				   "Offload not supported when exceed action is not drop");
673 		return -EOPNOTSUPP;
674 	}
675 
676 	if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&
677 	    act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {
678 		NL_SET_ERR_MSG_MOD(extack,
679 				   "Offload not supported when conform action is not pipe or ok");
680 		return -EOPNOTSUPP;
681 	}
682 
683 	if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT &&
684 	    !flow_action_is_last_entry(action, act)) {
685 		NL_SET_ERR_MSG_MOD(extack,
686 				   "Offload not supported when conform action is ok, but action is not last");
687 		return -EOPNOTSUPP;
688 	}
689 
690 	if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps ||
691 	    act->police.avrate || act->police.overhead) {
692 		NL_SET_ERR_MSG_MOD(extack,
693 				   "Offload not supported when bytes per second/peakrate/avrate/overhead is configured");
694 		return -EOPNOTSUPP;
695 	}
696 
697 	return 0;
698 }
699 
700 static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port,
701 					     struct flow_cls_offload *cls)
702 {
703 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
704 	struct netlink_ext_ack *extack = cls->common.extack;
705 	const struct flow_action_entry *act;
706 	int i, ret;
707 
708 	flow_action_for_each(i, act, &rule->action) {
709 		switch (act->id) {
710 		case FLOW_ACTION_POLICE:
711 			ret = am65_cpsw_qos_clsflower_policer_validate(&rule->action, act, extack);
712 			if (ret)
713 				return ret;
714 
715 			return am65_cpsw_qos_clsflower_add_policer(port, extack, cls,
716 								   act->police.rate_pkt_ps);
717 		default:
718 			NL_SET_ERR_MSG_MOD(extack,
719 					   "Action not supported");
720 			return -EOPNOTSUPP;
721 		}
722 	}
723 	return -EOPNOTSUPP;
724 }
725 
726 static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls)
727 {
728 	struct am65_cpsw_qos *qos = &port->qos;
729 
730 	if (cls->cookie == qos->ale_bc_ratelimit.cookie) {
731 		qos->ale_bc_ratelimit.cookie = 0;
732 		qos->ale_bc_ratelimit.rate_packet_ps = 0;
733 		cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, 0);
734 	}
735 
736 	if (cls->cookie == qos->ale_mc_ratelimit.cookie) {
737 		qos->ale_mc_ratelimit.cookie = 0;
738 		qos->ale_mc_ratelimit.rate_packet_ps = 0;
739 		cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, 0);
740 	}
741 
742 	return 0;
743 }
744 
745 static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port,
746 					    struct flow_cls_offload *cls_flower)
747 {
748 	switch (cls_flower->command) {
749 	case FLOW_CLS_REPLACE:
750 		return am65_cpsw_qos_configure_clsflower(port, cls_flower);
751 	case FLOW_CLS_DESTROY:
752 		return am65_cpsw_qos_delete_clsflower(port, cls_flower);
753 	default:
754 		return -EOPNOTSUPP;
755 	}
756 }
757 
758 static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
759 {
760 	struct am65_cpsw_port *port = cb_priv;
761 
762 	if (!tc_cls_can_offload_and_chain0(port->ndev, type_data))
763 		return -EOPNOTSUPP;
764 
765 	switch (type) {
766 	case TC_SETUP_CLSFLOWER:
767 		return am65_cpsw_qos_setup_tc_clsflower(port, type_data);
768 	default:
769 		return -EOPNOTSUPP;
770 	}
771 }
772 
773 static LIST_HEAD(am65_cpsw_qos_block_cb_list);
774 
775 static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f)
776 {
777 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
778 
779 	return flow_block_cb_setup_simple(f, &am65_cpsw_qos_block_cb_list,
780 					  am65_cpsw_qos_setup_tc_block_cb,
781 					  port, port, true);
782 }
783 
784 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
785 			       void *type_data)
786 {
787 	switch (type) {
788 	case TC_QUERY_CAPS:
789 		return am65_cpsw_tc_query_caps(ndev, type_data);
790 	case TC_SETUP_QDISC_TAPRIO:
791 		return am65_cpsw_setup_taprio(ndev, type_data);
792 	case TC_SETUP_BLOCK:
793 		return am65_cpsw_qos_setup_tc_block(ndev, type_data);
794 	default:
795 		return -EOPNOTSUPP;
796 	}
797 }
798 
799 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed)
800 {
801 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
802 
803 	if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS))
804 		return;
805 
806 	am65_cpsw_est_link_up(ndev, link_speed);
807 	port->qos.link_down_time = 0;
808 }
809 
810 void am65_cpsw_qos_link_down(struct net_device *ndev)
811 {
812 	struct am65_cpsw_port *port = am65_ndev_to_port(ndev);
813 
814 	if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS))
815 		return;
816 
817 	if (!port->qos.link_down_time)
818 		port->qos.link_down_time = ktime_get();
819 
820 	port->qos.link_speed = SPEED_UNKNOWN;
821 }
822