86 lines · c
1// SPDX-License-Identifier: GPL-2.0+2 3#include "lan966x_main.h"4 5int 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 else23 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 55int 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 else72 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