@@ -629,6 +629,157 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
}
EXPORT_SYMBOL(ocelot_get_txtstamp);
+static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
+ u32 *rval)
+{
+ u32 bytes_valid, val;
+
+ val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
+ if (val == XTR_NOT_READY) {
+ if (ifh)
+ return -EIO;
+
+ do {
+ val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
+ } while (val == XTR_NOT_READY);
+ }
+
+ switch (val) {
+ case XTR_ABORT:
+ return -EIO;
+ case XTR_EOF_0:
+ case XTR_EOF_1:
+ case XTR_EOF_2:
+ case XTR_EOF_3:
+ case XTR_PRUNED:
+ bytes_valid = XTR_VALID_BYTES(val);
+ val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
+ if (val == XTR_ESCAPE)
+ *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
+ else
+ *rval = val;
+
+ return bytes_valid;
+ case XTR_ESCAPE:
+ *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
+
+ return 4;
+ default:
+ *rval = val;
+
+ return 4;
+ }
+}
+
+static int ocelot_xtr_poll_xfh(struct ocelot *ocelot, int grp, u32 *xfh)
+{
+ int i, err = 0;
+
+ for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
+ err = ocelot_rx_frame_word(ocelot, grp, true, &xfh[i]);
+ if (err != 4)
+ return (err < 0) ? err : -EIO;
+ }
+
+ return 0;
+}
+
+int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **nskb)
+{
+ struct skb_shared_hwtstamps *shhwtstamps;
+ u64 tod_in_ns, full_ts_in_ns;
+ u64 timestamp, src_port, len;
+ u32 xfh[OCELOT_TAG_LEN / 4];
+ struct net_device *dev;
+ struct timespec64 ts;
+ struct sk_buff *skb;
+ int sz, buf_len;
+ u32 val, *buf;
+ int err;
+
+ err = ocelot_xtr_poll_xfh(ocelot, grp, xfh);
+ if (err)
+ return err;
+
+ ocelot_xfh_get_src_port(xfh, &src_port);
+ ocelot_xfh_get_len(xfh, &len);
+ ocelot_xfh_get_rew_val(xfh, ×tamp);
+
+ if (WARN_ON(src_port >= ocelot->num_phys_ports))
+ return -EINVAL;
+
+ dev = ocelot->ops->port_to_netdev(ocelot, src_port);
+ if (!dev)
+ return -EINVAL;
+
+ skb = netdev_alloc_skb(dev, len);
+ if (unlikely(!skb)) {
+ netdev_err(dev, "Unable to allocate sk_buff\n");
+ return -ENOMEM;
+ }
+
+ buf_len = len - ETH_FCS_LEN;
+ buf = (u32 *)skb_put(skb, buf_len);
+
+ len = 0;
+ do {
+ sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
+ if (sz < 0) {
+ err = sz;
+ goto out_free_skb;
+ }
+ *buf++ = val;
+ len += sz;
+ } while (len < buf_len);
+
+ /* Read the FCS */
+ sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
+ if (sz < 0) {
+ err = sz;
+ goto out_free_skb;
+ }
+
+ /* Update the statistics if part of the FCS was read before */
+ len -= ETH_FCS_LEN - sz;
+
+ if (unlikely(dev->features & NETIF_F_RXFCS)) {
+ buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
+ *buf = val;
+ }
+
+ if (ocelot->ptp) {
+ ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
+
+ tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
+ if ((tod_in_ns & 0xffffffff) < timestamp)
+ full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
+ timestamp;
+ else
+ full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
+ timestamp;
+
+ shhwtstamps = skb_hwtstamps(skb);
+ memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
+ shhwtstamps->hwtstamp = full_ts_in_ns;
+ }
+
+ /* Everything we see on an interface that is in the HW bridge
+ * has already been forwarded.
+ */
+ if (ocelot->bridge_mask & BIT(src_port))
+ skb->offload_fwd_mark = 1;
+
+ skb->protocol = eth_type_trans(skb, dev);
+ *nskb = skb;
+
+ return 0;
+
+out_free_skb:
+ kfree_skb(skb);
+ return err;
+}
+EXPORT_SYMBOL(ocelot_xtr_poll_frame);
+
bool ocelot_can_inject(struct ocelot *ocelot, int grp)
{
u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
@@ -531,153 +531,23 @@ static int ocelot_chip_init(struct ocelot *ocelot, const struct ocelot_ops *ops)
return 0;
}
-static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
- u32 *rval)
-{
- u32 val;
- u32 bytes_valid;
-
- val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
- if (val == XTR_NOT_READY) {
- if (ifh)
- return -EIO;
-
- do {
- val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
- } while (val == XTR_NOT_READY);
- }
-
- switch (val) {
- case XTR_ABORT:
- return -EIO;
- case XTR_EOF_0:
- case XTR_EOF_1:
- case XTR_EOF_2:
- case XTR_EOF_3:
- case XTR_PRUNED:
- bytes_valid = XTR_VALID_BYTES(val);
- val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
- if (val == XTR_ESCAPE)
- *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
- else
- *rval = val;
-
- return bytes_valid;
- case XTR_ESCAPE:
- *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
-
- return 4;
- default:
- *rval = val;
-
- return 4;
- }
-}
-
static irqreturn_t ocelot_xtr_irq_handler(int irq, void *arg)
{
struct ocelot *ocelot = arg;
- int i = 0, grp = 0;
- int err = 0;
+ int grp = 0, err;
while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)) {
- struct skb_shared_hwtstamps *shhwtstamps;
- struct ocelot_port_private *priv;
- struct ocelot_port *ocelot_port;
- u64 tod_in_ns, full_ts_in_ns;
- u64 src_port, len, timestamp;
- struct net_device *dev;
- u32 xfh[4], val, *buf;
- struct timespec64 ts;
struct sk_buff *skb;
- int sz, buf_len;
-
- for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
- err = ocelot_rx_frame_word(ocelot, grp, true, &xfh[i]);
- if (err != 4)
- goto out;
- }
-
- /* At this point the XFH was read correctly, so it is safe to
- * presume that there is no error. The err needs to be reset
- * otherwise a frame could come in CPU queue between the while
- * condition and the check for error later on. And in that case
- * the new frame is just removed and not processed.
- */
- err = 0;
-
- ocelot_xfh_get_src_port(xfh, &src_port);
- ocelot_xfh_get_len(xfh, &len);
- ocelot_xfh_get_rew_val(xfh, ×tamp);
-
- ocelot_port = ocelot->ports[src_port];
- priv = container_of(ocelot_port, struct ocelot_port_private,
- port);
- dev = priv->dev;
- skb = netdev_alloc_skb(dev, len);
-
- if (unlikely(!skb)) {
- netdev_err(dev, "Unable to allocate sk_buff\n");
- err = -ENOMEM;
- goto out;
- }
- buf_len = len - ETH_FCS_LEN;
- buf = (u32 *)skb_put(skb, buf_len);
-
- len = 0;
- do {
- sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
- if (sz < 0) {
- err = sz;
- goto out;
- }
- *buf++ = val;
- len += sz;
- } while (len < buf_len);
-
- /* Read the FCS */
- sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
- if (sz < 0) {
- err = sz;
+ err = ocelot_xtr_poll_frame(ocelot, grp, &skb);
+ if (err)
goto out;
- }
-
- /* Update the statistics if part of the FCS was read before */
- len -= ETH_FCS_LEN - sz;
-
- if (unlikely(dev->features & NETIF_F_RXFCS)) {
- buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
- *buf = val;
- }
-
- if (ocelot->ptp) {
- ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
-
- tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
- if ((tod_in_ns & 0xffffffff) < timestamp)
- full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
- timestamp;
- else
- full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
- timestamp;
-
- shhwtstamps = skb_hwtstamps(skb);
- memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
- shhwtstamps->hwtstamp = full_ts_in_ns;
- }
- /* Everything we see on an interface that is in the HW bridge
- * has already been forwarded.
- */
- if (ocelot->bridge_mask & BIT(src_port))
- skb->offload_fwd_mark = 1;
+ skb->dev->stats.rx_bytes += skb->len;
+ skb->dev->stats.rx_packets++;
- skb->protocol = eth_type_trans(skb, dev);
if (!skb_defer_rx_timestamp(skb))
netif_rx(skb);
- dev->stats.rx_bytes += len;
- dev->stats.rx_packets++;
}
out:
@@ -843,5 +843,6 @@ int ocelot_sb_occ_tc_port_bind_get(struct ocelot *ocelot, int port,
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);
+int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **skb);
#endif