1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include "lan966x_main.h"
4 
lan966x_tbf_add(struct lan966x_port * port,struct tc_tbf_qopt_offload * qopt)5 int 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 
lan966x_tbf_del(struct lan966x_port * port,struct tc_tbf_qopt_offload * qopt)55 int 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