Cc: Stefan Althöfer <Stefan.Althoefer@xxxxxxxxxxx>
Cc: Thomas Kopp <thomas.kopp@xxxxxxxxxxxxx>
Signed-off-by: Marc Kleine-Budde <mkl@xxxxxxxxxxxxxx>
---
.../net/can/spi/mcp251xfd/mcp251xfd-ring.c | 1 +
drivers/net/can/spi/mcp251xfd/mcp251xfd-tef.c | 50 +++++++++++++------
drivers/net/can/spi/mcp251xfd/mcp251xfd.h | 13 ++---
3 files changed, 40 insertions(+), 24 deletions(-)
diff --git a/drivers/net/can/spi/mcp251xfd/mcp251xfd-ring.c b/drivers/net/can/spi/mcp251xfd/mcp251xfd-ring.c
index f69d5fc8c9af..8ae9a3091dac 100644
--- a/drivers/net/can/spi/mcp251xfd/mcp251xfd-ring.c
+++ b/drivers/net/can/spi/mcp251xfd/mcp251xfd-ring.c
@@ -476,6 +476,7 @@ int mcp251xfd_ring_alloc(struct mcp251xfd_priv *priv)
clear_bit(MCP251XFD_FLAGS_FD_MODE, priv->flags);
}
+ tx_ring->obj_num_shift_to_u8 = BITS_PER_BYTE - ilog2(tx_ring->obj_num);
tx_ring->obj_size = tx_obj_size;
rem = priv->rx_obj_num;
diff --git a/drivers/net/can/spi/mcp251xfd/mcp251xfd-tef.c b/drivers/net/can/spi/mcp251xfd/mcp251xfd-tef.c
index 0c89e4f33976..e0c5ac943d8b 100644
--- a/drivers/net/can/spi/mcp251xfd/mcp251xfd-tef.c
+++ b/drivers/net/can/spi/mcp251xfd/mcp251xfd-tef.c
@@ -120,28 +120,49 @@ mcp251xfd_handle_tefif_one(struct mcp251xfd_priv *priv,
return 0;
}
-static int mcp251xfd_tef_ring_update(struct mcp251xfd_priv *priv)
+static inline bool mcp251xfd_tx_fifo_sta_full(u32 fifo_sta)
+{
+ return !(fifo_sta & MCP251XFD_REG_FIFOSTA_TFNRFNIF);
+}
+
+static int
+mcp251xfd_get_tef_len(struct mcp251xfd_priv *priv, u8 *len_p)
{
const struct mcp251xfd_tx_ring *tx_ring = priv->tx;
- unsigned int new_head;
- u8 chip_tx_tail;
+ u8 chip_tx_tail, tail, shift, len;
+ u32 fifo_sta;
int err;
- err = mcp251xfd_tx_tail_get_from_chip(priv, &chip_tx_tail);
+ err = regmap_read(priv->map_reg,
+ MCP251XFD_REG_FIFOSTA(priv->tx->fifo_nr),
+ &fifo_sta);
if (err)
return err;
+ if (mcp251xfd_tx_fifo_sta_full(fifo_sta)) {
+ *len_p = tx_ring->obj_num;
+ return 0;
+ }
+
/* chip_tx_tail, is the next TX-Object send by the HW.
- * The new TEF head must be >= the old head, ...
+ * The new TEF head must be >= the old head.
*/
- new_head = round_down(priv->tef->head, tx_ring->obj_num) + chip_tx_tail;
- if (new_head <= priv->tef->head)
- new_head += tx_ring->obj_num;
+ chip_tx_tail = FIELD_GET(MCP251XFD_REG_FIFOSTA_FIFOCI_MASK, fifo_sta);
- /* ... but it cannot exceed the TX head. */
- priv->tef->head = min(new_head, tx_ring->head);
+ err = mcp251xfd_check_tef_tail(priv);
+ if (err)
+ return err;
+ tail = mcp251xfd_get_tef_tail(priv);
+
+ /* First shift to full u8. The subtraction works on singed
+ * values, that keeps the difference steady around the u8
+ * overflow. The right shift acts on len, which is an u8.
+ */
+ shift = tx_ring->obj_num_shift_to_u8;
+ len = (chip_tx_tail << shift) - (tail << shift);
+ *len_p = len >> shift;
- return mcp251xfd_check_tef_tail(priv);
+ return 0;
}
static inline int
@@ -182,13 +203,12 @@ int mcp251xfd_handle_tefif(struct mcp251xfd_priv *priv)
u8 tef_tail, len, l;
int err, i;
- err = mcp251xfd_tef_ring_update(priv);
+ err = mcp251xfd_get_tef_len(priv, &len);
if (err)
return err;
tef_tail = mcp251xfd_get_tef_tail(priv);
- len = mcp251xfd_get_tef_len(priv);
- l = mcp251xfd_get_tef_linear_len(priv);
+ l = mcp251xfd_get_tef_linear_len(priv, len);
err = mcp251xfd_tef_obj_read(priv, hw_tef_obj, tef_tail, l);
if (err)
return err;
@@ -223,6 +243,8 @@ int mcp251xfd_handle_tefif(struct mcp251xfd_priv *priv)
struct mcp251xfd_tx_ring *tx_ring = priv->tx;
int offset;
+ ring->head += len;
+
/* Increment the TEF FIFO tail pointer 'len' times in
* a single SPI message.
*
diff --git a/drivers/net/can/spi/mcp251xfd/mcp251xfd.h b/drivers/net/can/spi/mcp251xfd/mcp251xfd.h
index 6008d38810e9..66d1fcf82bfd 100644
--- a/drivers/net/can/spi/mcp251xfd/mcp251xfd.h
+++ b/drivers/net/can/spi/mcp251xfd/mcp251xfd.h
@@ -518,6 +518,7 @@ struct mcp251xfd_tef_ring {
/* u8 obj_num equals tx_ring->obj_num */
/* u8 obj_size equals sizeof(struct mcp251xfd_hw_tef_obj) */
+ /* u8 obj_num_shift_to_u8 equals tx_ring->obj_num_shift_to_u8 */
union mcp251xfd_write_reg_buf irq_enable_buf;
struct spi_transfer irq_enable_xfer;
@@ -536,6 +537,7 @@ struct mcp251xfd_tx_ring {
u8 nr;
u8 fifo_nr;
u8 obj_num;
+ u8 obj_num_shift_to_u8;
u8 obj_size;
struct mcp251xfd_tx_obj obj[MCP251XFD_TX_OBJ_NUM_MAX];
@@ -859,17 +861,8 @@ static inline u8 mcp251xfd_get_tef_tail(const struct mcp251xfd_priv *priv)
return priv->tef->tail & (priv->tx->obj_num - 1);
}
-static inline u8 mcp251xfd_get_tef_len(const struct mcp251xfd_priv *priv)
-{
- return priv->tef->head - priv->tef->tail;
-}
-
-static inline u8 mcp251xfd_get_tef_linear_len(const struct mcp251xfd_priv *priv)
+static inline u8 mcp251xfd_get_tef_linear_len(const struct mcp251xfd_priv *priv, u8 len)
{
- u8 len;
-
- len = mcp251xfd_get_tef_len(priv);
-
return min_t(u8, len, priv->tx->obj_num - mcp251xfd_get_tef_tail(priv));
}
--
2.39.0
