71 lines · c
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}71