From 5c8560c4a19fc3e400715a347ce82bd909e3ac74 Mon Sep 17 00:00:00 2001 From: Grygorii Strashko Date: Mon, 27 Mar 2023 14:27:58 +0530 Subject: [PATCH] net: ethernet: ti: am65-cpsw: add .ndo to set dma per-queue rate Enable rate limiting TX DMA queues for CPSW interface by configuring the rate in absolute Mb/s units per TX queue. Example: ethtool -L eth0 tx 4 echo 100 > /sys/class/net/eth0/queues/tx-0/tx_maxrate echo 200 > /sys/class/net/eth0/queues/tx-1/tx_maxrate echo 50 > /sys/class/net/eth0/queues/tx-2/tx_maxrate echo 30 > /sys/class/net/eth0/queues/tx-3/tx_maxrate # disable echo 0 > /sys/class/net/eth0/queues/tx-0/tx_maxrate Signed-off-by: Grygorii Strashko Signed-off-by: Siddharth Vadapalli Link: https://lore.kernel.org/r/20230327085758.3237155-1-s-vadapalli@ti.com Signed-off-by: Paolo Abeni --- drivers/net/ethernet/ti/am65-cpsw-nuss.c | 12 ++- drivers/net/ethernet/ti/am65-cpsw-nuss.h | 2 + drivers/net/ethernet/ti/am65-cpsw-qos.c | 113 +++++++++++++++++++++++ drivers/net/ethernet/ti/am65-cpsw-qos.h | 4 + 4 files changed, 129 insertions(+), 2 deletions(-) diff --git a/drivers/net/ethernet/ti/am65-cpsw-nuss.c b/drivers/net/ethernet/ti/am65-cpsw-nuss.c index 9ddb79776c88..44368ecd994a 100644 --- a/drivers/net/ethernet/ti/am65-cpsw-nuss.c +++ b/drivers/net/ethernet/ti/am65-cpsw-nuss.c @@ -428,6 +428,8 @@ static int am65_cpsw_nuss_common_open(struct am65_cpsw_common *common) else am65_cpsw_init_host_port_switch(common); + am65_cpsw_qos_tx_p0_rate_init(common); + for (i = 0; i < common->rx_chns.descs_num; i++) { skb = __netdev_alloc_skb_ip_align(NULL, AM65_CPSW_MAX_PACKET_SIZE, @@ -599,8 +601,12 @@ static int am65_cpsw_nuss_ndo_slave_open(struct net_device *ndev) goto runtime_put; } - for (i = 0; i < common->tx_ch_num; i++) - netdev_tx_reset_queue(netdev_get_tx_queue(ndev, i)); + for (i = 0; i < common->tx_ch_num; i++) { + struct netdev_queue *txq = netdev_get_tx_queue(ndev, i); + + netdev_tx_reset_queue(txq); + txq->tx_maxrate = common->tx_chns[i].rate_mbps; + } ret = am65_cpsw_nuss_common_open(common); if (ret) @@ -1425,6 +1431,7 @@ static const struct net_device_ops am65_cpsw_nuss_netdev_ops = { .ndo_vlan_rx_kill_vid = am65_cpsw_nuss_ndo_slave_kill_vid, .ndo_eth_ioctl = am65_cpsw_nuss_ndo_slave_ioctl, .ndo_setup_tc = am65_cpsw_qos_ndo_setup_tc, + .ndo_set_tx_maxrate = am65_cpsw_qos_ndo_tx_p0_set_maxrate, }; static void am65_cpsw_disable_phy(struct phy *phy) @@ -1616,6 +1623,7 @@ void am65_cpsw_nuss_remove_tx_chns(struct am65_cpsw_common *common) devm_remove_action(dev, am65_cpsw_nuss_free_tx_chns, common); + common->tx_ch_rate_msk = 0; for (i = 0; i < common->tx_ch_num; i++) { struct am65_cpsw_tx_chn *tx_chn = &common->tx_chns[i]; diff --git a/drivers/net/ethernet/ti/am65-cpsw-nuss.h b/drivers/net/ethernet/ti/am65-cpsw-nuss.h index cad04662739c..bf40c88fbd9b 100644 --- a/drivers/net/ethernet/ti/am65-cpsw-nuss.h +++ b/drivers/net/ethernet/ti/am65-cpsw-nuss.h @@ -79,6 +79,7 @@ struct am65_cpsw_tx_chn { u32 id; u32 descs_num; char tx_chn_name[128]; + u32 rate_mbps; }; struct am65_cpsw_rx_chn { @@ -126,6 +127,7 @@ struct am65_cpsw_common { int usage_count; /* number of opened ports */ struct cpsw_ale *ale; int tx_ch_num; + u32 tx_ch_rate_msk; u32 rx_flow_id_base; struct am65_cpsw_tx_chn tx_chns[AM65_CPSW_MAX_TX_QUEUES]; diff --git a/drivers/net/ethernet/ti/am65-cpsw-qos.c b/drivers/net/ethernet/ti/am65-cpsw-qos.c index 8dc2c3085dcf..3a908db6e5b2 100644 --- a/drivers/net/ethernet/ti/am65-cpsw-qos.c +++ b/drivers/net/ethernet/ti/am65-cpsw-qos.c @@ -19,6 +19,7 @@ #define AM65_CPSW_PN_REG_CTL 0x004 #define AM65_CPSW_PN_REG_FIFO_STATUS 0x050 #define AM65_CPSW_PN_REG_EST_CTL 0x060 +#define AM65_CPSW_PN_REG_PRI_CIR(pri) (0x140 + 4 * (pri)) /* AM65_CPSW_REG_CTL register fields */ #define AM65_CPSW_CTL_EST_EN BIT(18) @@ -819,3 +820,115 @@ void am65_cpsw_qos_link_down(struct net_device *ndev) port->qos.link_speed = SPEED_UNKNOWN; } + +static u32 +am65_cpsw_qos_tx_rate_calc(u32 rate_mbps, unsigned long bus_freq) +{ + u32 ir; + + bus_freq /= 1000000; + ir = DIV_ROUND_UP(((u64)rate_mbps * 32768), bus_freq); + return ir; +} + +static void +am65_cpsw_qos_tx_p0_rate_apply(struct am65_cpsw_common *common, + int tx_ch, u32 rate_mbps) +{ + struct am65_cpsw_host *host = am65_common_get_host(common); + u32 ch_cir; + int i; + + ch_cir = am65_cpsw_qos_tx_rate_calc(rate_mbps, common->bus_freq); + writel(ch_cir, host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); + + /* update rates for every port tx queues */ + for (i = 0; i < common->port_num; i++) { + struct net_device *ndev = common->ports[i].ndev; + + if (!ndev) + continue; + netdev_get_tx_queue(ndev, tx_ch)->tx_maxrate = rate_mbps; + } +} + +int am65_cpsw_qos_ndo_tx_p0_set_maxrate(struct net_device *ndev, + int queue, u32 rate_mbps) +{ + struct am65_cpsw_port *port = am65_ndev_to_port(ndev); + struct am65_cpsw_common *common = port->common; + struct am65_cpsw_tx_chn *tx_chn; + u32 ch_rate, tx_ch_rate_msk_new; + u32 ch_msk = 0; + int ret; + + dev_dbg(common->dev, "apply TX%d rate limiting %uMbps tx_rate_msk%x\n", + queue, rate_mbps, common->tx_ch_rate_msk); + + if (common->pf_p0_rx_ptype_rrobin) { + dev_err(common->dev, "TX Rate Limiting failed - rrobin mode\n"); + return -EINVAL; + } + + ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate; + if (ch_rate == rate_mbps) + return 0; + + ret = pm_runtime_get_sync(common->dev); + if (ret < 0) { + pm_runtime_put_noidle(common->dev); + return ret; + } + ret = 0; + + tx_ch_rate_msk_new = common->tx_ch_rate_msk; + if (rate_mbps && !(tx_ch_rate_msk_new & BIT(queue))) { + tx_ch_rate_msk_new |= BIT(queue); + ch_msk = GENMASK(common->tx_ch_num - 1, queue); + ch_msk = tx_ch_rate_msk_new ^ ch_msk; + } else if (!rate_mbps) { + tx_ch_rate_msk_new &= ~BIT(queue); + ch_msk = queue ? GENMASK(queue - 1, 0) : 0; + ch_msk = tx_ch_rate_msk_new & ch_msk; + } + + if (ch_msk) { + dev_err(common->dev, "TX rate limiting has to be enabled sequentially hi->lo tx_rate_msk:%x tx_rate_msk_new:%x\n", + common->tx_ch_rate_msk, tx_ch_rate_msk_new); + ret = -EINVAL; + goto exit_put; + } + + tx_chn = &common->tx_chns[queue]; + tx_chn->rate_mbps = rate_mbps; + common->tx_ch_rate_msk = tx_ch_rate_msk_new; + + if (!common->usage_count) + /* will be applied on next netif up */ + goto exit_put; + + am65_cpsw_qos_tx_p0_rate_apply(common, queue, rate_mbps); + +exit_put: + pm_runtime_put(common->dev); + return ret; +} + +void am65_cpsw_qos_tx_p0_rate_init(struct am65_cpsw_common *common) +{ + struct am65_cpsw_host *host = am65_common_get_host(common); + int tx_ch; + + for (tx_ch = 0; tx_ch < common->tx_ch_num; tx_ch++) { + struct am65_cpsw_tx_chn *tx_chn = &common->tx_chns[tx_ch]; + u32 ch_cir; + + if (!tx_chn->rate_mbps) + continue; + + ch_cir = am65_cpsw_qos_tx_rate_calc(tx_chn->rate_mbps, + common->bus_freq); + writel(ch_cir, + host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); + } +} diff --git a/drivers/net/ethernet/ti/am65-cpsw-qos.h b/drivers/net/ethernet/ti/am65-cpsw-qos.h index fb223b43b196..0cc2a3b3d7f9 100644 --- a/drivers/net/ethernet/ti/am65-cpsw-qos.h +++ b/drivers/net/ethernet/ti/am65-cpsw-qos.h @@ -8,6 +8,8 @@ #include #include +struct am65_cpsw_common; + struct am65_cpsw_est { int buf; /* has to be the last one */ @@ -33,5 +35,7 @@ int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, void *type_data); void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed); void am65_cpsw_qos_link_down(struct net_device *ndev); +int am65_cpsw_qos_ndo_tx_p0_set_maxrate(struct net_device *ndev, int queue, u32 rate_mbps); +void am65_cpsw_qos_tx_p0_rate_init(struct am65_cpsw_common *common); #endif /* AM65_CPSW_QOS_H_ */ -- 2.39.5