diff --git a/drivers/usbdev/rndis.c b/drivers/usbdev/rndis.c index 166b768b8de..2ea46ce47f0 100644 --- a/drivers/usbdev/rndis.c +++ b/drivers/usbdev/rndis.c @@ -98,7 +98,7 @@ #define RNDIS_MXDESCLEN (128) #define RNDIS_MAXSTRLEN (RNDIS_MXDESCLEN-2) #define RNDIS_CTRLREQ_LEN (256) -#define RNDIS_RESP_QUEUE_LEN (256) +#define RNDIS_RESP_QUEUE_WORDS (64) #define RNDIS_BUFFER_SIZE CONFIG_NET_ETH_PKTSIZE #define RNDIS_BUFFER_COUNT 4 @@ -174,8 +174,8 @@ struct rndis_dev_s uint32_t rndis_host_rx_count; /* RX packet counter */ uint8_t host_mac_address[6]; /* Host side MAC address */ - uint8_t response_queue[RNDIS_RESP_QUEUE_LEN]; - size_t response_queue_bytes; /* Count of bytes waiting in response_queue. */ + size_t response_queue_words; /* Count of words waiting in response_queue. */ + uint32_t response_queue[RNDIS_RESP_QUEUE_WORDS]; }; /* The internal version of the class driver */ @@ -1362,11 +1362,12 @@ static FAR void * rndis_prepare_response(FAR struct rndis_dev_s *priv, size_t size, FAR struct rndis_command_header *request_hdr) { - uint8_t *buf = priv->response_queue + priv->response_queue_bytes; + size_t size_words = size / sizeof(uint32_t); + uint32_t *buf = priv->response_queue + priv->response_queue_words; FAR struct rndis_response_header *hdr = (FAR struct rndis_response_header *)buf; - if (priv->response_queue_bytes + size > RNDIS_RESP_QUEUE_LEN) + if (priv->response_queue_words + size_words > RNDIS_RESP_QUEUE_WORDS) { uerr("RNDIS response queue full, dropping command %08x", (unsigned int)request_hdr->msgtype); @@ -1399,13 +1400,18 @@ rndis_prepare_response(FAR struct rndis_dev_s *priv, size_t size, static int rndis_send_encapsulated_response(FAR struct rndis_dev_s *priv, size_t size) { + size_t size_words = size / sizeof(uint32_t); FAR struct rndis_notification *notif = (FAR struct rndis_notification *)priv->epintin_req->buf; + /* RNDIS packets should always be multiple of 4 bytes in size */ + + DEBUGASSERT(size_words * sizeof(uint32_t) == size); + /* Mark the response as available in the queue */ - priv->response_queue_bytes += size; - DEBUGASSERT(priv->response_queue_bytes <= RNDIS_RESP_QUEUE_LEN); + priv->response_queue_words += size_words; + DEBUGASSERT(priv->response_queue_words <= RNDIS_RESP_QUEUE_WORDS); /* Send notification on IRQ endpoint, to tell host to read the data. */ @@ -1467,7 +1473,7 @@ static int rndis_handle_control_message(FAR struct rndis_dev_s *priv, case RNDIS_HALT_MSG: { - priv->response_queue_bytes = 0; + priv->response_queue_words = 0; priv->connected = false; } break; @@ -1560,6 +1566,13 @@ static int rndis_handle_control_message(FAR struct rndis_dev_s *priv, resp->hdr.msglen += resp->buflen; + /* Align to word boundary */ + + if ((resp->hdr.msglen & 3) != 0) + { + resp->hdr.msglen += 4 - (resp->hdr.msglen & 3); + } + rndis_send_encapsulated_response(priv, resp->hdr.msglen); } break; @@ -1615,7 +1628,7 @@ static int rndis_handle_control_message(FAR struct rndis_dev_s *priv, FAR struct rndis_reset_cmplt *resp; size_t respsize = sizeof(struct rndis_reset_cmplt); - priv->response_queue_bytes = 0; + priv->response_queue_words = 0; resp = rndis_prepare_response(priv, respsize, cmd_hdr); if (!resp) @@ -1794,20 +1807,22 @@ static void usbclass_ep0incomplete(FAR struct usbdev_ep_s *ep, * subtract remaining byte count. */ + size_t len_words = req->len / sizeof(uint32_t); + DEBUGASSERT(len_words * sizeof(uint32_t) == req->len); req->priv = 0; - if (req->len >= priv->response_queue_bytes) + if (len_words >= priv->response_queue_words) { /* Queue now empty */ - priv->response_queue_bytes = 0; + priv->response_queue_words = 0; } else { /* Copy the remaining responses to beginning of buffer. */ - priv->response_queue_bytes -= req->len; - memcpy(priv->response_queue, priv->response_queue + req->len, - priv->response_queue_bytes); + priv->response_queue_words -= len_words; + memcpy(priv->response_queue, priv->response_queue + len_words, + priv->response_queue_words * sizeof(uint32_t)); } } } @@ -2258,7 +2273,7 @@ static int usbclass_bind(FAR struct usbdevclass_driver_s *driver, /* Initialize response queue to empty */ - priv->response_queue_bytes = 0; + priv->response_queue_words = 0; /* Report if we are selfpowered */ @@ -2541,7 +2556,7 @@ static int usbclass_setup(FAR struct usbdevclass_driver_s *driver, } else if (ctrl->req == RNDIS_GET_ENCAPSULATED_RESPONSE) { - if (priv->response_queue_bytes == 0) + if (priv->response_queue_words == 0) { /* No reply available is indicated with a single * 0x00 byte.