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 <grygorii.strashko@ti.com>
Signed-off-by: Siddharth Vadapalli <s-vadapalli@ti.com>
Link: https://lore.kernel.org/r/20230327085758.3237155-1-s-vadapalli@ti.com
Signed-off-by: Paolo Abeni <pabeni@redhat.com>

diff --git a/drivers/net/ethernet/ti/am65-cpsw-nuss.c b/drivers/net/ethernet/ti/am65-cpsw-nuss.c
index 9ddb79776c88d751e9a3204966769d9d61ac9b5f..44368ecd994a5ba735772a3033738872e7c02ff2 100644 (file)
--- 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 cad04662739c5224ac25df75bff65398da7913cf..bf40c88fbd9bde5483c7ba7536c634eca8522012 100644 (file)
--- 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 8dc2c3085dcff81d2e1621001b8f2d75e5f64cef..3a908db6e5b22eec99526cb6c8b263cb33e68fef 100644 (file)
--- 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 fb223b43b19665b86de0941b3a0c06a24af000bb..0cc2a3b3d7f9e3a28fe6fdb51512470d43971299 100644 (file)
--- a/drivers/net/ethernet/ti/am65-cpsw-qos.h
+++ b/drivers/net/ethernet/ti/am65-cpsw-qos.h
@@ -8,6 +8,8 @@
 #include <linux/netdevice.h>
 #include <net/pkt_sched.h>
 
+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_ */