1// SPDX-License-Identifier: GPL-2.0+
 2
 3#include "lan966x_main.h"
 4
 5int lan966x_cbs_add(struct lan966x_port *port,
 6		    struct tc_cbs_qopt_offload *qopt)
 7{
 8	struct lan966x *lan966x = port->lan966x;
 9	u32 cir, cbs;
10	u8 se_idx;
11
12	/* Check for invalid values */
13	if (qopt->idleslope <= 0 ||
14	    qopt->sendslope >= 0 ||
15	    qopt->locredit >= qopt->hicredit)
16		return -EINVAL;
17
18	se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + qopt->queue;
19	cir = qopt->idleslope;
20	cbs = (qopt->idleslope - qopt->sendslope) *
21		(qopt->hicredit - qopt->locredit) /
22		-qopt->sendslope;
23
24	/* Rate unit is 100 kbps */
25	cir = DIV_ROUND_UP(cir, 100);
26	/* Avoid using zero rate */
27	cir = cir ?: 1;
28	/* Burst unit is 4kB */
29	cbs = DIV_ROUND_UP(cbs, 4096);
30	/* Avoid using zero burst */
31	cbs = cbs ?: 1;
32
33	/* Check that actually the result can be written */
34	if (cir > GENMASK(15, 0) ||
35	    cbs > GENMASK(6, 0))
36		return -EINVAL;
37
38	lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(1) |
39		QSYS_SE_CFG_SE_FRM_MODE_SET(1),
40		QSYS_SE_CFG_SE_AVB_ENA |
41		QSYS_SE_CFG_SE_FRM_MODE,
42		lan966x, QSYS_SE_CFG(se_idx));
43
44	lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(cir) |
45	       QSYS_CIR_CFG_CIR_BURST_SET(cbs),
46	       lan966x, QSYS_CIR_CFG(se_idx));
47
48	return 0;
49}
50
51int lan966x_cbs_del(struct lan966x_port *port,
52		    struct tc_cbs_qopt_offload *qopt)
53{
54	struct lan966x *lan966x = port->lan966x;
55	u8 se_idx;
56
57	se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + qopt->queue;
58
59	lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(1) |
60		QSYS_SE_CFG_SE_FRM_MODE_SET(0),
61		QSYS_SE_CFG_SE_AVB_ENA |
62		QSYS_SE_CFG_SE_FRM_MODE,
63		lan966x, QSYS_SE_CFG(se_idx));
64
65	lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(0) |
66	       QSYS_CIR_CFG_CIR_BURST_SET(0),
67	       lan966x, QSYS_CIR_CFG(se_idx));
68
69	return 0;
70}