1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include "lan966x_main.h"
4 
5 #define DWRR_COST_BIT_WIDTH	BIT(5)
6 
7 static u32 lan966x_ets_hw_cost(u32 w_min, u32 weight)
8 {
9 	u32 res;
10 
11 	/* Round half up: Multiply with 16 before division,
12 	 * add 8 and divide result with 16 again
13 	 */
14 	res = (((DWRR_COST_BIT_WIDTH << 4) * w_min / weight) + 8) >> 4;
15 	return max_t(u32, 1, res) - 1;
16 }
17 
18 int lan966x_ets_add(struct lan966x_port *port,
19 		    struct tc_ets_qopt_offload *qopt)
20 {
21 	struct tc_ets_qopt_offload_replace_params *params;
22 	struct lan966x *lan966x = port->lan966x;
23 	u32 w_min = 100;
24 	u8 count = 0;
25 	u32 se_idx;
26 	u8 i;
27 
28 	/* Check the input */
29 	if (qopt->parent != TC_H_ROOT)
30 		return -EINVAL;
31 
32 	params = &qopt->replace_params;
33 	if (params->bands != NUM_PRIO_QUEUES)
34 		return -EINVAL;
35 
36 	for (i = 0; i < params->bands; ++i) {
37 		/* In the switch the DWRR is always on the lowest consecutive
38 		 * priorities. Due to this, the first priority must map to the
39 		 * first DWRR band.
40 		 */
41 		if (params->priomap[i] != (7 - i))
42 			return -EINVAL;
43 
44 		if (params->quanta[i] && params->weights[i] == 0)
45 			return -EINVAL;
46 	}
47 
48 	se_idx = SE_IDX_PORT + port->chip_port;
49 
50 	/* Find minimum weight */
51 	for (i = 0; i < params->bands; ++i) {
52 		if (params->quanta[i] == 0)
53 			continue;
54 
55 		w_min = min(w_min, params->weights[i]);
56 	}
57 
58 	for (i = 0; i < params->bands; ++i) {
59 		if (params->quanta[i] == 0)
60 			continue;
61 
62 		++count;
63 
64 		lan_wr(lan966x_ets_hw_cost(w_min, params->weights[i]),
65 		       lan966x, QSYS_SE_DWRR_CFG(se_idx, 7 - i));
66 	}
67 
68 	lan_rmw(QSYS_SE_CFG_SE_DWRR_CNT_SET(count) |
69 		QSYS_SE_CFG_SE_RR_ENA_SET(0),
70 		QSYS_SE_CFG_SE_DWRR_CNT |
71 		QSYS_SE_CFG_SE_RR_ENA,
72 		lan966x, QSYS_SE_CFG(se_idx));
73 
74 	return 0;
75 }
76 
77 int lan966x_ets_del(struct lan966x_port *port,
78 		    struct tc_ets_qopt_offload *qopt)
79 {
80 	struct lan966x *lan966x = port->lan966x;
81 	u32 se_idx;
82 	int i;
83 
84 	se_idx = SE_IDX_PORT + port->chip_port;
85 
86 	for (i = 0; i < NUM_PRIO_QUEUES; ++i)
87 		lan_wr(0, lan966x, QSYS_SE_DWRR_CFG(se_idx, i));
88 
89 	lan_rmw(QSYS_SE_CFG_SE_DWRR_CNT_SET(0) |
90 		QSYS_SE_CFG_SE_RR_ENA_SET(0),
91 		QSYS_SE_CFG_SE_DWRR_CNT |
92 		QSYS_SE_CFG_SE_RR_ENA,
93 		lan966x, QSYS_SE_CFG(se_idx));
94 
95 	return 0;
96 }
97