Signed-off-by: Gerhard Uttenthaler <uttenthaler@xxxxxxxxxxxxxxxx> --- drivers/net/can/usb/ems_usb.c | 83 ++++++++++++++++++----------------- 1 file changed, 42 insertions(+), 41 deletions(-) diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c index c664af4499a1..6a9ea6a4e687 100644 --- a/drivers/net/can/usb/ems_usb.c +++ b/drivers/net/can/usb/ems_usb.c @@ -460,6 +460,7 @@ static void ems_usb_read_bulk_callback(struct urb *urb) struct ems_usb *dev = urb->context; struct net_device *netdev; int retval; + u32 length, start; netdev = dev->netdev; @@ -478,50 +479,50 @@ static void ems_usb_read_bulk_callback(struct urb *urb) goto resubmit_urb; } - if (urb->actual_length > CPC_HEADER_SIZE) { + length = urb->actual_length; + start = CPC_HEADER_SIZE; + + while (length >= CPC_MSG_HEADER_LEN) { struct ems_cpc_msg *msg; u8 *ibuf = urb->transfer_buffer; - u8 msg_count, start; - - msg_count = ibuf[0] & ~0x80; - - start = CPC_HEADER_SIZE; - - while (msg_count) { - msg = (struct ems_cpc_msg *)&ibuf[start]; - - switch (msg->type) { - case CPC_MSG_TYPE_CAN_STATE: - /* Process CAN state changes */ - ems_usb_rx_err(dev, msg); - break; - - case CPC_MSG_TYPE_CAN_FRAME: - case CPC_MSG_TYPE_EXT_CAN_FRAME: - case CPC_MSG_TYPE_RTR_FRAME: - case CPC_MSG_TYPE_EXT_RTR_FRAME: - ems_usb_rx_can_msg(dev, msg); - break; - - case CPC_MSG_TYPE_CAN_FRAME_ERROR: - /* Process errorframe */ - ems_usb_rx_err(dev, msg); - break; - - case CPC_MSG_TYPE_OVERRUN: - /* Message lost while receiving */ - ems_usb_rx_err(dev, msg); - break; - } - - start += CPC_MSG_HEADER_LEN + msg->length; - msg_count--; - - if (start > urb->transfer_buffer_length) { - netdev_err(netdev, "format error\n"); - break; - } + u32 read_count; + + msg = (struct ems_cpc_msg *)&ibuf[start]; + + switch (msg->type) { + case CPC_MSG_TYPE_CAN_STATE: + /* Process CAN state changes */ + ems_usb_rx_err(dev, msg); + break; + + case CPC_MSG_TYPE_CAN_FRAME: + case CPC_MSG_TYPE_EXT_CAN_FRAME: + case CPC_MSG_TYPE_RTR_FRAME: + case CPC_MSG_TYPE_EXT_RTR_FRAME: + ems_usb_rx_can_msg(dev, msg); + break; + + case CPC_MSG_TYPE_CAN_FRAME_ERROR: + /* Process errorframe */ + ems_usb_rx_err(dev, msg); + break; + + case CPC_MSG_TYPE_OVERRUN: + /* Message lost while receiving */ + ems_usb_rx_err(dev, msg); + break; } + + read_count = CPC_MSG_HEADER_LEN + msg->length; + start += read_count; + + if (start > urb->transfer_buffer_length) { + netdev_err(netdev, "format error\n"); + break; + } + + if (read_count <= length) + length -= read_count; } resubmit_urb: -- 2.26.2 -- EMS Dr. Thomas Wuensche e.K. Sonnenhang 3 85304 Ilmmuenster HR Ingolstadt, HRA 170106 Phone: +49-8441-490260 Fax : +49-8441-81860 http://www.ems-wuensche.com