1// SPDX-License-Identifier: GPL-2.0+
2
3#include "lan966x_main.h"
4
5int lan966x_tbf_add(struct lan966x_port *port,
6		    struct tc_tbf_qopt_offload *qopt)
7{
8	struct lan966x *lan966x = port->lan966x;
9	bool root = qopt->parent == TC_H_ROOT;
10	u32 queue = 0;
11	u32 cir, cbs;
12	u32 se_idx;
13
14	if (!root) {
15		queue = TC_H_MIN(qopt->parent) - 1;
16		if (queue >= NUM_PRIO_QUEUES)
17			return -EOPNOTSUPP;
18	}
19
20	if (root)
21		se_idx = SE_IDX_PORT + port->chip_port;
22	else
23		se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + queue;
24
25	cir = div_u64(qopt->replace_params.rate.rate_bytes_ps, 1000) * 8;
26	cbs = qopt->replace_params.max_size;
27
28	/* Rate unit is 100 kbps */
29	cir = DIV_ROUND_UP(cir, 100);
30	/* Avoid using zero rate */
31	cir = cir ?: 1;
32	/* Burst unit is 4kB */
33	cbs = DIV_ROUND_UP(cbs, 4096);
34	/* Avoid using zero burst */
35	cbs = cbs ?: 1;
36
37	/* Check that actually the result can be written */
38	if (cir > GENMASK(15, 0) ||
39	    cbs > GENMASK(6, 0))
40		return -EINVAL;
41
42	lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(0) |
43		QSYS_SE_CFG_SE_FRM_MODE_SET(1),
44		QSYS_SE_CFG_SE_AVB_ENA |
45		QSYS_SE_CFG_SE_FRM_MODE,
46		lan966x, QSYS_SE_CFG(se_idx));
47
48	lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(cir) |
49	       QSYS_CIR_CFG_CIR_BURST_SET(cbs),
50	       lan966x, QSYS_CIR_CFG(se_idx));
51
52	return 0;
53}
54
55int lan966x_tbf_del(struct lan966x_port *port,
56		    struct tc_tbf_qopt_offload *qopt)
57{
58	struct lan966x *lan966x = port->lan966x;
59	bool root = qopt->parent == TC_H_ROOT;
60	u32 queue = 0;
61	u32 se_idx;
62
63	if (!root) {
64		queue = TC_H_MIN(qopt->parent) - 1;
65		if (queue >= NUM_PRIO_QUEUES)
66			return -EOPNOTSUPP;
67	}
68
69	if (root)
70		se_idx = SE_IDX_PORT + port->chip_port;
71	else
72		se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + queue;
73
74	lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(0) |
75		QSYS_SE_CFG_SE_FRM_MODE_SET(0),
76		QSYS_SE_CFG_SE_AVB_ENA |
77		QSYS_SE_CFG_SE_FRM_MODE,
78		lan966x, QSYS_SE_CFG(se_idx));
79
80	lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(0) |
81	       QSYS_CIR_CFG_CIR_BURST_SET(0),
82	       lan966x, QSYS_CIR_CFG(se_idx));
83
84	return 0;
85}
86