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