net: stmmac: Integrate EST with TAPRIO scheduler API

Now that we have the EST code for XGMAC and QoS we can use it with the
TAPRIO scheduler. Integrate it into the main driver and use the API to
configure the EST feature.

Signed-off-by: Jose Abreu <Jose.Abreu@synopsys.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 9fd34df6eb164f83123346bb18ba62c6f26623ff..22f79c0a934385c5ba308713fb09b3187fbc3140 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -521,6 +521,7 @@ struct stmmac_priv;
 struct tc_cls_u32_offload;
 struct tc_cbs_qopt_offload;
 struct flow_cls_offload;
+struct tc_taprio_qopt_offload;
 
 struct stmmac_tc_ops {
        int (*init)(struct stmmac_priv *priv);
@@ -530,6 +531,8 @@ struct stmmac_tc_ops {
                         struct tc_cbs_qopt_offload *qopt);
        int (*setup_cls)(struct stmmac_priv *priv,
                         struct flow_cls_offload *cls);
+       int (*setup_taprio)(struct stmmac_priv *priv,
+                           struct tc_taprio_qopt_offload *qopt);
 };
 
 #define stmmac_tc_init(__priv, __args...) \
@@ -540,6 +543,8 @@ struct stmmac_tc_ops {
        stmmac_do_callback(__priv, tc, setup_cbs, __args)
 #define stmmac_tc_setup_cls(__priv, __args...) \
        stmmac_do_callback(__priv, tc, setup_cls, __args)
+#define stmmac_tc_setup_taprio(__priv, __args...) \
+       stmmac_do_callback(__priv, tc, setup_taprio, __args)
 
 struct stmmac_counters;
 

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index ecb89c609fb2029c67a64d448acf51e8fadd8426..18a959589cbcfa04c2ac3e4dff5e83452dc0d135 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -4076,6 +4076,8 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
                                                  priv, priv, true);
        case TC_SETUP_QDISC_CBS:
                return stmmac_tc_setup_cbs(priv, priv, type_data);
+       case TC_SETUP_QDISC_TAPRIO:
+               return stmmac_tc_setup_taprio(priv, priv, type_data);
        default:
                return -EOPNOTSUPP;
        }

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index 7d972e0fd2b04d8759a016b023e07b30381e3dd3..58d4ce094381eb8800fa23d3de536bcad188d796 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -591,9 +591,117 @@ static int tc_setup_cls(struct stmmac_priv *priv,
        return ret;
 }
 
+static int tc_setup_taprio(struct stmmac_priv *priv,
+                          struct tc_taprio_qopt_offload *qopt)
+{
+       u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
+       struct plat_stmmacenet_data *plat = priv->plat;
+       struct timespec64 time;
+       int i, ret = 0;
+
+       if (!priv->dma_cap.estsel)
+               return -EOPNOTSUPP;
+
+       switch (wid) {
+       case 0x1:
+               wid = 16;
+               break;
+       case 0x2:
+               wid = 20;
+               break;
+       case 0x3:
+               wid = 24;
+               break;
+       default:
+               return -EOPNOTSUPP;
+       }
+
+       switch (dep) {
+       case 0x1:
+               dep = 64;
+               break;
+       case 0x2:
+               dep = 128;
+               break;
+       case 0x3:
+               dep = 256;
+               break;
+       case 0x4:
+               dep = 512;
+               break;
+       case 0x5:
+               dep = 1024;
+               break;
+       default:
+               return -EOPNOTSUPP;
+       }
+
+       if (!qopt->enable)
+               goto disable;
+       if (qopt->num_entries >= dep)
+               return -EINVAL;
+       if (!qopt->base_time)
+               return -ERANGE;
+       if (!qopt->cycle_time)
+               return -ERANGE;
+
+       if (!plat->est) {
+               plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
+                                        GFP_KERNEL);
+               if (!plat->est)
+                       return -ENOMEM;
+       } else {
+               memset(plat->est, 0, sizeof(*plat->est));
+       }
+
+       size = qopt->num_entries;
+
+       priv->plat->est->gcl_size = size;
+       priv->plat->est->enable = qopt->enable;
+
+       for (i = 0; i < size; i++) {
+               s64 delta_ns = qopt->entries[i].interval;
+               u32 gates = qopt->entries[i].gate_mask;
+
+               if (delta_ns > GENMASK(wid, 0))
+                       return -ERANGE;
+               if (gates > GENMASK(31 - wid, 0))
+                       return -ERANGE;
+               if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
+                       return -EOPNOTSUPP;
+
+               priv->plat->est->gcl[i] = delta_ns | (gates << wid);
+       }
+
+       /* Adjust for real system time */
+       time = ktime_to_timespec64(qopt->base_time);
+       priv->plat->est->btr[0] = (u32)time.tv_nsec;
+       priv->plat->est->btr[1] = (u32)time.tv_sec;
+
+       priv->plat->est->ctr[0] = (u32)(qopt->cycle_time % NSEC_PER_SEC);
+       priv->plat->est->ctr[1] = (u32)(qopt->cycle_time / NSEC_PER_SEC);
+
+       ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
+                                  priv->plat->clk_ptp_rate);
+       if (ret) {
+               netdev_err(priv->dev, "failed to configure EST\n");
+               goto disable;
+       }
+
+       netdev_info(priv->dev, "configured EST\n");
+       return 0;
+
+disable:
+       priv->plat->est->enable = false;
+       stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
+                            priv->plat->clk_ptp_rate);
+       return ret;
+}
+
 const struct stmmac_tc_ops dwmac510_tc_ops = {
        .init = tc_init,
        .setup_cls_u32 = tc_setup_cls_u32,
        .setup_cbs = tc_setup_cbs,
        .setup_cls = tc_setup_cls,
+       .setup_taprio = tc_setup_taprio,
 };