From: Vladimir Oltean <vladimir.olt...@nxp.com>
The felix DSA driver will inject some frames through register MMIO, same
as ocelot switchdev currently does. So we need to be able to reuse the
common code.
Signed-off-by: Vladimir Oltean <vladimir.olt...@nxp.com>
---
drivers/net/ethernet/mscc/ocelot.c | 80 +++++++++++++++++++++++++
drivers/net/ethernet/mscc/ocelot_net.c | 81 +++-----------------------
include/soc/mscc/ocelot.h | 4 ++
3 files changed, 91 insertions(+), 74 deletions(-)
diff --git a/drivers/net/ethernet/mscc/ocelot.c
b/drivers/net/ethernet/mscc/ocelot.c
index d1a9cdbf7a3e..7106d9ee534a 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -628,6 +628,86 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
}
EXPORT_SYMBOL(ocelot_get_txtstamp);
+/* Generate the IFH for frame injection
+ *
+ * The IFH is a 128bit-value
+ * bit 127: bypass the analyzer processing
+ * bit 56-67: destination mask
+ * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
+ * bit 20-27: cpu extraction queue mask
+ * bit 16: tag type 0: C-tag, 1: S-tag
+ * bit 0-11: VID
+ */
+static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
+{
+ ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
+ ifh[1] = (0xf00 & info->port) >> 8;
+ ifh[2] = (0xff & info->port) << 24;
+ ifh[3] = (info->tag_type << 16) | info->vid;
+
+ return 0;
+}
+
+bool ocelot_can_inject(struct ocelot *ocelot, int grp)
+{
+ u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
+
+ if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
+ return false;
+ if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
+ return false;
+
+ return true;
+}
+EXPORT_SYMBOL(ocelot_can_inject);
+
+void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
+ u32 rew_op, struct sk_buff *skb)
+{
+ struct frame_info info = {};
+ u32 ifh[OCELOT_TAG_LEN / 4];
+ unsigned int i, count, last;
+
+ ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
+ QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
+
+ info.port = BIT(port);
+ info.tag_type = IFH_TAG_TYPE_C;
+ info.vid = skb_vlan_tag_get(skb);
+ info.rew_op = rew_op;
+
+ ocelot_gen_ifh(ifh, &info);
+
+ for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
+ ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
+ QS_INJ_WR, grp);
+
+ count = DIV_ROUND_UP(skb->len, 4);
+ last = skb->len % 4;
+ for (i = 0; i < count; i++)
+ ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
+
+ /* Add padding */
+ while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
+ ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
+ i++;
+ }
+
+ /* Indicate EOF and valid bytes in last word */
+ ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
+ QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ
? 0 : last) |
+ QS_INJ_CTRL_EOF,
+ QS_INJ_CTRL, grp);
+
+ /* Add dummy CRC */
+ ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
+ skb_tx_timestamp(skb);
+
+ skb->dev->stats.tx_packets++;
+ skb->dev->stats.tx_bytes += skb->len;
+}
+EXPORT_SYMBOL(ocelot_port_inject_frame);
+
int ocelot_fdb_add(struct ocelot *ocelot, int port,
const unsigned char *addr, u16 vid)
{
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c
b/drivers/net/ethernet/mscc/ocelot_net.c
index 1ab453298a18..6518262532f0 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -488,53 +488,20 @@ static int ocelot_port_stop(struct net_device *dev)
return 0;
}
-/* Generate the IFH for frame injection
- *
- * The IFH is a 128bit-value
- * bit 127: bypass the analyzer processing
- * bit 56-67: destination mask
- * bit 28-29: pop_cnt: 3 disables all rewriting of the frame
- * bit 20-27: cpu extraction queue mask
- * bit 16: tag type 0: C-tag, 1: S-tag
- * bit 0-11: VID
- */
-static int ocelot_gen_ifh(u32 *ifh, struct frame_info *info)
-{
- ifh[0] = IFH_INJ_BYPASS | ((0x1ff & info->rew_op) << 21);
- ifh[1] = (0xf00 & info->port) >> 8;
- ifh[2] = (0xff & info->port) << 24;
- ifh[3] = (info->tag_type << 16) | info->vid;
-
- return 0;
-}
-
-static int ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev)
+static netdev_tx_t ocelot_port_xmit(struct sk_buff *skb, struct net_device
*dev)
{
struct ocelot_port_private *priv = netdev_priv(dev);
- struct skb_shared_info *shinfo = skb_shinfo(skb);
struct ocelot_port *ocelot_port = &priv->port;
struct ocelot *ocelot = ocelot_port->ocelot;
- u32 val, ifh[OCELOT_TAG_LEN / 4];
- struct frame_info info = {};
- u8 grp = 0; /* Send everything on CPU group 0 */
- unsigned int i, count, last;
int port = priv->chip_port;
+ u32 rew_op = 0;
- val = ocelot_read(ocelot, QS_INJ_STATUS);
- if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))) ||
- (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp))))
+ if (!ocelot_can_inject(ocelot, 0))
return NETDEV_TX_BUSY;
- ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
- QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
-
- info.port = BIT(port);
- info.tag_type = IFH_TAG_TYPE_C;
- info.vid = skb_vlan_tag_get(skb);
-
/* Check if timestamping is needed */
- if (ocelot->ptp && (shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
- info.rew_op = ocelot_port->ptp_cmd;
+ if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+ rew_op = ocelot_port->ptp_cmd;
if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
struct sk_buff *clone;
@@ -547,45 +514,11 @@ static int ocelot_port_xmit(struct sk_buff *skb, struct
net_device *dev)
ocelot_port_add_txtstamp_skb(ocelot, port, clone);
- info.rew_op |= clone->cb[0] << 3;
+ rew_op |= clone->cb[0] << 3;
}
}
- if (ocelot->ptp && shinfo->tx_flags & SKBTX_HW_TSTAMP) {
- info.rew_op = ocelot_port->ptp_cmd;
- if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
- info.rew_op |= skb->cb[0] << 3;
- }
-
- ocelot_gen_ifh(ifh, &info);
-
- for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
- ocelot_write_rix(ocelot, (__force u32)cpu_to_be32(ifh[i]),
- QS_INJ_WR, grp);
-
- count = DIV_ROUND_UP(skb->len, 4);
- last = skb->len % 4;
- for (i = 0; i < count; i++)
- ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
-
- /* Add padding */
- while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
- ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
- i++;
- }
-
- /* Indicate EOF and valid bytes in last word */
- ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
- QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ
? 0 : last) |
- QS_INJ_CTRL_EOF,
- QS_INJ_CTRL, grp);
-
- /* Add dummy CRC */
- ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
- skb_tx_timestamp(skb);
-
- dev->stats.tx_packets++;
- dev->stats.tx_bytes += skb->len;
+ ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);
kfree_skb(skb);
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 40792b37bb9f..656fd8bc818d 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -847,4 +847,8 @@ int ocelot_sb_occ_tc_port_bind_get(struct ocelot *ocelot,
int port,
enum devlink_sb_pool_type pool_type,
u32 *p_cur, u32 *p_max);
+bool ocelot_can_inject(struct ocelot *ocelot, int grp);
+void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
+ u32 rew_op, struct sk_buff *skb);
+
#endif
--
2.25.1
