[RFC, net-next 3/3] net: dsa: felix: tc-taprio preempt set support

[Xiaoliang Yang <xiaoliang.yang_1@nxp.com>]

After using ethtool to enable and configure frame preemption on
vsc9959, use tc-taprio preempt set to mark the preempt queues and
express queueus.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/dsa/ocelot/felix_vsc9959.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index c0e41d499639..f2b9a5ee1ff5 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -1310,6 +1310,20 @@ static int vsc9959_qos_port_cbs_set(struct dsa_switch *ds, int port,
 	return 0;
 }
 
+static int vsc9959_port_preempt_queues(struct ocelot *ocelot, int port,
+				       struct tc_preempt_qopt_offload *qopt)
+{
+	u8 p_queues = qopt->preemptible_queues;
+
+	ocelot_rmw_rix(ocelot,
+		       QSYS_PREEMPTION_CFG_P_QUEUES(p_queues),
+		       QSYS_PREEMPTION_CFG_P_QUEUES_M,
+		       QSYS_PREEMPTION_CFG,
+		       port);
+
+	return 0;
+}
+
 static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 				 enum tc_setup_type type,
 				 void *type_data)
@@ -1321,6 +1335,8 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 		return vsc9959_qos_port_tas_set(ocelot, port, type_data);
 	case TC_SETUP_QDISC_CBS:
 		return vsc9959_qos_port_cbs_set(ds, port, type_data);
+	case TC_SETUP_PREEMPT:
+		return vsc9959_port_preempt_queues(ocelot, port, type_data);
 	default:
 		return -EOPNOTSUPP;
 	}
-- 
2.18.4