diff options
| -rw-r--r-- | drivers/net/can/c_can/c_can.c | 52 |
1 files changed, 50 insertions, 2 deletions
diff --git a/drivers/net/can/c_can/c_can.c b/drivers/net/can/c_can/c_can.c index 38f9adaf15ac..cef9967eff93 100644 --- a/drivers/net/can/c_can/c_can.c +++ b/drivers/net/can/c_can/c_can.c @@ -817,6 +817,38 @@ static void c_can_do_tx(struct net_device *dev) } /* + * If we have a gap in the pending bits, that means we either + * raced with the hardware or failed to readout all upper + * objects in the last run due to quota limit. + */ +static u32 c_can_adjust_pending(u32 pend) +{ + u32 weight, lasts; + + if (pend == RECEIVE_OBJECT_BITS) + return pend; + + /* + * If the last set bit is larger than the number of pending + * bits we have a gap. + */ + weight = hweight32(pend); + lasts = fls(pend); + + /* If the bits are linear, nothing to do */ + if (lasts == weight) + return pend; + + /* + * Find the first set bit after the gap. We walk backwards + * from the last set bit. + */ + for (lasts--; pend & (1 << (lasts - 1)); lasts--); + + return pend & ~((1 << lasts) - 1); +} + +/* * theory of operation: * * c_can core saves a received CAN message into the first free message @@ -843,7 +875,7 @@ static int c_can_do_rx_poll(struct net_device *dev, int quota) u32 num_rx_pkts = 0; unsigned int msg_obj, msg_ctrl_save; struct c_can_priv *priv = netdev_priv(dev); - u16 val; + u32 val, pend = 0; /* * It is faster to read only one 16bit register. This is only possible @@ -852,7 +884,23 @@ static int c_can_do_rx_poll(struct net_device *dev, int quota) BUILD_BUG_ON_MSG(C_CAN_MSG_OBJ_RX_LAST > 16, "Implementation does not support more message objects than 16"); - while (quota > 0 && (val = priv->read_reg(priv, C_CAN_INTPND1_REG))) { + while (quota > 0) { + + if (!pend) { + pend = priv->read_reg(priv, C_CAN_INTPND1_REG); + if (!pend) + return num_rx_pkts; + /* + * If the pending field has a gap, handle the + * bits above the gap first. + */ + val = c_can_adjust_pending(pend); + } else { + val = pend; + } + /* Remove the bits from pend */ + pend &= ~val; + while ((msg_obj = ffs(val)) && quota > 0) { val &= ~BIT(msg_obj - 1); |