1 // SPDX-License-Identifier: GPL-2.0+ 2 3 #include "lan966x_main.h" 4 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 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