1// SPDX-License-Identifier: GPL-2.0+
 2
 3#include "lan966x_main.h"
 4
 5#define DWRR_COST_BIT_WIDTH	BIT(5)
 6
 7static 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
18int 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
77int 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}