1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include "lan966x_main.h"
4 
lan966x_cbs_add(struct lan966x_port * port,struct tc_cbs_qopt_offload * qopt)5 int 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 
lan966x_cbs_del(struct lan966x_port * port,struct tc_cbs_qopt_offload * qopt)51 int 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 }
71