162306a36Sopenharmony_ci// SPDX-License-Identifier: GPL-2.0+ 262306a36Sopenharmony_ci 362306a36Sopenharmony_ci#include "lan966x_main.h" 462306a36Sopenharmony_ci 562306a36Sopenharmony_ciint lan966x_cbs_add(struct lan966x_port *port, 662306a36Sopenharmony_ci struct tc_cbs_qopt_offload *qopt) 762306a36Sopenharmony_ci{ 862306a36Sopenharmony_ci struct lan966x *lan966x = port->lan966x; 962306a36Sopenharmony_ci u32 cir, cbs; 1062306a36Sopenharmony_ci u8 se_idx; 1162306a36Sopenharmony_ci 1262306a36Sopenharmony_ci /* Check for invalid values */ 1362306a36Sopenharmony_ci if (qopt->idleslope <= 0 || 1462306a36Sopenharmony_ci qopt->sendslope >= 0 || 1562306a36Sopenharmony_ci qopt->locredit >= qopt->hicredit) 1662306a36Sopenharmony_ci return -EINVAL; 1762306a36Sopenharmony_ci 1862306a36Sopenharmony_ci se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + qopt->queue; 1962306a36Sopenharmony_ci cir = qopt->idleslope; 2062306a36Sopenharmony_ci cbs = (qopt->idleslope - qopt->sendslope) * 2162306a36Sopenharmony_ci (qopt->hicredit - qopt->locredit) / 2262306a36Sopenharmony_ci -qopt->sendslope; 2362306a36Sopenharmony_ci 2462306a36Sopenharmony_ci /* Rate unit is 100 kbps */ 2562306a36Sopenharmony_ci cir = DIV_ROUND_UP(cir, 100); 2662306a36Sopenharmony_ci /* Avoid using zero rate */ 2762306a36Sopenharmony_ci cir = cir ?: 1; 2862306a36Sopenharmony_ci /* Burst unit is 4kB */ 2962306a36Sopenharmony_ci cbs = DIV_ROUND_UP(cbs, 4096); 3062306a36Sopenharmony_ci /* Avoid using zero burst */ 3162306a36Sopenharmony_ci cbs = cbs ?: 1; 3262306a36Sopenharmony_ci 3362306a36Sopenharmony_ci /* Check that actually the result can be written */ 3462306a36Sopenharmony_ci if (cir > GENMASK(15, 0) || 3562306a36Sopenharmony_ci cbs > GENMASK(6, 0)) 3662306a36Sopenharmony_ci return -EINVAL; 3762306a36Sopenharmony_ci 3862306a36Sopenharmony_ci lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(1) | 3962306a36Sopenharmony_ci QSYS_SE_CFG_SE_FRM_MODE_SET(1), 4062306a36Sopenharmony_ci QSYS_SE_CFG_SE_AVB_ENA | 4162306a36Sopenharmony_ci QSYS_SE_CFG_SE_FRM_MODE, 4262306a36Sopenharmony_ci lan966x, QSYS_SE_CFG(se_idx)); 4362306a36Sopenharmony_ci 4462306a36Sopenharmony_ci lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(cir) | 4562306a36Sopenharmony_ci QSYS_CIR_CFG_CIR_BURST_SET(cbs), 4662306a36Sopenharmony_ci lan966x, QSYS_CIR_CFG(se_idx)); 4762306a36Sopenharmony_ci 4862306a36Sopenharmony_ci return 0; 4962306a36Sopenharmony_ci} 5062306a36Sopenharmony_ci 5162306a36Sopenharmony_ciint lan966x_cbs_del(struct lan966x_port *port, 5262306a36Sopenharmony_ci struct tc_cbs_qopt_offload *qopt) 5362306a36Sopenharmony_ci{ 5462306a36Sopenharmony_ci struct lan966x *lan966x = port->lan966x; 5562306a36Sopenharmony_ci u8 se_idx; 5662306a36Sopenharmony_ci 5762306a36Sopenharmony_ci se_idx = SE_IDX_QUEUE + port->chip_port * NUM_PRIO_QUEUES + qopt->queue; 5862306a36Sopenharmony_ci 5962306a36Sopenharmony_ci lan_rmw(QSYS_SE_CFG_SE_AVB_ENA_SET(1) | 6062306a36Sopenharmony_ci QSYS_SE_CFG_SE_FRM_MODE_SET(0), 6162306a36Sopenharmony_ci QSYS_SE_CFG_SE_AVB_ENA | 6262306a36Sopenharmony_ci QSYS_SE_CFG_SE_FRM_MODE, 6362306a36Sopenharmony_ci lan966x, QSYS_SE_CFG(se_idx)); 6462306a36Sopenharmony_ci 6562306a36Sopenharmony_ci lan_wr(QSYS_CIR_CFG_CIR_RATE_SET(0) | 6662306a36Sopenharmony_ci QSYS_CIR_CFG_CIR_BURST_SET(0), 6762306a36Sopenharmony_ci lan966x, QSYS_CIR_CFG(se_idx)); 6862306a36Sopenharmony_ci 6962306a36Sopenharmony_ci return 0; 7062306a36Sopenharmony_ci} 71