@@ -151,6 +151,9 @@ struct stmmac_fpe_cfg {
struct ethtool_mmsv mmsv;
const struct stmmac_fpe_reg *reg;
u32 fpe_csr; /* MAC_FPE_CTRL_STS reg cache */
+ u32 preemptible_tcs;
+ u32 add_frag_size;
+ bool mapping_configured;
};
struct stmmac_tc_entry {
@@ -194,6 +197,10 @@ struct stmmac_flow_entry {
unsigned long cookie;
unsigned long action;
u8 ip_proto;
+ u32 ip4_src;
+ u32 ip4_dst;
+ u16 port_src;
+ u16 port_dst;
int in_use;
int idx;
int is_l4;
@@ -440,6 +447,7 @@ void stmmac_set_ethtool_ops(struct net_device *netdev);
void stmmac_ptp_register(struct stmmac_priv *priv);
void stmmac_ptp_unregister(struct stmmac_priv *priv);
int stmmac_ptp_restore(struct stmmac_priv *priv);
+int stmmac_tc_restore_filters(struct stmmac_priv *priv);
int stmmac_xdp_open(struct net_device *dev);
void stmmac_xdp_release(struct net_device *dev);
int stmmac_get_phy_intf_sel(phy_interface_t interface);
@@ -80,29 +80,42 @@ static int est_configure(struct stmmac_priv *priv, struct stmmac_est *cfg,
return 0;
}
-int __stmmac_setup_est(struct stmmac_priv *priv)
+/* Program either the installed schedule or an unpublished replacement. */
+int __stmmac_setup_est(struct stmmac_priv *priv, struct stmmac_est *est)
{
struct timespec64 current_time, time;
ktime_t current_time_ns, basetime;
+ unsigned long flags;
+ u64 now;
u64 cycle_time;
int err;
lockdep_assert_held(&priv->est_lock);
+ lockdep_assert_held(&priv->ptp_mutex);
- priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time);
+ if (!priv->ptp_enabled)
+ return -EOPNOTSUPP;
+
+ /* Reset replay owns ptp_mutex while public PHC reads are blocked. */
+ read_lock_irqsave(&priv->ptp_lock, flags);
+ err = stmmac_get_systime(priv, priv->ptpaddr, &now);
+ read_unlock_irqrestore(&priv->ptp_lock, flags);
+ if (err)
+ return err;
+ current_time = ns_to_timespec64(now);
current_time_ns = timespec64_to_ktime(current_time);
- time.tv_nsec = priv->est.btr_reserve[0];
- time.tv_sec = priv->est.btr_reserve[1];
+ time.tv_nsec = est->btr_reserve[0];
+ time.tv_sec = est->btr_reserve[1];
basetime = timespec64_to_ktime(time);
- cycle_time = (u64)priv->est.ctr[1] * NSEC_PER_SEC + priv->est.ctr[0];
+ cycle_time = (u64)est->ctr[1] * NSEC_PER_SEC + est->ctr[0];
time = stmmac_calc_tas_basetime(basetime, current_time_ns, cycle_time);
- priv->est.btr[0] = (u32)time.tv_nsec;
- priv->est.btr[1] = (u32)time.tv_sec;
+ est->btr[0] = (u32)time.tv_nsec;
+ est->btr[1] = (u32)time.tv_sec;
- err = stmmac_est_configure(priv, priv, &priv->est,
+ err = stmmac_est_configure(priv, priv, est,
priv->plat->clk_ptp_rate, true);
if (err)
netdev_err(priv->dev, "failed to re-configure EST\n");
@@ -66,14 +66,14 @@
extern const struct stmmac_est_ops dwmac510_est_ops;
-int __stmmac_setup_est(struct stmmac_priv *priv);
+int __stmmac_setup_est(struct stmmac_priv *priv, struct stmmac_est *est);
static inline int stmmac_setup_est(struct stmmac_priv *priv)
{
int ret = 0;
mutex_lock(&priv->est_lock);
if (priv->est.enable)
- ret = __stmmac_setup_est(priv);
+ ret = __stmmac_setup_est(priv, &priv->est);
mutex_unlock(&priv->est_lock);
return ret;
@@ -195,6 +195,7 @@ void stmmac_fpe_set_add_frag_size(struct stmmac_priv *priv, u32 add_frag_size)
value = readl(ioaddr + reg->mtl_fpe_reg);
writel(u32_replace_bits(value, add_frag_size, FPE_MTL_ADD_FRAG_SZ),
ioaddr + reg->mtl_fpe_reg);
+ priv->fpe_cfg.add_frag_size = add_frag_size;
}
#define ALG_ERR_MSG "TX algorithm SP is not suitable for one-to-many mapping"
@@ -3829,6 +3829,11 @@ static int stmmac_hw_setup(struct net_device *dev)
if (ret)
return ret;
}
+ ret = stmmac_tc_restore_filters(priv);
+ if (ret)
+ return ret;
+ if (stmmac_fpe_supported(priv))
+ stmmac_fpe_set_add_frag_size(priv, priv->fpe_cfg.add_frag_size);
/* Initialize Safety Features */
stmmac_safety_feat_configuration(priv);
@@ -3891,9 +3896,6 @@ static int stmmac_hw_setup(struct net_device *dev)
stmmac_enable_tbs(priv, priv->ioaddr, enable, chan);
}
- /* Start the ball rolling... */
- stmmac_start_all_dma(priv);
-
phylink_rx_clk_stop_block(priv->phylink);
stmmac_set_hw_vlan_mode(priv, priv->hw);
phylink_rx_clk_stop_unblock(priv->phylink);
@@ -4441,16 +4443,17 @@ static int __stmmac_open(struct net_device *dev,
if (ret)
goto init_error;
- /* The core soft reset in stmmac_hw_setup() clears the MTL_EST
- * registers, so re-apply the taprio offload after PTP is up.
- */
- ret = stmmac_setup_est(priv);
- if (ret < 0)
- goto est_error;
-
stmmac_init_coalesce(priv);
stmmac_vlan_restore(priv);
+ mutex_lock(&priv->ptp_mutex);
+ ret = stmmac_setup_est(priv);
+ mutex_unlock(&priv->ptp_mutex);
+ if (ret)
+ goto irq_error;
+
+ /* All reset-sensitive offloads must be installed before DMA runs. */
+ stmmac_start_all_dma(priv);
ret = stmmac_request_irq(dev);
if (ret)
@@ -4478,7 +4481,6 @@ static int __stmmac_open(struct net_device *dev,
for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
hrtimer_cancel(&priv->dma_conf->tx_queue[chan].txtimer);
-est_error:
stmmac_release_ptp(priv);
init_error:
/* Undo phylink_prepare_resume() even if hardware setup failed before
@@ -8892,10 +8894,11 @@ int stmmac_resume(struct device *dev)
init_coalesce:
mutex_lock(&priv->ptp_mutex);
- stmmac_block_ptp(priv, false);
- mutex_unlock(&priv->ptp_mutex);
ret = stmmac_setup_est(priv);
- if (ret < 0)
+ if (!ret)
+ stmmac_block_ptp(priv, false);
+ mutex_unlock(&priv->ptp_mutex);
+ if (ret)
goto error_stop_dma;
stmmac_init_coalesce(priv);
@@ -8905,6 +8908,7 @@ int stmmac_resume(struct device *dev)
stmmac_vlan_restore(priv);
+ stmmac_start_all_dma(priv);
stmmac_enable_all_queues(priv);
stmmac_enable_all_dma_irq(priv);
stmmac_unmask_pci_irq(priv);
@@ -107,7 +107,7 @@ static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
* error, but do not report success if only schedule replay failed.
*/
if (priv->est.enable) {
- err = __stmmac_setup_est(priv);
+ err = __stmmac_setup_est(priv, &priv->est);
if (!ret)
ret = err;
}
@@ -15,12 +15,17 @@
static int tc_config_preemption(struct stmmac_priv *priv,
struct netlink_ext_ack *extack, u32 preemptible_tcs)
{
- /* Qdisc teardown must not access unpowered registers. */
- if (priv->hw_unavailable)
- return 0;
+ int ret = 0;
- return stmmac_fpe_map_preemption_class(priv, priv->dev, extack,
- preemptible_tcs);
+ /* Qdisc teardown still clears the saved mapping after failed resume. */
+ if (!priv->hw_unavailable)
+ ret = stmmac_fpe_map_preemption_class(priv, priv->dev, extack,
+ preemptible_tcs);
+ if (!ret) {
+ priv->fpe_cfg.preemptible_tcs = preemptible_tcs;
+ priv->fpe_cfg.mapping_configured = true;
+ }
+ return ret;
}
static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry)
@@ -520,31 +525,15 @@ static int tc_add_ip4_flow(struct stmmac_priv *priv,
{
struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
struct flow_dissector *dissector = rule->match.dissector;
- bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
struct flow_match_ipv4_addrs match;
- u32 hw_match;
- int ret;
/* Nothing to do here */
if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
return -EINVAL;
flow_rule_match_ipv4_addrs(rule, &match);
- hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
- if (hw_match) {
- ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
- false, true, inv, hw_match);
- if (ret)
- return ret;
- }
-
- hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
- if (hw_match) {
- ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
- false, false, inv, hw_match);
- if (ret)
- return ret;
- }
+ entry->ip4_src = ntohl(match.key->src) & ntohl(match.mask->src);
+ entry->ip4_dst = ntohl(match.key->dst) & ntohl(match.mask->dst);
return 0;
}
@@ -555,11 +544,7 @@ static int tc_add_ports_flow(struct stmmac_priv *priv,
{
struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
struct flow_dissector *dissector = rule->match.dissector;
- bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
struct flow_match_ports match;
- u32 hw_match;
- bool is_udp;
- int ret;
/* Nothing to do here */
if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
@@ -567,10 +552,7 @@ static int tc_add_ports_flow(struct stmmac_priv *priv,
switch (entry->ip_proto) {
case IPPROTO_TCP:
- is_udp = false;
- break;
case IPPROTO_UDP:
- is_udp = true;
break;
default:
return -EINVAL;
@@ -578,23 +560,46 @@ static int tc_add_ports_flow(struct stmmac_priv *priv,
flow_rule_match_ports(rule, &match);
- hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
- if (hw_match) {
- ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
- is_udp, true, inv, hw_match);
+ entry->port_src = ntohs(match.key->src) & ntohs(match.mask->src);
+ entry->port_dst = ntohs(match.key->dst) & ntohs(match.mask->dst);
+
+ entry->is_l4 = true;
+ return 0;
+}
+
+static int tc_config_flow(struct stmmac_priv *priv,
+ const struct stmmac_flow_entry *entry)
+{
+ bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
+ bool udp = entry->ip_proto == IPPROTO_UDP;
+ int ret;
+
+ /* Clear the whole slot, including matches removed by a replacement. */
+ ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
+ false, false, false, 0);
+ if (ret || !entry->in_use)
+ return ret;
+ if (entry->ip4_src) {
+ ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
+ false, true, inv, entry->ip4_src);
if (ret)
return ret;
}
-
- hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
- if (hw_match) {
+ if (entry->ip4_dst) {
+ ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
+ false, false, inv, entry->ip4_dst);
+ if (ret)
+ return ret;
+ }
+ if (entry->port_src) {
ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
- is_udp, false, inv, hw_match);
+ udp, true, inv, entry->port_src);
if (ret)
return ret;
}
-
- entry->is_l4 = true;
+ if (entry->port_dst)
+ return stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
+ udp, false, inv, entry->port_dst);
return 0;
}
@@ -630,6 +635,7 @@ static int tc_add_flow(struct stmmac_priv *priv,
{
struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
+ struct stmmac_flow_entry new = {};
int i, ret;
if (!entry) {
@@ -638,23 +644,33 @@ static int tc_add_flow(struct stmmac_priv *priv,
return -ENOENT;
}
- ret = tc_parse_flow_actions(priv, &rule->action, entry,
+ new.idx = entry->idx;
+ ret = tc_parse_flow_actions(priv, &rule->action, &new,
cls->common.extack);
if (ret)
return ret;
for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
- ret = tc_flow_parsers[i].fn(priv, cls, entry);
+ ret = tc_flow_parsers[i].fn(priv, cls, &new);
if (!ret)
- entry->in_use = true;
+ new.in_use = true;
else if (ret == -EOPNOTSUPP)
return ret;
}
- if (!entry->in_use)
+ if (!new.in_use)
return -EINVAL;
- entry->cookie = cls->cookie;
+ ret = tc_config_flow(priv, &new);
+ if (ret) {
+ /* Do not publish a rule that was only partially programmed. */
+ if (tc_config_flow(priv, entry))
+ netdev_err(priv->dev, "failed to restore flower filter %d\n",
+ entry->idx);
+ return ret;
+ }
+ new.cookie = cls->cookie;
+ *entry = new;
return 0;
}
@@ -740,6 +756,8 @@ static int tc_add_vlan_flow(struct stmmac_priv *priv,
prio = BIT(match.key->vlan_priority);
stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
+ priv->plat->rx_queues_cfg[tc].prio = prio;
+ priv->plat->rx_queues_cfg[tc].use_prio = true;
entry->in_use = true;
entry->cookie = cls->cookie;
@@ -761,6 +779,8 @@ static int tc_del_vlan_flow(struct stmmac_priv *priv,
if (stmmac_tc_active(priv))
stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc);
+ priv->plat->rx_queues_cfg[entry->tc].prio = 0;
+ priv->plat->rx_queues_cfg[entry->tc].use_prio = true;
entry->in_use = false;
entry->cookie = 0;
@@ -935,6 +955,45 @@ static int tc_setup_cls(struct stmmac_priv *priv,
return ret;
}
+int stmmac_tc_restore_filters(struct stmmac_priv *priv)
+{
+ int i, ret;
+
+ for (i = 0; i < priv->flow_entries_max; i++) {
+ struct stmmac_flow_entry *entry = &priv->flow_entries[i];
+
+ if (!entry->in_use)
+ continue;
+ ret = tc_config_flow(priv, entry);
+ if (ret)
+ return ret;
+ }
+ for (i = 0; i < priv->rfs_entries_total; i++) {
+ struct stmmac_rfs_entry *entry = &priv->rfs_entries[i];
+
+ if (!entry->in_use)
+ continue;
+ switch (entry->type) {
+ /* VLAN priorities are replayed by stmmac_mtl_configuration(). */
+ case STMMAC_RFS_T_VLAN:
+ break;
+ case STMMAC_RFS_T_LLDP:
+ stmmac_rx_queue_routing(priv, priv->hw, PACKET_DCBCPQ,
+ entry->tc);
+ break;
+ case STMMAC_RFS_T_1588:
+ stmmac_rx_queue_routing(priv, priv->hw, PACKET_PTPQ,
+ entry->tc);
+ break;
+ }
+ }
+ /* The XGMAC callback also restores the runtime TC-to-queue mapping. */
+ if (priv->fpe_cfg.mapping_configured)
+ return tc_config_preemption(priv, NULL,
+ priv->fpe_cfg.preemptible_tcs);
+ return 0;
+}
+
struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
ktime_t current_time,
u64 cycle_time)
@@ -958,7 +1017,7 @@ struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
return time;
}
-static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv,
+static void tc_taprio_map_maxsdu_txq(struct stmmac_est *est,
struct tc_taprio_qopt_offload *qopt)
{
u32 num_tc = qopt->mqprio.qopt.num_tc;
@@ -975,7 +1034,7 @@ static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv,
count = qopt->mqprio.qopt.count[i];
for (j = offset; j < offset + count; j++)
- priv->est.max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN;
+ est->max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN;
}
}
@@ -984,10 +1043,10 @@ static int tc_taprio_configure(struct stmmac_priv *priv,
{
u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
struct netlink_ext_ack *extack = qopt->mqprio.extack;
+ struct timespec64 qopt_time;
u64 ctr = qopt->cycle_time;
- struct timespec64 time;
- u32 *gcl = NULL;
- int i, ret = 0;
+ struct stmmac_est *est;
+ int i, ret;
if (qopt->base_time < 0)
return -ERANGE;
@@ -1032,48 +1091,43 @@ static int tc_taprio_configure(struct stmmac_priv *priv,
return -EOPNOTSUPP;
}
- mutex_lock(&priv->est_lock);
-
if (qopt->cmd == TAPRIO_CMD_DESTROY)
goto disable;
- if (priv->hw_unavailable) {
- ret = -EHOSTDOWN;
- goto unlock;
- }
-
- if (qopt->num_entries > dep) {
- ret = -EINVAL;
- goto unlock;
- }
-
- if (!qopt->cycle_time) {
- ret = -ERANGE;
- goto unlock;
- }
+ if (priv->hw_unavailable)
+ return -EHOSTDOWN;
+ if (!priv->ptp_enabled || !priv->ptp_clock_ops.gettime64)
+ return -EOPNOTSUPP;
+ /* Unlike reset replay, a new schedule requires an accessible PHC. */
+ if (priv->ptp_blocked)
+ return -EBUSY;
- if (qopt->cycle_time_extension >= BIT(wid + 7)) {
- ret = -ERANGE;
- goto unlock;
- }
+ if (qopt->num_entries > dep)
+ return -EINVAL;
+ if (!qopt->cycle_time)
+ return -ERANGE;
+ if (qopt->cycle_time_extension >= BIT(wid + 7))
+ return -ERANGE;
- gcl = kzalloc(sizeof(*gcl) * EST_GCL, GFP_KERNEL);
- if (!gcl) {
- ret = -ENOMEM;
- goto unlock;
- }
+ /* Build the replacement without changing the installed schedule. An
+ * entry rejected below must not leave an enabled, zero-cycle cache for
+ * PHC adjustment or reset replay to consume.
+ */
+ est = kzalloc_obj(*est);
+ if (!est)
+ return -ENOMEM;
size = qopt->num_entries;
+ est->gcl_size = size;
+ est->enable = true;
+
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 - 1, 0)) {
+ if (delta_ns > GENMASK(wid - 1, 0) ||
+ gates > GENMASK(31 - wid, 0)) {
ret = -ERANGE;
- goto free_gcl;
- }
- if (gates > GENMASK(31 - wid, 0)) {
- ret = -ERANGE;
- goto free_gcl;
+ goto free_est;
}
switch (qopt->entries[i].command) {
@@ -1087,61 +1141,59 @@ static int tc_taprio_configure(struct stmmac_priv *priv,
break;
default:
ret = -EOPNOTSUPP;
- goto free_gcl;
+ goto free_est;
}
- gcl[i] = delta_ns | (gates << wid);
+ est->gcl[i] = delta_ns | (gates << wid);
}
- memset(&priv->est, 0, sizeof(priv->est));
- memcpy(priv->est.gcl, gcl, sizeof(priv->est.gcl));
- priv->est.gcl_size = size;
-
- time = ktime_to_timespec64(qopt->base_time);
- priv->est.btr_reserve[0] = (u32)time.tv_nsec;
- priv->est.btr_reserve[1] = (u32)time.tv_sec;
+ qopt_time = ktime_to_timespec64(qopt->base_time);
+ est->btr_reserve[0] = (u32)qopt_time.tv_nsec;
+ est->btr_reserve[1] = (u32)qopt_time.tv_sec;
+ est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
+ est->ctr[1] = (u32)ctr;
+ est->ter = qopt->cycle_time_extension;
- priv->est.ctr[0] = do_div(ctr, NSEC_PER_SEC);
- priv->est.ctr[1] = (u32)ctr;
+ tc_taprio_map_maxsdu_txq(est, qopt);
- priv->est.ter = qopt->cycle_time_extension;
- tc_taprio_map_maxsdu_txq(priv, qopt);
-
- ret = __stmmac_setup_est(priv);
+ mutex_lock(&priv->est_lock);
+ ret = __stmmac_setup_est(priv, est);
if (ret)
- goto disable;
+ goto restore;
ret = tc_config_preemption(priv, extack, qopt->mqprio.preemptible_tcs);
if (ret)
- goto disable;
-
- priv->est.enable = true;
- kfree(gcl);
+ goto restore;
+ priv->est = *est;
mutex_unlock(&priv->est_lock);
+free_est:
+ kfree(est);
+ return ret;
- return 0;
+restore:
+ /* A failed hardware update must not publish the rejected schedule. */
+ if (stmmac_est_configure(priv, priv, &priv->est,
+ priv->plat->clk_ptp_rate, priv->est.enable))
+ netdev_err(priv->dev, "failed to restore EST\n");
+ mutex_unlock(&priv->est_lock);
+ goto free_est;
disable:
+ mutex_lock(&priv->est_lock);
priv->est.enable = false;
if (!priv->hw_unavailable)
stmmac_est_configure(priv, priv, &priv->est,
priv->plat->clk_ptp_rate, false);
- /* Reset taprio status */
+ /* Reset taprio stats */
for (i = 0; i < priv->plat->tx_queues_to_use; i++) {
priv->xstats.max_sdu_txq_drop[i] = 0;
priv->xstats.mtl_est_txq_hlbf[i] = 0;
priv->xstats.mtl_est_txq_hlbs[i] = 0;
}
- i = tc_config_preemption(priv, extack, 0);
- if (qopt->cmd == TAPRIO_CMD_DESTROY)
- ret = i;
-free_gcl:
- kfree(gcl);
-unlock:
mutex_unlock(&priv->est_lock);
- return ret;
+ return tc_config_preemption(priv, extack, 0);
}
static void tc_taprio_stats(struct stmmac_priv *priv,
@@ -1182,7 +1234,10 @@ static int tc_setup_taprio(struct stmmac_priv *priv,
switch (qopt->cmd) {
case TAPRIO_CMD_REPLACE:
case TAPRIO_CMD_DESTROY:
+ /* Serialize cache publication with PHC adjustment and reset replay. */
+ mutex_lock(&priv->ptp_mutex);
err = tc_taprio_configure(priv, qopt);
+ mutex_unlock(&priv->ptp_mutex);
break;
case TAPRIO_CMD_STATS:
tc_taprio_stats(priv, qopt);