diff --git a/drivers/net/dsa/ocelot/Kconfig b/drivers/net/dsa/ocelot/Kconfig
index 9948544ba1c4b0080ce41d231983335182872bd7..aa588116be5f054d73ae8f4b0eff8c13cf7f4e49 100644
--- a/drivers/net/dsa/ocelot/Kconfig
+++ b/drivers/net/dsa/ocelot/Kconfig
@@ -28,3 +28,11 @@ config NET_DSA_MSCC_SEVILLE
help
This driver supports the VSC9953 (Seville) switch, which is embedded
as a platform device on the NXP T1040 SoC.
+
+config MSCC_FELIX_SWITCH_TSN
+ bool "TSN support on Felix switch"
+ depends on NET_DSA_MSCC_FELIX
+ depends on TSN
+ help
+ This driver supports TSN which is using tsntool netlink
+ infrastructure on felix switch.
diff --git a/drivers/net/dsa/ocelot/Makefile b/drivers/net/dsa/ocelot/Makefile
index f6dd131e7491a3e2d71bdb7c50c5a21415b9c51f..86243f4a17a49b678c06ea127a42e277a0750fd5 100644
--- a/drivers/net/dsa/ocelot/Makefile
+++ b/drivers/net/dsa/ocelot/Makefile
@@ -9,3 +9,7 @@ mscc_felix-objs := \
mscc_seville-objs := \
felix.o \
seville_vsc9953.o
+
+ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
+mscc_felix-objs += felix_tsn.o
+endif
diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index e5288c41542c85c904843cfc450ddd70073a35cc..6b84c817aa91b8502f8d5c898a1424aba6c5976d 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -1246,6 +1246,10 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
ocelot_port->phy_mode = port_phy_modes[port];
ocelot_port->ocelot = ocelot;
ocelot_port->target = target;
+ /* Enable cut-through forwarding on all traffic classes by
+ * default, to be compatible with the upstream kernel.
+ */
+ ocelot_port->cut_thru = GENMASK(7, 0);
ocelot->ports[port] = ocelot_port;
}
diff --git a/drivers/net/dsa/ocelot/felix.h b/drivers/net/dsa/ocelot/felix.h
index 9395ac119d33787b11ce654edf45106f2dcb682d..b50240f8c88e03fcc3ee6a4d066a00189e4c2dac 100644
--- a/drivers/net/dsa/ocelot/felix.h
+++ b/drivers/net/dsa/ocelot/felix.h
@@ -49,7 +49,7 @@ struct felix_info {
int (*port_setup_tc)(struct dsa_switch *ds, int port,
enum tc_setup_type type, void *type_data);
void (*port_sched_speed_set)(struct ocelot *ocelot, int port,
- u32 speed);
+ int speed);
struct regmap *(*init_regmap)(struct ocelot *ocelot,
struct resource *res);
};
diff --git a/drivers/net/dsa/ocelot/felix_tsn.c b/drivers/net/dsa/ocelot/felix_tsn.c
new file mode 100644
index 0000000000000000000000000000000000000000..eee7c726a3aabcd74eb18ce09224d2fbc60c070c
--- /dev/null
+++ b/drivers/net/dsa/ocelot/felix_tsn.c
@@ -0,0 +1,1759 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/* Felix Switch TSN driver
+ *
+ * Copyright 2020-2022 NXP
+ */
+
+#include <soc/mscc/ocelot_qsys.h>
+#include <soc/mscc/ocelot_ana.h>
+#include <soc/mscc/ocelot_dev.h>
+#include <soc/mscc/ocelot_sys.h>
+#include <soc/mscc/ocelot_ptp.h>
+#include <soc/mscc/ocelot.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pcs-lynx.h>
+#include <linux/io.h>
+#include <net/dsa.h>
+#include <net/tsn.h>
+#include "felix_tsn.h"
+#include "felix.h"
+
+#define ETH_P_8021CB 0x2345
+#define FELIX_QSYS_HSCH_NUM 72
+/* MSCC TSN parameters limited */
+#define FELIX_PSFP_SFID_NUM 176
+#define FELIX_FRER_SSID_NUM 128
+#define FELIX_STREAM_NUM 2048
+
+struct felix_switch_capa {
+ u8 num_tas_gcl;
+ u32 tas_ct_min;
+ u32 tas_ct_max;
+ u32 tas_cte_max;
+ u32 tas_it_max;
+ u32 tas_it_min;
+ u8 num_hsch;
+ u8 num_psfp_sfid;
+ u8 num_frer_ssid;
+ u8 num_psfp_sgid;
+ u16 psfp_fmi_max;
+ u16 psfp_fmi_min;
+ u8 num_sgi_gcl;
+ u32 sgi_ct_min;
+ u32 sgi_ct_max;
+ u32 sgi_cte_max;
+ u16 qos_pol_max;
+ u8 pol_cbs_max;
+ u8 pol_pbs_max;
+ u8 frer_seq_len_min;
+ u8 frer_seq_len_max;
+ u8 frer_his_len_min;
+ u8 frer_his_len_max;
+ u8 qos_dscp_max;
+ u8 qos_cos_max;
+ u8 qos_dp_max;
+};
+
+struct stream_filter {
+ struct list_head list;
+ unsigned char mac[ETH_ALEN];
+ u16 vid;
+ u32 index;
+ u8 handle;
+ u8 dst_idx;
+};
+
+static struct list_head streamtable;
+static int hsch_bw[FELIX_QSYS_HSCH_NUM] = {0};
+
+static const struct felix_switch_capa capa = {
+ .num_tas_gcl = 64,
+ .tas_ct_min = 100,
+ .tas_ct_max = 1000000000,
+ .tas_cte_max = 999999999,
+ .tas_it_max = 999999999,
+ .tas_it_min = 1000,
+ .num_hsch = 72,
+ .num_psfp_sfid = FELIX_PSFP_SFID_NUM,
+ .num_psfp_sgid = 184,
+ .psfp_fmi_max = 246,
+ .psfp_fmi_min = 63,
+ .num_sgi_gcl = 4,
+ .sgi_ct_min = 5000,
+ .sgi_ct_max = 1000000000,
+ .sgi_cte_max = 999999999,
+ .qos_pol_max = 383,
+ /* Maximum allowed value of committed burst size(CBS) is 240 KB */
+ .pol_cbs_max = 60,
+ /* Maximum allowed value of excess burst size(EBS) is 240 KB */
+ .pol_pbs_max = 60,
+ .num_frer_ssid = FELIX_FRER_SSID_NUM,
+ .frer_seq_len_min = 1,
+ .frer_seq_len_max = 28,
+ .frer_his_len_min = 1,
+ .frer_his_len_max = 32,
+ .qos_dscp_max = 63,
+ .qos_cos_max = OCELOT_NUM_TC - 1,
+ .qos_dp_max = 1,
+};
+
+static u32 felix_tsn_get_cap(struct net_device *ndev)
+{
+ return TSN_CAP_QBV | TSN_CAP_QCI | TSN_CAP_QBU | TSN_CAP_CBS |
+ TSN_CAP_CB | TSN_CAP_TBS | TSN_CAP_CTH;
+}
+
+static void felix_get_basetime(struct ocelot *ocelot, ptptime_t basetime,
+ u64 cycle_time, struct timespec64 *ts_base)
+{
+ ptptime_t new_basetime;
+ ptptime_t cur_time;
+
+ ocelot_ptp_gettime64(&ocelot->ptp_info, ts_base);
+ cur_time = ts_base->tv_sec * NSEC_PER_SEC + ts_base->tv_nsec;
+
+ new_basetime = basetime;
+ if (cur_time > basetime) {
+ u64 nr_of_cycles = cur_time - basetime;
+
+ do_div(nr_of_cycles, cycle_time);
+ new_basetime += cycle_time * (nr_of_cycles + 1);
+ }
+
+ ts_base->tv_sec = new_basetime / NSEC_PER_SEC;
+ ts_base->tv_nsec = new_basetime % NSEC_PER_SEC;
+}
+
+static int felix_tas_gcl_set(struct ocelot *ocelot, const u8 gcl_ix,
+ struct tsn_qbv_entry *control_list)
+{
+ if (gcl_ix >= capa.num_tas_gcl) {
+ dev_err(ocelot->dev, "Invalid gcl ix %u\n", gcl_ix);
+ return -EINVAL;
+ }
+ if (control_list->time_interval < capa.tas_it_min ||
+ control_list->time_interval > capa.tas_it_max) {
+ dev_err(ocelot->dev, "Invalid time_interval %u\n",
+ control_list->time_interval);
+
+ return -EINVAL;
+ }
+
+ ocelot_write(ocelot,
+ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(gcl_ix) |
+ QSYS_GCL_CFG_REG_1_GATE_STATE(control_list->gate_state),
+ QSYS_GCL_CFG_REG_1);
+
+ ocelot_write(ocelot,
+ control_list->time_interval,
+ QSYS_GCL_CFG_REG_2);
+
+ return 0;
+}
+
+static u32 felix_tas_read_status(struct ocelot *ocelot)
+{
+ return ocelot_read(ocelot, QSYS_TAS_PARAM_CFG_CTRL);
+}
+
+static int felix_qbv_set(struct net_device *ndev,
+ struct tsn_qbv_conf *shaper_config)
+{
+ struct tsn_qbv_basic *admin_basic = &shaper_config->admin;
+ struct tsn_qbv_entry *control_list = admin_basic->control_list;
+ struct ocelot_port *ocelot_port;
+ struct timespec64 ts_base;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int ret = 0;
+ int i, port;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ if (admin_basic->control_list_length > capa.num_tas_gcl) {
+ dev_err(ocelot->dev,
+ "Invalid admin_control_list_length %u\n",
+ admin_basic->control_list_length);
+ return -EINVAL;
+ }
+
+ if ((admin_basic->cycle_time < capa.tas_ct_min ||
+ admin_basic->cycle_time > capa.tas_ct_max) &&
+ shaper_config->gate_enabled) {
+ dev_err(ocelot->dev, "Invalid admin_cycle_time %u ns\n",
+ admin_basic->cycle_time);
+ return -EINVAL;
+ }
+ if (admin_basic->cycle_time_extension > capa.tas_cte_max) {
+ dev_err(ocelot->dev,
+ "Invalid admin_cycle_time_extension %u\n",
+ admin_basic->cycle_time_extension);
+ return -EINVAL;
+ }
+
+ ocelot_port = ocelot->ports[port];
+ ocelot_port->base_time = admin_basic->base_time;
+
+ mutex_lock(&ocelot->tas_lock);
+
+ felix_get_basetime(ocelot, admin_basic->base_time,
+ admin_basic->cycle_time, &ts_base);
+
+ /* Select port */
+ ocelot_rmw(ocelot,
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port),
+ QSYS_TAS_PARAM_CFG_CTRL_ALWAYS_GUARD_BAND_SCH_Q |
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
+ QSYS_TAS_PARAM_CFG_CTRL);
+
+ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
+ if (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) {
+ ocelot_rmw_rix(ocelot, 0, QSYS_TAG_CONFIG_ENABLE,
+ QSYS_TAG_CONFIG, port);
+ }
+
+ if (!shaper_config->gate_enabled)
+ admin_basic->gate_states = 0xff;
+
+ ocelot_rmw_rix(ocelot,
+ (shaper_config->gate_enabled ? QSYS_TAG_CONFIG_ENABLE : 0) |
+ QSYS_TAG_CONFIG_INIT_GATE_STATE(admin_basic->gate_states) |
+ QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES(0xff),
+ QSYS_TAG_CONFIG_ENABLE |
+ QSYS_TAG_CONFIG_INIT_GATE_STATE_M |
+ QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES_M,
+ QSYS_TAG_CONFIG,
+ port);
+
+ if (shaper_config->maxsdu) {
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_0, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_1, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_2, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_3, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_4, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_5, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_6, port);
+ ocelot_write_rix(ocelot, shaper_config->maxsdu,
+ QSYS_QMAXSDU_CFG_7, port);
+ }
+
+ if (shaper_config->gate_enabled) {
+ ocelot_write(ocelot, ts_base.tv_nsec,
+ QSYS_PARAM_CFG_REG_1);
+
+ ocelot_write(ocelot, lower_32_bits(ts_base.tv_sec),
+ QSYS_PARAM_CFG_REG_2);
+
+ val = upper_32_bits(ts_base.tv_sec);
+ ocelot_write(ocelot,
+ QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(val) |
+ QSYS_PARAM_CFG_REG_3_LIST_LENGTH(admin_basic->control_list_length),
+ QSYS_PARAM_CFG_REG_3);
+
+ ocelot_write(ocelot, admin_basic->cycle_time,
+ QSYS_PARAM_CFG_REG_4);
+
+ ocelot_write(ocelot, admin_basic->cycle_time_extension,
+ QSYS_PARAM_CFG_REG_5);
+
+ for (i = 0; i < admin_basic->control_list_length; i++) {
+ felix_tas_gcl_set(ocelot, i, control_list);
+ control_list++;
+ }
+
+ /* Start configuration change */
+ ocelot_rmw(ocelot,
+ QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
+ QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
+ QSYS_TAS_PARAM_CFG_CTRL);
+
+ ret = readx_poll_timeout(felix_tas_read_status, ocelot, val,
+ !(QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE
+ & val), 10, 100000);
+ }
+
+ mutex_unlock(&ocelot->tas_lock);
+
+ return ret;
+}
+
+static int felix_qbv_get(struct net_device *ndev, struct tsn_qbv_conf *shaper_config)
+{
+ struct tsn_qbv_basic *admin = &shaper_config->admin;
+ struct tsn_qbv_entry *list;
+ u32 base_timel, base_timeh;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 val, reg;
+ int i, port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ mutex_lock(&ocelot->tas_lock);
+
+ ocelot_rmw(ocelot,
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port),
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
+ QSYS_TAS_PARAM_CFG_CTRL);
+
+ shaper_config->maxsdu = ocelot_read_rix(ocelot, QSYS_QMAXSDU_CFG_0, port);
+
+ val = ocelot_read_rix(ocelot, QSYS_TAG_CONFIG, port);
+ shaper_config->gate_enabled = (val & QSYS_TAG_CONFIG_ENABLE);
+ admin->gate_states = QSYS_TAG_CONFIG_INIT_GATE_STATE_X(val);
+
+ base_timel = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_1);
+ base_timeh = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_2);
+ reg = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_3);
+ admin->base_time = base_timeh |
+ (((u64)QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(reg)) << 32);
+
+ admin->base_time = (admin->base_time * 1000000000) + base_timel;
+
+ admin->control_list_length =
+ QSYS_PARAM_CFG_REG_3_LIST_LENGTH_X(reg);
+
+ admin->cycle_time = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_4);
+ admin->cycle_time_extension =
+ ocelot_read(ocelot, QSYS_PARAM_CFG_REG_5);
+
+ list = kmalloc_array(admin->control_list_length,
+ sizeof(struct tsn_qbv_entry), GFP_KERNEL);
+ if (!list) {
+ mutex_unlock(&ocelot->tas_lock);
+ return -ENOMEM;
+ }
+
+ admin->control_list = list;
+
+ for (i = 0; i < admin->control_list_length; i++) {
+ ocelot_rmw(ocelot,
+ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(i),
+ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM_M,
+ QSYS_GCL_CFG_REG_1);
+
+ list->time_interval =
+ ocelot_read(ocelot, QSYS_GCL_CFG_REG_2);
+
+ reg = ocelot_read(ocelot, QSYS_GCL_CFG_REG_1);
+ list->gate_state = QSYS_GCL_CFG_REG_1_GATE_STATE_X(reg);
+
+ list++;
+ }
+
+ mutex_unlock(&ocelot->tas_lock);
+
+ return 0;
+}
+
+static int felix_qbv_get_gatelist(struct ocelot *ocelot,
+ struct tsn_qbv_basic *oper)
+{
+ u32 base_timel;
+ u32 base_timeh;
+ u32 val;
+ struct tsn_qbv_entry *glist;
+ int i;
+
+ base_timel = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_1);
+ base_timeh = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_2);
+ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_3);
+ oper->base_time = base_timeh;
+ oper->base_time +=
+ ((u64)QSYS_PARAM_STATUS_REG_3_BASE_TIME_SEC_MSB(val)) <<
+ 32;
+ oper->base_time = (oper->base_time * 1000000000) + base_timel;
+
+ oper->control_list_length =
+ QSYS_PARAM_STATUS_REG_3_LIST_LENGTH_X(val);
+
+ oper->cycle_time = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_4);
+ oper->cycle_time_extension = ocelot_read(ocelot,
+ QSYS_PARAM_STATUS_REG_5);
+
+ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
+ oper->gate_states = QSYS_PARAM_STATUS_REG_8_OPER_GATE_STATE_X(val);
+
+ glist = kmalloc_array(oper->control_list_length,
+ sizeof(struct tsn_qbv_entry), GFP_KERNEL);
+ if (!glist)
+ return -ENOMEM;
+
+ oper->control_list = glist;
+
+ for (i = 0; i < oper->control_list_length; i++) {
+ ocelot_rmw(ocelot,
+ QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM(i),
+ QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM_M,
+ QSYS_GCL_STATUS_REG_1);
+
+ val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_2);
+ glist->time_interval = val;
+ val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_1);
+ glist->gate_state =
+ QSYS_GCL_STATUS_REG_1_GATE_STATE_X(val);
+
+ glist++;
+ }
+
+ return 0;
+}
+
+static int felix_qbv_get_status(struct net_device *ndev,
+ struct tsn_qbv_status *qbvstatus)
+{
+ struct tsn_qbv_basic *oper = &qbvstatus->oper;
+ struct ocelot *ocelot;
+ struct timespec64 ts;
+ struct dsa_port *dp;
+ ptptime_t cur_time;
+ int port;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ mutex_lock(&ocelot->tas_lock);
+
+ ocelot_rmw(ocelot,
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port),
+ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
+ QSYS_TAS_PARAM_CFG_CTRL);
+
+ qbvstatus->supported_list_max = capa.num_tas_gcl;
+
+ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
+ qbvstatus->config_pending =
+ (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) ? 1 : 0;
+
+ qbvstatus->config_change_time =
+ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_7);
+
+ qbvstatus->config_change_time +=
+ ((u64)QSYS_PARAM_STATUS_REG_8_CFG_CHG_TIME_SEC_MSB(val)) <<
+ 32;
+
+ qbvstatus->config_change_time =
+ (qbvstatus->config_change_time * 1000000000) +
+ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_6);
+
+ qbvstatus->config_change_error =
+ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_9);
+
+ ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
+ cur_time = ts.tv_sec * NSEC_PER_SEC + ts.tv_nsec;
+
+ qbvstatus->current_time = cur_time;
+ felix_qbv_get_gatelist(ocelot, oper);
+
+ mutex_unlock(&ocelot->tas_lock);
+
+ return 0;
+}
+
+static int felix_qbu_set(struct net_device *ndev, u8 preemptible)
+{
+ struct ocelot_port *ocelot_port;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ ocelot_port = ocelot->ports[port];
+
+ ocelot_port_rmwl(ocelot_port,
+ DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
+ DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
+ DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
+ DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
+ DEV_MM_ENABLE_CONFIG);
+
+ ocelot_rmw_rix(ocelot,
+ QSYS_PREEMPTION_CFG_P_QUEUES(preemptible),
+ QSYS_PREEMPTION_CFG_P_QUEUES_M,
+ QSYS_PREEMPTION_CFG,
+ port);
+
+ return 0;
+}
+
+static int felix_qbu_get(struct net_device *ndev, struct tsn_preempt_status *c)
+{
+ struct ocelot_port *ocelot_port;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int port;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ ocelot_port = ocelot->ports[port];
+
+ val = ocelot_read_rix(ocelot, QSYS_PREEMPTION_CFG, port);
+
+ c->admin_state = QSYS_PREEMPTION_CFG_P_QUEUES(val);
+ c->hold_advance = QSYS_PREEMPTION_CFG_HOLD_ADVANCE_X(val);
+
+ val = ocelot_port_readl(ocelot_port, DEV_MM_STATUS);
+ c->preemption_active =
+ DEV_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS & val;
+
+ return 0;
+}
+
+static int felix_stream_table_add(u32 index, const unsigned char mac[ETH_ALEN],
+ int vid, u8 dst_idx, u8 handle)
+{
+ struct stream_filter *stream, *tmp;
+ struct list_head *pos, *q;
+
+ list_for_each_safe(pos, q, &streamtable) {
+ tmp = list_entry(pos, struct stream_filter, list);
+ if (tmp->index == index) {
+ ether_addr_copy(tmp->mac, mac);
+ tmp->vid = vid;
+ tmp->dst_idx = dst_idx;
+ tmp->handle = handle;
+ return 0;
+ }
+ if (tmp->index > index)
+ break;
+ }
+ stream = kzalloc(sizeof(*stream), GFP_KERNEL);
+ if (!stream)
+ return -ENOMEM;
+
+ stream->index = index;
+ ether_addr_copy(stream->mac, mac);
+ stream->vid = vid;
+ stream->dst_idx = dst_idx;
+ stream->handle = handle;
+ list_add(&stream->list, pos->prev);
+
+ return 0;
+}
+
+static void felix_stream_table_del(u32 index)
+{
+ struct stream_filter *tmp;
+ struct list_head *pos, *q;
+
+ list_for_each_safe(pos, q, &streamtable) {
+ tmp = list_entry(pos, struct stream_filter, list);
+ if (tmp->index == index) {
+ list_del(pos);
+ kfree(tmp);
+ break;
+ }
+ }
+}
+
+static struct stream_filter *felix_stream_table_get(u32 index)
+{
+ struct stream_filter *tmp;
+
+ list_for_each_entry(tmp, &streamtable, list)
+ if (tmp->index == index)
+ return tmp;
+
+ return NULL;
+}
+
+static int felix_streamid_force_forward_clear(struct ocelot *ocelot, u8 port)
+{
+ struct stream_filter *tmp;
+
+ if (port >= ocelot->num_phys_ports)
+ return 0;
+
+ list_for_each_entry(tmp, &streamtable, list)
+ if (tmp->dst_idx == port)
+ return 0;
+
+ ocelot_bridge_force_forward_port(ocelot, port, false);
+
+ return 0;
+}
+
+static int felix_cb_streamid_set(struct net_device *ndev, u32 index, bool enable,
+ struct tsn_cb_streamid *streamid)
+{
+ enum macaccess_entry_type type;
+ struct stream_filter *stream;
+ unsigned char mac[ETH_ALEN];
+ struct ocelot *ocelot;
+ int sfid, ssid, port;
+ struct dsa_port *dp;
+ u32 dst_idx;
+ u16 vid;
+ int ret;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ if (index >= FELIX_STREAM_NUM) {
+ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
+ index, FELIX_STREAM_NUM - 1);
+ return -EINVAL;
+ }
+
+ if (!enable) {
+ stream = felix_stream_table_get(index);
+ if (!stream)
+ return -EINVAL;
+
+ ocelot_mact_forget(ocelot, stream->mac, stream->vid);
+ felix_stream_table_del(index);
+
+ felix_streamid_force_forward_clear(ocelot, stream->dst_idx);
+
+ return 0;
+ }
+
+ if (streamid->type != 1) {
+ dev_err(ocelot->dev, "Invalid stream type\n");
+ return -EINVAL;
+ }
+
+ if (streamid->handle >= FELIX_PSFP_SFID_NUM) {
+ dev_err(ocelot->dev,
+ "Invalid stream handle %u, maximum:%u\n",
+ streamid->handle, FELIX_PSFP_SFID_NUM - 1);
+ return -EINVAL;
+ }
+
+ sfid = streamid->handle;
+ ssid = (streamid->handle < FELIX_FRER_SSID_NUM ?
+ streamid->handle : (FELIX_FRER_SSID_NUM - 1));
+
+ u64_to_ether_addr(streamid->para.nid.dmac, mac);
+ vid = streamid->para.nid.vid;
+
+ ret = ocelot_mact_lookup(ocelot, &dst_idx, mac, vid, &type);
+ if (ret && ret != -ENOENT)
+ return ret;
+
+ if (ret == -ENOENT) {
+ /* The MAC table doesn't contain this entry, learn it as static
+ * and annotate it with a SSID and a SFID.
+ */
+ ret = ocelot_mact_learn_streamdata(ocelot, port, mac, vid,
+ ENTRYTYPE_LOCKED, sfid,
+ ssid);
+ if (ret)
+ return ret;
+
+ return felix_stream_table_add(index, mac, vid, port,
+ streamid->handle);
+ }
+
+ if (type == ENTRYTYPE_NORMAL)
+ type = ENTRYTYPE_LOCKED;
+
+ ret = ocelot_mact_learn_streamdata(ocelot, dst_idx, mac, vid, type,
+ sfid, ssid);
+ if (ret)
+ return ret;
+
+ return felix_stream_table_add(index, mac, vid, dst_idx,
+ streamid->handle);
+}
+
+static int felix_cb_streamid_get(struct net_device *ndev, u32 index,
+ struct tsn_cb_streamid *streamid)
+{
+ enum macaccess_entry_type type;
+ struct stream_filter *stream;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 dst, fwdmask;
+ int ret;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= FELIX_STREAM_NUM) {
+ dev_err(ocelot->dev,
+ "Invalid stream handle %u, maximum:%u\n",
+ index, FELIX_STREAM_NUM - 1);
+ return -EINVAL;
+ }
+
+ stream = felix_stream_table_get(index);
+ if (!stream)
+ return -EINVAL;
+
+ ret = ocelot_mact_lookup(ocelot, &dst, stream->mac, stream->vid, &type);
+ if (ret)
+ return ret;
+
+ streamid->type = type;
+
+ fwdmask = ocelot_read_rix(ocelot, ANA_PGID_PGID, dst);
+ streamid->ofac_oport = ANA_PGID_PGID_PGID(fwdmask);
+
+ streamid->para.nid.dmac = ether_addr_to_u64(stream->mac);
+ streamid->para.nid.vid = stream->vid;
+
+ streamid->handle = stream->handle;
+
+ return 0;
+}
+
+static int felix_cb_streamid_counters_get(struct net_device *ndev, u32 index,
+ struct tsn_cb_streamid_counters *sc)
+{
+ return 0;
+}
+
+static int felix_qci_sfi_set(struct net_device *ndev, u32 index, bool enable,
+ struct tsn_qci_psfp_sfi_conf *sfi)
+{
+ int igr_prio = sfi->priority_spec;
+ u16 sgid = sfi->stream_gate_instance_id;
+ int fmid = sfi->stream_filter.flow_meter_instance_id;
+ u16 max_sdu_len = sfi->stream_filter.maximum_sdu_size;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int sfid = index;
+ int i, port;
+ u16 pol_idx;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ if (fmid == -1)
+ pol_idx = capa.psfp_fmi_max;
+ else
+ pol_idx = (u16)fmid;
+
+ if (!enable) {
+ val = ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE);
+ ocelot_write(ocelot,
+ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
+ ANA_TABLES_SFIDTIDX);
+ ocelot_write(ocelot, val, ANA_TABLES_SFIDACCESS);
+ return 0;
+ }
+
+ /* Port default SFID set */
+ if (sfi->stream_handle_spec < 0) {
+ val = ANA_PORT_SFID_CFG_SFID_VALID |
+ ANA_PORT_SFID_CFG_SFID(sfid);
+ if (igr_prio < 0) {
+ for (i = 0; i < OCELOT_NUM_TC; i++)
+ ocelot_write_ix(ocelot, val, ANA_PORT_SFID_CFG,
+ port, i);
+ } else {
+ ocelot_write_ix(ocelot, val, ANA_PORT_SFID_CFG,
+ port, igr_prio & 0x7);
+ }
+ } else if (index != sfi->stream_handle_spec) {
+ dev_err(ocelot->dev, "Index must equal to streamHandle\n");
+ return -EINVAL;
+ }
+
+ if (sgid >= capa.num_psfp_sgid) {
+ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
+ sgid, capa.num_psfp_sgid);
+ return -EINVAL;
+ }
+ if (pol_idx > capa.psfp_fmi_max || pol_idx < capa.psfp_fmi_min) {
+ dev_err(ocelot->dev, "Invalid pol_idx %u, range:%d~%d\n",
+ pol_idx, capa.psfp_fmi_min, capa.psfp_fmi_max);
+ return -EINVAL;
+ }
+
+ ocelot_write(ocelot, ANA_TABLES_SFIDTIDX_SGID_VALID |
+ ANA_TABLES_SFIDTIDX_SGID(sgid) |
+ ((fmid != -1) ? ANA_TABLES_SFIDTIDX_POL_ENA : 0) |
+ ANA_TABLES_SFIDTIDX_POL_IDX(pol_idx) |
+ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
+ ANA_TABLES_SFIDTIDX);
+
+ ocelot_write(ocelot,
+ ((igr_prio >= 0) ?
+ ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA : 0) |
+ ANA_TABLES_SFIDACCESS_IGR_PRIO(igr_prio) |
+ ANA_TABLES_SFIDACCESS_MAX_SDU_LEN(max_sdu_len) |
+ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE),
+ ANA_TABLES_SFIDACCESS);
+
+ return 0;
+}
+
+static int felix_qci_sfi_get(struct net_device *ndev, u32 index,
+ struct tsn_qci_psfp_sfi_conf *sfi)
+{
+ u32 val, reg, fmeter_id, max_sdu;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 sfid = index;
+ int enable = 1;
+ int i, port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ if (sfid >= capa.num_psfp_sfid) {
+ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
+ sfid, capa.num_psfp_sfid);
+ return -EINVAL;
+ }
+
+ ocelot_rmw(ocelot,
+ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
+ ANA_TABLES_SFIDTIDX_SFID_INDEX_M,
+ ANA_TABLES_SFIDTIDX);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_READ),
+ ANA_TABLES_SFIDACCESS);
+
+ val = ocelot_read(ocelot, ANA_TABLES_SFIDTIDX);
+ if (!(val & ANA_TABLES_SFIDTIDX_SGID_VALID)) {
+ enable = 0;
+ return enable;
+ }
+
+ sfi->stream_gate_instance_id = ANA_TABLES_SFIDTIDX_SGID_X(val);
+ fmeter_id = ANA_TABLES_SFIDTIDX_POL_IDX_X(val);
+ sfi->stream_filter.flow_meter_instance_id = fmeter_id;
+
+ reg = ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
+ max_sdu = ANA_TABLES_SFIDACCESS_MAX_SDU_LEN_X(reg);
+ sfi->stream_filter.maximum_sdu_size = max_sdu;
+
+ if (reg & ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA)
+ sfi->priority_spec = ANA_TABLES_SFIDACCESS_IGR_PRIO_X(reg);
+ else
+ dev_err(ocelot->dev, "priority not enable\n");
+
+ for (i = 0; i < OCELOT_NUM_TC; i++) {
+ val = ocelot_read_ix(ocelot, ANA_PORT_SFID_CFG, port, i);
+ if ((val & ANA_PORT_SFID_CFG_SFID_VALID) &&
+ sfid == ANA_PORT_SFID_CFG_SFID(val)) {
+ sfi->stream_handle_spec = -1;
+ return enable;
+ }
+ }
+
+ sfi->stream_handle_spec = sfid;
+ return enable;
+}
+
+static int felix_qci_sfi_counters_get(struct net_device *ndev, u32 index,
+ struct tsn_qci_psfp_sfi_counters *sfi_cnt)
+{
+ u32 match, not_pass, not_pass_sdu, red;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 sfid = index;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (sfid >= capa.num_psfp_sfid) {
+ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
+ sfid, capa.num_psfp_sfid);
+ return -EINVAL;
+ }
+
+ ocelot_rmw(ocelot,
+ SYS_STAT_CFG_STAT_VIEW(sfid),
+ SYS_STAT_CFG_STAT_VIEW_M,
+ SYS_STAT_CFG);
+
+ match = ocelot_read_gix(ocelot, SYS_CNT, 0x200);
+ not_pass = ocelot_read_gix(ocelot, SYS_CNT, 0x201);
+ not_pass_sdu = ocelot_read_gix(ocelot, SYS_CNT, 0x202);
+ red = ocelot_read_gix(ocelot, SYS_CNT, 0x203);
+
+ sfi_cnt->matching_frames_count = match;
+ sfi_cnt->not_passing_frames_count = not_pass;
+ sfi_cnt->not_passing_sdu_count = not_pass_sdu;
+ sfi_cnt->red_frames_count = red;
+
+ sfi_cnt->passing_frames_count = match - not_pass;
+ sfi_cnt->passing_sdu_count = match - not_pass - not_pass_sdu;
+
+ return 0;
+}
+
+static int felix_qci_max_cap_get(struct net_device *ndev,
+ struct tsn_qci_psfp_stream_param *stream_para)
+{
+ /* MaxStreamFilterInstances */
+ stream_para->max_sf_instance = capa.num_psfp_sfid;
+ /* MaxStreamGateInstances */
+ stream_para->max_sg_instance = capa.num_psfp_sgid;
+ /* MaxFlowMeterInstances */
+ stream_para->max_fm_instance = capa.psfp_fmi_max -
+ capa.psfp_fmi_min + 1;
+ /* SupportedListMax */
+ stream_para->supported_list_max = capa.num_sgi_gcl;
+
+ return 0;
+}
+
+static int felix_sgi_set_glist(struct ocelot *ocelot,
+ struct tsn_qci_psfp_gcl *gcl, uint32_t num)
+{
+ u32 time_sum = 0;
+ int i;
+
+ if (num > capa.num_sgi_gcl)
+ return -EINVAL;
+
+ for (i = 0; i < num; i++) {
+ u32 val = ANA_SG_GCL_GS_CONFIG_IPS((gcl->ipv < 0) ?
+ 0 : gcl->ipv + 8);
+ val |= (gcl->gate_state ? ANA_SG_GCL_GS_CONFIG_GATE_STATE : 0);
+ ocelot_write_rix(ocelot, val, ANA_SG_GCL_GS_CONFIG, i);
+
+ time_sum += gcl->time_interval;
+ ocelot_write_rix(ocelot, time_sum, ANA_SG_GCL_TI_CONFIG, i);
+
+ gcl++;
+ }
+
+ return 0;
+}
+
+static u32 felix_sgi_read_status(struct ocelot *ocelot)
+{
+ return ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
+}
+
+static int felix_qci_sgi_set(struct net_device *ndev, u32 index,
+ struct tsn_qci_psfp_sgi_conf *sgi_conf)
+{
+ struct tsn_qci_sg_control *admin_list = &sgi_conf->admin;
+ u32 list_length = sgi_conf->admin.control_list_length;
+ u32 cycle_time = sgi_conf->admin.cycle_time;
+ u32 cycle_time_ex = sgi_conf->admin.cycle_time_extension;
+ struct timespec64 ts_base;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 sgid = index;
+ int ret;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (sgid >= capa.num_psfp_sgid) {
+ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
+ sgid, capa.num_psfp_sgid);
+ return -EINVAL;
+ }
+ if ((cycle_time < capa.sgi_ct_min ||
+ cycle_time > capa.sgi_ct_max) &&
+ sgi_conf->gate_enabled) {
+ dev_err(ocelot->dev, "Invalid cycle_time %u ns\n",
+ cycle_time);
+ return -EINVAL;
+ }
+ if (cycle_time_ex > capa.sgi_cte_max) {
+ dev_err(ocelot->dev,
+ "Invalid cycle_time_extension %u\n",
+ cycle_time_ex);
+ return -EINVAL;
+ }
+ if (list_length > capa.num_sgi_gcl) {
+ dev_err(ocelot->dev,
+ "Invalid sgi_gcl len %u, maximum:%u\n",
+ list_length, capa.num_sgi_gcl);
+ return -EINVAL;
+ }
+
+ /* configure SGID */
+ ocelot_rmw(ocelot,
+ ANA_SG_ACCESS_CTRL_SGID(sgid),
+ ANA_SG_ACCESS_CTRL_SGID_M,
+ ANA_SG_ACCESS_CTRL);
+
+ /* Disable SG */
+ if (!sgi_conf->gate_enabled) {
+ ocelot_rmw(ocelot,
+ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE,
+ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
+ ANA_SG_CONFIG_REG_3_GATE_ENABLE,
+ ANA_SG_CONFIG_REG_3);
+ return 0;
+ }
+
+ felix_get_basetime(ocelot, sgi_conf->admin.base_time,
+ sgi_conf->admin.cycle_time, &ts_base);
+
+ ocelot_write(ocelot, ts_base.tv_nsec, ANA_SG_CONFIG_REG_1);
+ ocelot_write(ocelot, lower_32_bits(ts_base.tv_sec),
+ ANA_SG_CONFIG_REG_2);
+
+ val = upper_32_bits(ts_base.tv_sec);
+ ocelot_write(ocelot,
+ (sgi_conf->admin.init_ipv < 0 ?
+ 0 : ANA_SG_CONFIG_REG_3_IPV_VALID) |
+ ANA_SG_CONFIG_REG_3_INIT_IPV(sgi_conf->admin.init_ipv) |
+ ANA_SG_CONFIG_REG_3_GATE_ENABLE |
+ ANA_SG_CONFIG_REG_3_LIST_LENGTH(list_length) |
+ (sgi_conf->admin.gate_states > 0 ?
+ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE : 0) |
+ ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val),
+ ANA_SG_CONFIG_REG_3);
+
+ ocelot_write(ocelot, cycle_time, ANA_SG_CONFIG_REG_4);
+ ocelot_write(ocelot, cycle_time_ex, ANA_SG_CONFIG_REG_5);
+
+ ret = felix_sgi_set_glist(ocelot, admin_list->gcl, list_length);
+ if (ret < 0)
+ return ret;
+
+ /* Start configuration change */
+ ocelot_rmw(ocelot,
+ ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+ ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+ ANA_SG_ACCESS_CTRL);
+
+ ret = readx_poll_timeout(felix_sgi_read_status, ocelot, val,
+ (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
+ 10, 100000);
+
+ return ret;
+}
+
+static int felix_sgi_get_glist(struct ocelot *ocelot,
+ struct tsn_qci_psfp_gcl *gcl,
+ uint32_t num)
+{
+ u32 time = 0;
+ u32 reg;
+ u16 val;
+ int i;
+
+ if (num > capa.num_sgi_gcl)
+ return -EINVAL;
+
+ for (i = 0; i < num; i++) {
+ val = ocelot_read_rix(ocelot, ANA_SG_GCL_GS_CONFIG, i);
+ gcl->gate_state = (val & ANA_SG_GCL_GS_CONFIG_GATE_STATE);
+
+ if (val & ANA_SG_GCL_GS_CONFIG_IPV_VALID)
+ gcl->ipv = ANA_SG_GCL_GS_CONFIG_IPV(val);
+ else
+ gcl->ipv = -1;
+
+ reg = ocelot_read_rix(ocelot, ANA_SG_GCL_TI_CONFIG, i);
+ gcl->time_interval = (reg - time);
+ time = reg;
+
+ gcl++;
+ }
+
+ return 0;
+}
+
+static int felix_qci_sgi_get(struct net_device *ndev, u32 index,
+ struct tsn_qci_psfp_sgi_conf *sgi_conf)
+{
+ struct tsn_qci_sg_control *admin = &sgi_conf->admin;
+ struct tsn_qci_psfp_gcl *glist;
+ u32 val, reg, list_num;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= capa.num_psfp_sgid) {
+ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
+ index, capa.num_psfp_sgid);
+ return -EINVAL;
+ }
+
+ ocelot_rmw(ocelot,
+ ANA_SG_ACCESS_CTRL_SGID(index),
+ ANA_SG_ACCESS_CTRL_SGID_M,
+ ANA_SG_ACCESS_CTRL);
+
+ admin->cycle_time = ocelot_read(ocelot, ANA_SG_CONFIG_REG_4);
+ admin->cycle_time_extension =
+ ocelot_read(ocelot, ANA_SG_CONFIG_REG_5);
+
+ val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_2);
+ admin->base_time = val;
+
+ reg = ocelot_read(ocelot, ANA_SG_CONFIG_REG_1);
+ val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_3);
+
+ admin->base_time +=
+ ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val) << 32;
+
+ admin->base_time = admin->base_time * 1000000000 + reg;
+
+ if (val & ANA_SG_CONFIG_REG_3_IPV_VALID)
+ admin->init_ipv = ANA_SG_CONFIG_REG_3_INIT_IPV_X(val);
+ else
+ admin->init_ipv = -1;
+
+ if (val & ANA_SG_CONFIG_REG_3_GATE_ENABLE)
+ sgi_conf->gate_enabled = 1;
+
+ admin->control_list_length = ANA_SG_CONFIG_REG_3_LIST_LENGTH_X(val);
+
+ list_num = admin->control_list_length;
+
+ glist = kmalloc_array(list_num, sizeof(struct tsn_qci_psfp_gcl),
+ GFP_KERNEL);
+ if (!glist)
+ return -ENOMEM;
+
+ admin->gcl = glist;
+
+ return felix_sgi_get_glist(ocelot, glist, list_num);
+}
+
+static int felix_qci_sgi_status_get(struct net_device *ndev, u16 index,
+ struct tsn_psfp_sgi_status *sgi_status)
+{
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 val, reg;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= capa.num_psfp_sgid) {
+ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
+ index, capa.num_psfp_sgid);
+ return -EINVAL;
+ }
+
+ ocelot_rmw(ocelot,
+ ANA_SG_ACCESS_CTRL_SGID(index),
+ ANA_SG_ACCESS_CTRL_SGID_M,
+ ANA_SG_ACCESS_CTRL);
+
+ val = ocelot_read(ocelot, ANA_SG_STATUS_REG_2);
+ sgi_status->config_change_time = val;
+
+ reg = ocelot_read(ocelot, ANA_SG_STATUS_REG_1);
+ val = ocelot_read(ocelot, ANA_SG_STATUS_REG_3);
+ sgi_status->config_change_time +=
+ ANA_SG_STATUS_REG_3_CFG_CHG_TIME_SEC_MSB(val) << 32;
+ sgi_status->config_change_time =
+ sgi_status->config_change_time * 1000000000 + reg;
+
+ if (val & ANA_SG_STATUS_REG_3_CONFIG_PENDING)
+ sgi_status->config_pending = 1;
+ else
+ sgi_status->config_pending = 0;
+
+ if (val & ANA_SG_STATUS_REG_3_GATE_STATE)
+ sgi_status->oper.gate_states = 1;
+ else
+ sgi_status->oper.gate_states = 0;
+ /*bit 3 encoding 0:IPV [0:2]is invalid . 1:IPV[0:2] is valid*/
+ if (val & ANA_SG_STATUS_REG_3_IPV_VALID)
+ sgi_status->oper.init_ipv = ANA_SG_STATUS_REG_3_IPV_X(val);
+ else
+ sgi_status->oper.init_ipv = -1;
+
+ return 0;
+}
+
+static int felix_qci_fmi_set(struct net_device *ndev, u32 index,
+ bool enable, struct tsn_qci_psfp_fmi *fmi)
+{
+ u32 cir = 0, cbs = 0, pir = 0, pbs = 0;
+ bool cir_discard = 0, pir_discard = 0;
+ u32 pbs_max = 0, cbs_max = 0;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 cir_ena = 0;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index > capa.qos_pol_max) {
+ dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
+ index, capa.qos_pol_max);
+ return -EINVAL;
+ }
+
+ if (fmi->mark_red_enable && fmi->mark_red) {
+ fmi->eir = 0;
+ fmi->ebs = 0;
+ fmi->cir = 0;
+ fmi->cbs = 0;
+ }
+
+ pir = fmi->eir;
+ pbs = fmi->ebs;
+
+ if (!fmi->drop_on_yellow)
+ cir_ena = 1;
+
+ if (cir_ena) {
+ cir = fmi->cir;
+ cbs = fmi->cbs;
+ if (cir == 0 && cbs == 0) {
+ cir_discard = 1;
+ } else {
+ cir = DIV_ROUND_UP(cir, 100);
+ cir *= 3; /* Rate unit is 33 1/3 kbps */
+ cbs = DIV_ROUND_UP(cbs, 4096);
+ cbs = (cbs ? cbs : 1);
+ cbs_max = capa.pol_cbs_max;
+ if (fmi->cf)
+ pir += fmi->cir;
+ }
+ }
+
+ if (pir == 0 && pbs == 0) {
+ pir_discard = 1;
+ } else {
+ pir = DIV_ROUND_UP(pir, 100);
+ pir *= 3; /* Rate unit is 33 1/3 kbps */
+ pbs = DIV_ROUND_UP(pbs, 4096);
+ pbs = (pbs ? pbs : 1);
+ pbs_max = capa.pol_pbs_max;
+ }
+ pir = min_t(u32, GENMASK(15, 0), pir);
+ cir = min_t(u32, GENMASK(15, 0), cir);
+ pbs = min(pbs_max, pbs);
+ cbs = min(cbs_max, cbs);
+
+ ocelot_write_gix(ocelot, (ANA_POL_MODE_CFG_IPG_SIZE(20) |
+ ANA_POL_MODE_CFG_FRM_MODE(1) |
+ (fmi->cf ? ANA_POL_MODE_CFG_DLB_COUPLED : 0) |
+ (cir_ena ? ANA_POL_MODE_CFG_CIR_ENA : 0) |
+ ANA_POL_MODE_CFG_OVERSHOOT_ENA),
+ ANA_POL_MODE_CFG, index);
+
+ ocelot_write_gix(ocelot, ANA_POL_PIR_CFG_PIR_RATE(pir) |
+ ANA_POL_PIR_CFG_PIR_BURST(pbs),
+ ANA_POL_PIR_CFG, index);
+
+ ocelot_write_gix(ocelot,
+ (pir_discard ? GENMASK(22, 0) : 0),
+ ANA_POL_PIR_STATE, index);
+
+ ocelot_write_gix(ocelot, ANA_POL_CIR_CFG_CIR_RATE(cir) |
+ ANA_POL_CIR_CFG_CIR_BURST(cbs),
+ ANA_POL_CIR_CFG, index);
+
+ ocelot_write_gix(ocelot,
+ (cir_discard ? GENMASK(22, 0) : 0),
+ ANA_POL_CIR_STATE, index);
+
+ return 0;
+}
+
+static int felix_qci_fmi_get(struct net_device *ndev, u32 index,
+ struct tsn_qci_psfp_fmi *fmi,
+ struct tsn_qci_psfp_fmi_counters *counters)
+{
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 val, reg;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index > capa.qos_pol_max) {
+ dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
+ index, capa.qos_pol_max);
+ return -EINVAL;
+ }
+
+ val = ocelot_read_gix(ocelot, ANA_POL_PIR_CFG, index);
+ reg = ocelot_read_gix(ocelot, ANA_POL_CIR_CFG, index);
+
+ fmi->eir = ANA_POL_PIR_CFG_PIR_RATE_X(val);
+ fmi->eir = fmi->eir * 100 / 3;
+ fmi->ebs = ANA_POL_PIR_CFG_PIR_BURST(val);
+ fmi->ebs *= 4096;
+ fmi->cir = ANA_POL_CIR_CFG_CIR_RATE_X(reg);
+ fmi->cir = fmi->cir * 100 / 3;
+ fmi->cbs = ANA_POL_CIR_CFG_CIR_BURST(reg);
+ fmi->cbs *= 4096;
+ if (!(fmi->eir | fmi->ebs | fmi->cir | fmi->cbs))
+ fmi->mark_red = 1;
+ else
+ fmi->mark_red = 0;
+
+ val = ocelot_read_gix(ocelot, ANA_POL_MODE_CFG, index);
+ if (val & ANA_POL_MODE_CFG_DLB_COUPLED)
+ fmi->cf = 1;
+ else
+ fmi->cf = 0;
+ if (val & ANA_POL_MODE_CFG_CIR_ENA)
+ fmi->drop_on_yellow = 0;
+ else
+ fmi->drop_on_yellow = 1;
+
+ return 0;
+}
+
+static int felix_qos_shaper_conf_set(struct ocelot *ocelot, int idx,
+ u8 percent, int speed)
+{
+ u32 cbs = 0;
+ u32 cir = 0;
+
+ if (percent > 100) {
+ dev_err(ocelot->dev, "percentage %d larger than 100\n",
+ percent);
+ return -EINVAL;
+ }
+ if (idx >= capa.num_hsch) {
+ dev_err(ocelot->dev,
+ "CIR_CFG: id %d is exceed num of HSCH instance\n",
+ idx);
+ return -EINVAL;
+ }
+
+ switch (speed) {
+ case SPEED_10:
+ cir = 10000;
+ break;
+ case SPEED_100:
+ cir = 100000;
+ break;
+ case SPEED_1000:
+ cir = 1000000;
+ break;
+ case SPEED_2500:
+ cir = 2500000;
+ break;
+ }
+
+ cir = cir * percent / 100;
+ cir = DIV_ROUND_UP(cir, 100); /* Rate unit is 100 kbps */
+ cir = (cir ? cir : 1); /* Avoid using zero rate */
+ cbs = DIV_ROUND_UP(cbs, 4096); /* Burst unit is 4kB */
+ cbs = (cbs ? cbs : 1); /* Avoid using zero burst size */
+ cir = min_t(u32, GENMASK(15, 0), cir);
+ cbs = min_t(u32, GENMASK(6, 0), cbs);
+
+ if (!percent) {
+ cir = 0;
+ cbs = 0;
+ }
+
+ ocelot_write_gix(ocelot,
+ QSYS_CIR_CFG_CIR_RATE(cir) |
+ QSYS_CIR_CFG_CIR_BURST(cbs),
+ QSYS_CIR_CFG,
+ idx);
+
+ return 0;
+}
+
+static int felix_cbs_set(struct net_device *ndev, u8 tc, u8 bw)
+{
+ struct ocelot_port *ocelot_port;
+ struct phylink_link_state state;
+ struct phylink_pcs *pcs;
+ struct ocelot *ocelot;
+ struct felix *felix;
+ struct dsa_port *dp;
+ int port, speed;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+ ocelot_port = ocelot->ports[port];
+
+ if (tc > capa.qos_cos_max) {
+ dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
+ return -EINVAL;
+ }
+
+ memset(&state, 0, sizeof(state));
+ state.interface = ocelot_port->phy_mode;
+
+ felix = ocelot_to_felix(ocelot);
+ pcs = felix->pcs[port];
+ pcs->ops->pcs_get_state(pcs, &state);
+
+ speed = state.speed;
+
+ felix_qos_shaper_conf_set(ocelot, port * 8 + tc, bw, speed);
+
+ ocelot_rmw_gix(ocelot,
+ QSYS_SE_CFG_SE_AVB_ENA,
+ QSYS_SE_CFG_SE_AVB_ENA,
+ QSYS_SE_CFG,
+ port * 8 + tc);
+
+ hsch_bw[port * 8 + tc] = bw;
+
+ return 0;
+}
+
+void felix_cbs_reset(struct ocelot *ocelot, int port, int speed)
+{
+ int i, idx;
+
+ for (i = 0; i < OCELOT_NUM_TC; i++) {
+ idx = port * 8 + i;
+ if (hsch_bw[idx] > 0)
+ felix_qos_shaper_conf_set(ocelot, idx, hsch_bw[idx],
+ speed);
+ }
+}
+
+static int felix_cbs_get(struct net_device *ndev, u8 tc)
+{
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ if (tc > capa.qos_cos_max) {
+ dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
+ return -EINVAL;
+ }
+
+ return hsch_bw[port * 8 + tc];
+}
+
+static int felix_cut_thru_set(struct net_device *ndev, u8 cut_thru)
+{
+ struct ocelot_port *ocelot_port;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+ ocelot_port = ocelot->ports[port];
+
+ mutex_lock(&ocelot->fwd_domain_lock);
+
+ ocelot_port->cut_thru = cut_thru;
+ ocelot->ops->cut_through_fwd(ocelot);
+
+ mutex_unlock(&ocelot->fwd_domain_lock);
+
+ return 0;
+}
+
+static int felix_rtag_parse_enable(struct ocelot *ocelot, u8 port)
+{
+ ocelot_rmw_rix(ocelot,
+ ANA_PORT_MODE_REDTAG_PARSE_CFG,
+ ANA_PORT_MODE_REDTAG_PARSE_CFG,
+ ANA_PORT_MODE,
+ port);
+
+ ocelot_write(ocelot, ETH_P_8021CB, SYS_SR_ETYPE_CFG);
+
+ /* No harm, no foul: we are telling the switch to adjust maximum frame
+ * length for double-tagged VLANs lying that the EtherType for S-Tags
+ * is the one for 802.1CB. This is not an issue because with 802.1CB
+ * traffic, the switch will not parse more than 2 tags anyway, so
+ * either it doesn't support 802.1CB or the second VLAN tag.
+ */
+ ocelot_port_writel(ocelot->ports[port],
+ DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021CB) |
+ DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
+ DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA |
+ DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
+ DEV_MAC_TAGS_CFG);
+
+ return 0;
+}
+
+static int felix_seq_gen_set(struct net_device *ndev, u32 index,
+ struct tsn_seq_gen_conf *sg_conf)
+{
+ u8 iport_mask = sg_conf->iport_mask;
+ u8 split_mask = sg_conf->split_mask;
+ u8 seq_len = sg_conf->seq_len;
+ u32 seq_num = sg_conf->seq_num;
+ struct stream_filter *tmp;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= capa.num_frer_ssid) {
+ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
+ index, capa.num_frer_ssid - 1);
+ return -EINVAL;
+ }
+ if (seq_len < capa.frer_seq_len_min ||
+ seq_len > capa.frer_seq_len_max) {
+ dev_err(ocelot->dev,
+ "Invalid seq_space_bits num %u,range:%d~%d\n",
+ seq_len,
+ capa.frer_seq_len_min,
+ capa.frer_seq_len_max);
+ return -EINVAL;
+ }
+
+ list_for_each_entry(tmp, &streamtable, list)
+ if (tmp->handle == index &&
+ tmp->dst_idx < ocelot->num_phys_ports)
+ ocelot_bridge_force_forward_port(ocelot, tmp->dst_idx, true);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_SEQ_MASK_SPLIT_MASK(split_mask) |
+ ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(iport_mask),
+ ANA_TABLES_SEQ_MASK);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMTIDX_S_INDEX(index) |
+ ANA_TABLES_STREAMTIDX_STREAM_SPLIT |
+ ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
+ ANA_TABLES_STREAMTIDX);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM(seq_num) |
+ ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
+ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
+ ANA_TABLES_STREAMACCESS);
+
+ return 0;
+}
+
+static int felix_seq_rec_set(struct net_device *ndev, u32 index,
+ struct tsn_seq_rec_conf *sr_conf)
+{
+ u8 seq_len = sr_conf->seq_len;
+ u8 hislen = sr_conf->his_len;
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ int i;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= capa.num_frer_ssid) {
+ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
+ index, capa.num_frer_ssid - 1);
+ return -EINVAL;
+ }
+ if (seq_len < capa.frer_seq_len_min ||
+ seq_len > capa.frer_seq_len_max) {
+ dev_err(ocelot->dev,
+ "Invalid seq_space_bits num %u,range:%d~%d\n",
+ seq_len,
+ capa.frer_seq_len_min,
+ capa.frer_seq_len_max);
+ return -EINVAL;
+ }
+ if (hislen < capa.frer_his_len_min ||
+ hislen > capa.frer_his_len_max) {
+ dev_err(ocelot->dev,
+ "Invalid history_bits num %u,range:%d~%d\n",
+ hislen,
+ capa.frer_his_len_min,
+ capa.frer_his_len_max);
+ return -EINVAL;
+ }
+
+ for (i = 0; i < ocelot->num_phys_ports; i++)
+ felix_rtag_parse_enable(ocelot, i);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMTIDX_S_INDEX(index) |
+ ANA_TABLES_STREAMTIDX_FORCE_SF_BEHAVIOUR |
+ ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN(hislen) |
+ ANA_TABLES_STREAMTIDX_RESET_ON_ROGUE |
+ (sr_conf->rtag_pop_en ?
+ ANA_TABLES_STREAMTIDX_REDTAG_POP : 0) |
+ ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
+ ANA_TABLES_STREAMTIDX);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
+ ANA_TABLES_STREAMACCESS_GEN_REC_TYPE |
+ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
+ ANA_TABLES_STREAMACCESS);
+
+ return 0;
+}
+
+static int felix_cb_get(struct net_device *ndev, u32 index,
+ struct tsn_cb_status *c)
+{
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 val;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+
+ if (index >= capa.num_frer_ssid) {
+ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
+ index, capa.num_frer_ssid - 1);
+ return -EINVAL;
+ }
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMTIDX_S_INDEX(index),
+ ANA_TABLES_STREAMTIDX);
+
+ ocelot_write(ocelot,
+ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_READ),
+ ANA_TABLES_STREAMACCESS);
+
+ val = ocelot_read(ocelot, ANA_TABLES_STREAMACCESS);
+ c->gen_rec = (ANA_TABLES_STREAMACCESS_GEN_REC_TYPE & val) >> 2;
+ c->seq_num = ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM_X(val);
+
+ val = ocelot_read(ocelot, ANA_TABLES_STREAMTIDX);
+ c->err = ANA_TABLES_STREAMTIDX_SEQ_GEN_ERR_STATUS_X(val);
+ c->his_len = ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN_X(val);
+ c->seq_len = ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(val);
+
+ val = ocelot_read(ocelot, ANA_TABLES_SEQ_MASK);
+ c->split_mask = ANA_TABLES_SEQ_MASK_SPLIT_MASK_X(val);
+ c->iport_mask = ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(val);
+
+ c->seq_his = ocelot_read(ocelot, ANA_TABLES_SEQ_HISTORY);
+
+ return 0;
+}
+
+static int felix_dscp_set(struct net_device *ndev, bool enable, const u8 dscp_ix,
+ struct tsn_qos_switch_dscp_conf *c)
+{
+ struct ocelot *ocelot;
+ struct dsa_port *dp;
+ u32 ri = dscp_ix;
+ u32 val;
+ int port;
+
+ dp = dsa_port_from_netdev(ndev);
+ ocelot = dp->ds->priv;
+ port = dp->index;
+
+ c->dscp = 0;
+ c->trust = 1;
+ c->remark = 0;
+
+ if (dscp_ix > capa.qos_dscp_max) {
+ dev_err(ocelot->dev, "Invalid dscp_ix %u\n", dscp_ix);
+ return -EINVAL;
+ }
+ if (c->cos > capa.qos_cos_max) {
+ dev_err(ocelot->dev, "Invalid cos %d\n", c->cos);
+ return -EINVAL;
+ }
+ if (c->dpl > capa.qos_dp_max) {
+ dev_err(ocelot->dev, "Invalid dpl %d\n", c->dpl);
+ return -EINVAL;
+ }
+
+ ocelot_rmw_gix(ocelot,
+ (enable ? ANA_PORT_QOS_CFG_QOS_DSCP_ENA : 0) |
+ (c->dscp ? ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA : 0),
+ ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
+ ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA,
+ ANA_PORT_QOS_CFG,
+ port);
+
+ val = (c->dpl ? ANA_DSCP_CFG_DP_DSCP_VAL : 0) |
+ ANA_DSCP_CFG_QOS_DSCP_VAL(c->cos) |
+ ANA_DSCP_CFG_DSCP_TRANSLATE_VAL(c->dscp) |
+ (c->trust ? ANA_DSCP_CFG_DSCP_TRUST_ENA : 0) |
+ (c->remark ? ANA_DSCP_CFG_DSCP_REWR_ENA : 0);
+
+ ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, ri);
+
+ return 0;
+}
+
+void felix_preempt_irq_clean(struct ocelot *ocelot)
+{
+ struct ocelot_port *ocelot_port;
+ int port;
+ u32 val;
+
+ val = DEV_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STICKY;
+ for (port = 0; port < ocelot->num_phys_ports; port++) {
+ ocelot_port = ocelot->ports[port];
+ ocelot_port_rmwl(ocelot_port, val, val, DEV_MM_STATUS);
+ }
+}
+
+static const struct tsn_ops felix_tsn_ops = {
+ .get_capability = felix_tsn_get_cap,
+ .qbv_set = felix_qbv_set,
+ .qbv_get = felix_qbv_get,
+ .qbv_get_status = felix_qbv_get_status,
+ .qbu_set = felix_qbu_set,
+ .qbu_get = felix_qbu_get,
+ .cb_streamid_set = felix_cb_streamid_set,
+ .cb_streamid_get = felix_cb_streamid_get,
+ .cb_streamid_counters_get = felix_cb_streamid_counters_get,
+ .qci_sfi_set = felix_qci_sfi_set,
+ .qci_sfi_get = felix_qci_sfi_get,
+ .qci_sfi_counters_get = felix_qci_sfi_counters_get,
+ .qci_get_maxcap = felix_qci_max_cap_get,
+ .qci_sgi_set = felix_qci_sgi_set,
+ .qci_sgi_get = felix_qci_sgi_get,
+ .qci_sgi_status_get = felix_qci_sgi_status_get,
+ .qci_fmi_set = felix_qci_fmi_set,
+ .qci_fmi_get = felix_qci_fmi_get,
+ .cbs_set = felix_cbs_set,
+ .cbs_get = felix_cbs_get,
+ .ct_set = felix_cut_thru_set,
+ .cbgen_set = felix_seq_gen_set,
+ .cbrec_set = felix_seq_rec_set,
+ .cb_get = felix_cb_get,
+ .dscp_set = felix_dscp_set,
+};
+
+int felix_tsn_enable(struct dsa_switch *ds)
+{
+ struct net_device *dev;
+ struct dsa_port *dp;
+ int port;
+
+ for (port = 0; port < ds->num_ports; port++) {
+ dp = dsa_to_port(ds, port);
+ if (dp->type == DSA_PORT_TYPE_USER) {
+ dev = dp->slave;
+ tsn_port_register(dev, (struct tsn_ops *)&felix_tsn_ops,
+ GROUP_OFFSET_SWITCH);
+ }
+ }
+
+ INIT_LIST_HEAD(&streamtable);
+
+ return 0;
+}
diff --git a/drivers/net/dsa/ocelot/felix_tsn.h b/drivers/net/dsa/ocelot/felix_tsn.h
new file mode 100644
index 0000000000000000000000000000000000000000..9895b85de8d904fb019b7bd74a7c203528d1d02b
--- /dev/null
+++ b/drivers/net/dsa/ocelot/felix_tsn.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: (GPL-2.0 OR MIT)
+ *
+ * Felix Switch TSN driver
+ *
+ * Copyright 2020-2021 NXP Semiconductors
+ */
+
+#ifndef _MSCC_FELIX_SWITCH_TSN_H_
+#define _MSCC_FELIX_SWITCH_TSN_H_
+
+#include <soc/mscc/ocelot.h>
+#include <net/dsa.h>
+
+void felix_preempt_irq_clean(struct ocelot *ocelot);
+void felix_cbs_reset(struct ocelot *ocelot, int port, int speed);
+int felix_tsn_enable(struct dsa_switch *ds);
+#endif
diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index a58956ffa4db9414a7ce40693e80502cc0f9d903..84bf75cdf7c6049c2e3946e97d26c7b4b7b14272 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -16,6 +16,7 @@
#include <linux/iopoll.h>
#include <linux/mdio.h>
#include <linux/pci.h>
+#include "felix_tsn.h"
#include "felix.h"
#define VSC9959_TAS_GCL_ENTRY_MAX 63
@@ -346,6 +347,9 @@ static const u32 vsc9959_dev_gmii_regmap[] = {
REG(DEV_MAC_FC_MAC_LOW_CFG, 0x3c),
REG(DEV_MAC_FC_MAC_HIGH_CFG, 0x40),
REG(DEV_MAC_STICKY, 0x44),
+ REG(DEV_MM_ENABLE_CONFIG, 0x48),
+ REG(DEV_MM_VERIF_CONFIG, 0x4C),
+ REG(DEV_MM_STATUS, 0x50),
REG_RESERVED(PCS1G_CFG),
REG_RESERVED(PCS1G_MODE_CFG),
REG_RESERVED(PCS1G_SD_CFG),
@@ -1139,7 +1143,7 @@ static void vsc9959_mdio_bus_free(struct ocelot *ocelot)
}
static void vsc9959_sched_speed_set(struct ocelot *ocelot, int port,
- u32 speed)
+ int speed)
{
u8 tas_speed;
@@ -1165,6 +1169,10 @@ static void vsc9959_sched_speed_set(struct ocelot *ocelot, int port,
QSYS_TAG_CONFIG_LINK_SPEED(tas_speed),
QSYS_TAG_CONFIG_LINK_SPEED_M,
QSYS_TAG_CONFIG, port);
+
+#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
+ felix_cbs_reset(ocelot, port, speed);
+#endif
}
static void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
@@ -2257,9 +2265,11 @@ static void vsc9959_cut_through_fwd(struct ocelot *ocelot)
min_speed = other_ocelot_port->speed;
}
- /* Enable cut-through forwarding for all traffic classes. */
+ /* Enable cut-through forwarding for the traffic classes
+ * selected by tsntool.
+ */
if (ocelot_port->speed == min_speed)
- val = GENMASK(7, 0);
+ val = ocelot_port->cut_thru;
set:
tmp = ocelot_read_rix(ocelot, ANA_CUT_THRU_CFG, port);
@@ -2326,10 +2336,13 @@ static irqreturn_t felix_irq_handler(int irq, void *data)
* and preemption status change interrupt on each port.
*
* - Get txtstamp if have
- * - TODO: handle preemption. Without handling it, driver may get
- * interrupt storm.
+ * - Handle preemption if it's preemption IRQ. Without handling it,
+ * driver may get interrupt storm.
*/
+#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
+ felix_preempt_irq_clean(ocelot);
+#endif
ocelot_get_txtstamp(ocelot);
return IRQ_HANDLED;
@@ -2402,6 +2415,10 @@ static int felix_pci_probe(struct pci_dev *pdev,
goto err_register_ds;
}
+#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
+ felix_tsn_enable(ds);
+#endif
+
return 0;
err_register_ds:
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index b039b4dba6ac9076bea13f0ffeb48d7c2bfdbc53..934775f8c3656f13cfeb7a2b1bf9e56de6cec763 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -15,12 +15,6 @@
#define TABLE_UPDATE_TIMEOUT_US 100000
#define OCELOT_RSV_VLAN_RANGE_START 4000
-struct ocelot_mact_entry {
- u8 mac[ETH_ALEN];
- u16 vid;
- enum macaccess_entry_type type;
-};
-
/* Caller must hold &ocelot->mact_lock */
static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
{
@@ -1401,10 +1395,10 @@ int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
/* Caller must hold &ocelot->mact_lock */
-static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
- struct ocelot_mact_entry *entry)
+int ocelot_mact_read(struct ocelot *ocelot, int row, int col, int *dst,
+ struct ocelot_mact_entry *entry)
{
- u32 val, dst, macl, mach;
+ u32 val, macl, mach;
char mac[ETH_ALEN];
/* Set row and column to read from */
@@ -1424,12 +1418,9 @@ static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
if (!(val & ANA_TABLES_MACACCESS_VALID))
return -EINVAL;
- /* If the entry read has another port configured as its destination,
- * do not report it.
- */
- dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
- if (dst != port)
- return -EINVAL;
+ *dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
+
+ entry->type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(val);
/* Get the entry's MAC address and VLAN id */
macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
@@ -1447,6 +1438,7 @@ static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
return 0;
}
+EXPORT_SYMBOL(ocelot_mact_read);
int ocelot_mact_flush(struct ocelot *ocelot, int port)
{
@@ -1501,16 +1493,18 @@ int ocelot_fdb_dump(struct ocelot *ocelot, int port,
for (j = 0; j < 4; j++) {
struct ocelot_mact_entry entry;
bool is_static;
+ int dst;
- err = ocelot_mact_read(ocelot, port, i, j, &entry);
- /* If the entry is invalid (wrong port, invalid...),
- * skip it.
- */
+ err = ocelot_mact_read(ocelot, i, j, &dst, &entry);
+ /* If the entry is invalid, skip it. */
if (err == -EINVAL)
continue;
else if (err)
break;
+ if (dst != port)
+ continue;
+
is_static = (entry.type == ENTRYTYPE_LOCKED);
/* Hide the reserved VLANs used for
@@ -2184,6 +2178,42 @@ void ocelot_port_unset_dsa_8021q_cpu(struct ocelot *ocelot, int port)
}
EXPORT_SYMBOL_GPL(ocelot_port_unset_dsa_8021q_cpu);
+void ocelot_bridge_force_forward_port(struct ocelot *ocelot, int port, bool en)
+{
+ struct ocelot_port *ocelot_port = ocelot->ports[port];
+ u32 mask;
+ int i;
+
+ mutex_lock(&ocelot->fwd_domain_lock);
+
+ if (!en) {
+ if (ocelot_port->force_forward) {
+ ocelot_apply_bridge_fwd_mask(ocelot, false);
+ ocelot_port->force_forward = 0;
+ }
+ mutex_unlock(&ocelot->fwd_domain_lock);
+ return;
+ }
+
+ if (ocelot_port->force_forward) {
+ mutex_unlock(&ocelot->fwd_domain_lock);
+ return;
+ }
+
+ ocelot_port->force_forward = 1;
+ for (i = 0; i < ocelot->num_phys_ports; i++) {
+ if (i == port)
+ continue;
+
+ mask = ocelot_read_rix(ocelot, ANA_PGID_PGID, PGID_SRC + i);
+ mask |= BIT(port);
+ ocelot_write_rix(ocelot, mask, ANA_PGID_PGID, PGID_SRC + i);
+ }
+
+ mutex_unlock(&ocelot->fwd_domain_lock);
+}
+EXPORT_SYMBOL(ocelot_bridge_force_forward_port);
+
void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
{
struct ocelot_port *ocelot_port = ocelot->ports[port];
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 8e9c3a085327d5c15cb2874562106d0c950b004e..19985c4c07492c1fc166f22cc7ff0f5b41c10a79 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -51,6 +51,7 @@
*/
/* Reserve some destination PGIDs at the end of the range:
+ * PGID_FRER: Destinations for multicast traffic in 802.1CB redundant network.
* PGID_BLACKHOLE: used for not forwarding the frames
* PGID_CPU: used for whitelisting certain MAC addresses, such as the addresses
* of the switch port net devices, towards the CPU port module.
@@ -60,6 +61,7 @@
* PGID_MCIPV6: the flooding destinations for IPv6 multicast traffic.
* PGID_BC: the flooding destinations for broadcast traffic.
*/
+#define PGID_FRER 56
#define PGID_BLACKHOLE 57
#define PGID_CPU 58
#define PGID_UC 59
@@ -75,7 +77,7 @@
#define for_each_nonreserved_multicast_dest_pgid(ocelot, pgid) \
for ((pgid) = (ocelot)->num_phys_ports + 1; \
- (pgid) < PGID_BLACKHOLE; \
+ (pgid) < PGID_FRER; \
(pgid)++)
#define for_each_aggr_pgid(ocelot, pgid) \
@@ -414,6 +416,9 @@ enum ocelot_reg {
DEV_MAC_FC_MAC_LOW_CFG,
DEV_MAC_FC_MAC_HIGH_CFG,
DEV_MAC_STICKY,
+ DEV_MM_ENABLE_CONFIG,
+ DEV_MM_VERIF_CONFIG,
+ DEV_MM_STATUS,
PCS1G_CFG,
PCS1G_MODE_CFG,
PCS1G_SD_CFG,
@@ -632,6 +637,12 @@ enum macaccess_entry_type {
ENTRYTYPE_MACv6,
};
+struct ocelot_mact_entry {
+ u8 mac[ETH_ALEN];
+ u16 vid;
+ enum macaccess_entry_type type;
+};
+
#define OCELOT_QUIRK_PCS_PERFORMS_RATE_ADAPTATION BIT(0)
#define OCELOT_QUIRK_QSGMII_PORTS_MUST_BE_UP BIT(1)
@@ -671,6 +682,9 @@ struct ocelot_port {
int bridge_num;
u8 stp_state;
+ bool force_forward;
+ u8 cut_thru;
+
int speed;
s64 base_time;
@@ -762,6 +776,22 @@ struct ocelot_policer {
u32 burst; /* bytes */
};
+int ocelot_mact_read(struct ocelot *ocelot, int row, int col, int *dst,
+ struct ocelot_mact_entry *entry);
+int ocelot_mact_learn(struct ocelot *ocelot, int port,
+ const unsigned char mac[ETH_ALEN],
+ unsigned int vid, enum macaccess_entry_type type);
+int ocelot_mact_forget(struct ocelot *ocelot, const unsigned char mac[ETH_ALEN],
+ unsigned int vid);
+int ocelot_mact_lookup(struct ocelot *ocelot, int *dst_idx,
+ const unsigned char mac[ETH_ALEN],
+ unsigned int vid, enum macaccess_entry_type *type);
+int ocelot_mact_learn_streamdata(struct ocelot *ocelot, int dst_idx,
+ const unsigned char mac[ETH_ALEN],
+ unsigned int vid,
+ enum macaccess_entry_type type,
+ int sfid, int ssid);
+
#define ocelot_bulk_read_rix(ocelot, reg, ri, buf, count) \
__ocelot_bulk_read_ix(ocelot, reg, reg##_RSZ * (ri), buf, count)
@@ -860,6 +890,7 @@ int ocelot_get_ts_info(struct ocelot *ocelot, int port,
void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs);
int ocelot_port_vlan_filtering(struct ocelot *ocelot, int port, bool enabled,
struct netlink_ext_ack *extack);
+void ocelot_bridge_force_forward_port(struct ocelot *ocelot, int port, bool en);
void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state);
u32 ocelot_get_dsa_8021q_cpu_mask(struct ocelot *ocelot);
u32 ocelot_get_bridge_fwd_mask(struct ocelot *ocelot, int src_port);
@@ -967,15 +998,6 @@ void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port,
bool tx_pause, bool rx_pause,
unsigned long quirks);
-int ocelot_mact_lookup(struct ocelot *ocelot, int *dst_idx,
- const unsigned char mac[ETH_ALEN],
- unsigned int vid, enum macaccess_entry_type *type);
-int ocelot_mact_learn_streamdata(struct ocelot *ocelot, int dst_idx,
- const unsigned char mac[ETH_ALEN],
- unsigned int vid,
- enum macaccess_entry_type type,
- int sfid, int ssid);
-
int ocelot_vcap_policer_add(struct ocelot *ocelot, u32 pol_ix,
struct ocelot_policer *pol);
int ocelot_vcap_policer_del(struct ocelot *ocelot, u32 pol_ix);
diff --git a/include/soc/mscc/ocelot_ana.h b/include/soc/mscc/ocelot_ana.h
index 67e0ae05a5ab6ac79262675c701661a2904d5dc1..ff2b20e9af08d1a8fdf8007e66803406c2fed085 100644
--- a/include/soc/mscc/ocelot_ana.h
+++ b/include/soc/mscc/ocelot_ana.h
@@ -138,6 +138,9 @@
#define MACACCESS_CMD_READ 6
#define MACACCESS_CMD_WRITE 7
+#define MACACCESS_ENTRY_TYPE_NORMAL 0
+#define MACACCESS_ENTRY_TYPE_LOCKED 1
+
#define ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(x) (((x) << 2) & GENMASK(13, 2))
#define ANA_TABLES_VLANACCESS_VLAN_PORT_MASK_M GENMASK(13, 2)
#define ANA_TABLES_VLANACCESS_VLAN_PORT_MASK_X(x) (((x) & GENMASK(13, 2)) >> 2)
@@ -271,6 +274,9 @@
#define ANA_SG_GCL_GS_CONFIG_IPS(x) ((x) & GENMASK(3, 0))
#define ANA_SG_GCL_GS_CONFIG_IPS_M GENMASK(3, 0)
+#define ANA_SG_GCL_GS_CONFIG_IPV_VALID BIT(3)
+#define ANA_SG_GCL_GS_CONFIG_IPV(x) ((x) & GENMASK(2, 0))
+#define ANA_SG_GCL_GS_CONFIG_IPV_M GENMASK(2, 0)
#define ANA_SG_GCL_GS_CONFIG_GATE_STATE BIT(4)
#define ANA_SG_GCL_TI_CONFIG_RSZ 0x4
@@ -281,6 +287,10 @@
#define ANA_SG_STATUS_REG_3_IPS(x) (((x) << 20) & GENMASK(23, 20))
#define ANA_SG_STATUS_REG_3_IPS_M GENMASK(23, 20)
#define ANA_SG_STATUS_REG_3_IPS_X(x) (((x) & GENMASK(23, 20)) >> 20)
+#define ANA_SG_STATUS_REG_3_IPV_VALID BIT(23)
+#define ANA_SG_STATUS_REG_3_IPV(x) (((x) << 20) & GENMASK(22, 20))
+#define ANA_SG_STATUS_REG_3_IPV_M GENMASK(22, 20)
+#define ANA_SG_STATUS_REG_3_IPV_X(x) (((x) & GENMASK(22, 20)) >> 20)
#define ANA_SG_STATUS_REG_3_CONFIG_PENDING BIT(24)
#define ANA_PORT_VLAN_CFG_GSZ 0x100
diff --git a/include/soc/mscc/ocelot_dev.h b/include/soc/mscc/ocelot_dev.h
index 0c6021f02fee675e0b86dbe49a6007c1460db387..cb1d8f5a62ee0f3078e9b8166bb539804f4e4c1c 100644
--- a/include/soc/mscc/ocelot_dev.h
+++ b/include/soc/mscc/ocelot_dev.h
@@ -93,6 +93,29 @@
#define DEV_MAC_STICKY_TX_FRM_LEN_OVR_STICKY BIT(1)
#define DEV_MAC_STICKY_TX_ABORT_STICKY BIT(0)
+#define DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA BIT(0)
+#define DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA BIT(4)
+#define DEV_MM_CONFIG_ENABLE_CONFIG_KEEP_S_AFTER_D BIT(8)
+
+#define DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_DIS BIT(0)
+#define DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME(x) (((x) << 4) & GENMASK(11, 4))
+#define DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_M GENMASK(11, 4)
+#define DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_X(x) (((x) & GENMASK(11, 4)) >> 4)
+#define DEV_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS(x) (((x) << 12) & GENMASK(13, 12))
+#define DEV_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_M GENMASK(13, 12)
+#define DEV_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_X(x) (((x) & GENMASK(13, 12)) >> 12)
+
+#define DEV_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS BIT(0)
+#define DEV_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STICKY BIT(4)
+#define DEV_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE(x) (((x) << 8) & GENMASK(10, 8))
+#define DEV_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_M GENMASK(10, 8)
+#define DEV_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_X(x) (((x) & GENMASK(10, 8)) >> 8)
+#define DEV_MM_STATISTICS_MM_STATUS_UNEXP_RX_PFRM_STICKY BIT(12)
+#define DEV_MM_STATISTICS_MM_STATUS_UNEXP_TX_PFRM_STICKY BIT(16)
+#define DEV_MM_STATISTICS_MM_STATUS_MM_RX_FRAME_STATUS BIT(20)
+#define DEV_MM_STATISTICS_MM_STATUS_MM_TX_FRAME_STATUS BIT(24)
+#define DEV_MM_STATISTICS_MM_STATUS_MM_TX_PRMPT_STATUS BIT(28)
+
#define PCS1G_CFG_LINK_STATUS_TYPE BIT(4)
#define PCS1G_CFG_AN_LINK_CTRL_ENA BIT(1)
#define PCS1G_CFG_PCS_ENA BIT(0)