diff --git a/drivers/usbd/hcd/hcd.c b/drivers/usbd/hcd/hcd.c
index ccdd53325..234cc48e4 100755
--- a/drivers/usbd/hcd/hcd.c
+++ b/drivers/usbd/hcd/hcd.c
@@ -27,7 +27,7 @@ static int hcd_enumerate(hcd_device_state *);
 static int hcd_get_device_descriptor(hcd_device_state *);
 static int hcd_set_address(hcd_device_state *, int);
 static int hcd_get_descriptor_tree(hcd_device_state *);
-static int hcd_set_configuration(hcd_device_state *, int);
+static int hcd_set_configuration(hcd_device_state *, hcd_reg1);
 static int hcd_handle_urb(hcd_device_state *);
 static int hcd_control_urb(hcd_device_state *);
 static int hcd_non_control_urb(hcd_device_state *, int);
@@ -196,6 +196,12 @@ hcd_enumerate(hcd_device_state * this_device)
 		return EXIT_FAILURE;
 	}
 
+	/* Default MaxPacketSize, based on speed */
+	if (HCD_SPEED_LOW == this_device->speed)
+		this_device->max_packet_size = HCD_LS_MAXPACKETSIZE;
+	else
+		this_device->max_packet_size = HCD_HS_MAXPACKETSIZE;
+
 	/* Get device descriptor */
 	if (EXIT_SUCCESS != hcd_get_device_descriptor(this_device)) {
 		USB_MSG("Failed to get device descriptor");
@@ -415,7 +421,7 @@ hcd_get_descriptor_tree(hcd_device_state * this_device)
  *    hcd_set_configuration                                                  *
  *===========================================================================*/
 static int
-hcd_set_configuration(hcd_device_state * this_device, int configuration)
+hcd_set_configuration(hcd_device_state * this_device, hcd_reg1 configuration)
 {
 	hcd_ctrlrequest setup;
 
@@ -454,7 +460,7 @@ hcd_handle_urb(hcd_device_state * this_device)
 
 	USB_ASSERT(NULL != urb, "NULL URB given");
 	/* TODO: One device only */
-	USB_ASSERT((void *)this_device != (void *)urb->dev,
+	USB_ASSERT((void *)this_device == (void *)urb->dev,
 		"Unknown device for URB");
 
 	switch (urb->type) {
@@ -536,7 +542,12 @@ hcd_control_urb(hcd_device_state * this_device)
 		return EXIT_FAILURE;
 	}
 
+	/* TODO: Calling memcpy may be removed when writing directly to URB */
+	/* Put what was read back into URB */
+	memcpy(urb->data, this_device->buffer, this_device->data_len);
+	urb->actual_length = (unsigned int)this_device->data_len;
 	urb->status = EXIT_SUCCESS;
+
 	return EXIT_SUCCESS;
 }
 
@@ -563,8 +574,8 @@ hcd_non_control_urb(hcd_device_state * this_device, int type)
 		return EXIT_FAILURE;
 	}
 
-	if ((UE_GET_ADDR(urb->endpoint) >= 16) ||
-		(UE_GET_ADDR(urb->endpoint) <= 0)) {
+	if ((UE_GET_ADDR(urb->endpoint) >= HCD_TOTAL_EP) ||
+		(UE_GET_ADDR(urb->endpoint) <= HCD_DEFAULT_EP)) {
 		USB_MSG("Illegal EP number");
 		return EXIT_FAILURE;
 	}
@@ -594,7 +605,7 @@ hcd_non_control_urb(hcd_device_state * this_device, int type)
 	/* Assign to data request structure */
 	request.endpoint = urb->endpoint;
 	request.direction = urb->direction;
-	request.size = (int)urb->size;
+	request.data_left = (int)urb->size;
 	request.data = urb->data;
 	request.interval = urb->interval;
 
@@ -633,7 +644,9 @@ hcd_non_control_urb(hcd_device_state * this_device, int type)
 	}
 
 	/* Transfer successfully completed */
+	urb->actual_length = urb->size - request.data_left;
 	urb->status = EXIT_SUCCESS;
+
 	return EXIT_SUCCESS;
 }
 
@@ -646,15 +659,18 @@ hcd_setup_packet(hcd_device_state * this_device, hcd_ctrlrequest * setup)
 {
 	hcd_driver_state * d;
 	hcd_reg1 * current_byte;
-	int expected_len;
-	int received_len;
+	int rx_len;
 
 	DEBUG_DUMP;
 
+	/* Should have been set at enumeration or with default values */
+	USB_ASSERT(this_device->max_packet_size > 0,
+		"Illegal MaxPacketSize for EP0");
+
 	/* Initially... */
 	d = this_device->driver;
-	expected_len = (int)setup->wLength;
-	current_byte = this_device->buffer;
+	current_byte = this_device->buffer;	/* Start reading into this */
+	this_device->data_len = 0;		/* Nothing read yet */
 
 	/* Send setup packet */
 	d->setup_stage(d->private_data, setup);
@@ -669,16 +685,14 @@ hcd_setup_packet(hcd_device_state * this_device, hcd_ctrlrequest * setup)
 		return EXIT_FAILURE;
 
 	/* For data packets... */
-	if (expected_len > 0) {
+	if (setup->wLength > 0) {
 
 		/* TODO: magic number */
 		/* ...IN data packets */
 		if (setup->bRequestType & 0x80) {
 
-			/* What was received until now */
-			this_device->data_len = 0;
+			for(;;) {
 
-			do {
 				/* Try getting data */
 				d->in_data_stage(d->private_data);
 
@@ -695,24 +709,30 @@ hcd_setup_packet(hcd_device_state * this_device, hcd_ctrlrequest * setup)
 					return EXIT_FAILURE;
 
 				/* Read data received as response */
-				received_len = d->read_data(d->private_data,
-							current_byte, 0);
+				rx_len = d->read_data(d->private_data,
+						current_byte, HCD_DEFAULT_EP);
 
-				/* Data reading should always yield positive
-				 * results for proper setup packet */
-				if (received_len > 0) {
-					/* Try next packet */
-					this_device->data_len += received_len;
-					current_byte += received_len;
-				} else
-					return EXIT_FAILURE;
+				/* Increment */
+				current_byte += rx_len;
+				this_device->data_len += rx_len;
 
-			} while (expected_len > this_device->data_len);
+				/* If full max sized packet was read... */
+				if (rx_len == (int)this_device->max_packet_size)
+					/* ...try reading next packet even if
+					 * zero bytes may be received */
+					continue;
 
-			/* Should be exactly what we requested, no more */
-			if (this_device->data_len != expected_len) {
-				USB_MSG("Received more data than expected");
-				return EXIT_FAILURE;
+				/* If less than max data was read... */
+				if (rx_len < (int)this_device->max_packet_size)
+					/* ...it must have been
+					 * the last packet */
+					break;
+
+				/* Unreachable during normal operation */
+				USB_MSG("rx_len: %d; max_packet_size: %d",
+					rx_len, this_device->max_packet_size);
+				USB_ASSERT(0, "Illegal state of data "
+					"receive operation");
 			}
 
 		} else {
@@ -755,7 +775,7 @@ hcd_setup_packet(hcd_device_state * this_device, hcd_ctrlrequest * setup)
 			return EXIT_FAILURE;
 
 		/* Read zero data from response to clear registers */
-		if (0 != d->read_data(d->private_data, NULL, 0))
+		if (0 != d->read_data(d->private_data, NULL, HCD_DEFAULT_EP))
 			return EXIT_FAILURE;
 	}
 
@@ -775,13 +795,18 @@ hcd_data_transfer(hcd_device_state * this_device, hcd_datarequest * request)
 
 	DEBUG_DUMP;
 
+	USB_ASSERT((hcd_reg1)(UE_GET_ADDR(request->endpoint)) <= HCD_LAST_EP,
+		"Invalid EP number");
+	USB_ASSERT((hcd_reg1)(this_device->address) <= HCD_LAST_ADDR,
+		"Invalid device address");
+
 	/* Initially... */
 	d = this_device->driver;
 
 	/* Set parameters for further communication */
 	d->setup_device(d->private_data,
-			request->endpoint,
-			this_device->address);
+			(hcd_reg1)request->endpoint,
+			(hcd_reg1)this_device->address);
 
 	/* TODO: broken USB_IN... constants */
 	if (1 == request->direction) {
@@ -803,46 +828,34 @@ hcd_data_transfer(hcd_device_state * this_device, hcd_datarequest * request)
 			/* Read data received as response */
 			transfer_len = d->read_data(d->private_data,
 						(hcd_reg1 *)request->data,
-						request->endpoint);
+						(hcd_reg1)request->endpoint);
 
-			request->size -= transfer_len;
+			request->data_left -= transfer_len;
 			request->data += transfer_len;
 
 			/* Total length shall not become negative */
-			if (request->size < 0) {
+			if (request->data_left < 0) {
 				USB_MSG("Invalid amount of data received");
 				return EXIT_FAILURE;
 			}
 
-#ifdef DEBUG
-			/* TODO: REMOVEME (dumping of data transfer) */
-			{
-				int i;
-				USB_MSG("RECEIVED: %d", transfer_len);
-				for (i = 0; i < transfer_len; i++)
-					USB_MSG("0x%02X: %c",
-					(request->data-transfer_len)[i],
-					(request->data-transfer_len)[i]);
-			}
-#endif
-
-		} while (0 != request->size);
+		} while (0 != request->data_left);
 
 	} else if (0 == request->direction) {
 
 		do {
 			temp_req = *request;
 
-			/* Decide transfer size */
-			if (temp_req.size > (int)temp_req.max_packet_size) {
-				temp_req.size = temp_req.max_packet_size;
-			}
+			/* Decide temporary transfer size */
+			if (temp_req.data_left > (int)temp_req.max_packet_size)
+				temp_req.data_left = temp_req.max_packet_size;
 
-			request->data += temp_req.size;
-			request->size -= temp_req.size;
+			/* Alter actual transfer size */
+			request->data += temp_req.data_left;
+			request->data_left -= temp_req.data_left;
 
 			/* Total length shall not become negative */
-			USB_ASSERT(request->size >= 0,
+			USB_ASSERT(request->data_left >= 0,
 				"Invalid amount of transfer data calculated");
 
 			/* Start actual data transfer */
@@ -858,7 +871,7 @@ hcd_data_transfer(hcd_device_state * this_device, hcd_datarequest * request)
 							HCD_DIRECTION_OUT))
 				return EXIT_FAILURE;
 
-		} while (0 != request->size);
+		} while (0 != request->data_left);
 
 	} else
 		USB_ASSERT(0, "Invalid transfer direction");
diff --git a/drivers/usbd/hcd/hcd_common.c b/drivers/usbd/hcd/hcd_common.c
index d3728c0f9..305a2f191 100755
--- a/drivers/usbd/hcd/hcd_common.c
+++ b/drivers/usbd/hcd/hcd_common.c
@@ -23,7 +23,7 @@
  *===========================================================================*/
 static int hcd_fill_configuration(hcd_reg1 *, int, hcd_configuration *, int);
 static int hcd_fill_interface(hcd_reg1 *, int, hcd_interface *, int);
-static int hcd_fill_endpoint(hcd_reg1 *, int, hcd_endpoint *, int);
+static int hcd_fill_endpoint(hcd_reg1 *, int, hcd_endpoint *);
 
 
 /*===========================================================================*
@@ -343,10 +343,9 @@ hcd_buffer_to_tree(hcd_reg1 * buf, int len, hcd_configuration * c)
 			if (NULL == i)
 				goto PARSE_ERROR;
 
-			e = &(i->endpoint[ep_num]);
+			e = &(i->endpoint[ep_num++]);
 
-			if (EXIT_SUCCESS != hcd_fill_endpoint(buf, len,
-								e, ep_num++))
+			if (EXIT_SUCCESS != hcd_fill_endpoint(buf, len, e))
 				goto PARSE_ERROR;
 		} else
 			USB_DBG("Unhandled descriptor type 0x%02X",
@@ -507,6 +506,7 @@ hcd_fill_interface(hcd_reg1 * buf, int len, hcd_interface * i, int num)
 	if (sizeof(*desc) != desc->bLength)
 		return EXIT_FAILURE;
 
+	/* It is mandatory to supply interfaces in correct order */
 	if (desc->bInterfaceNumber != num)
 		return EXIT_FAILURE;
 
@@ -541,7 +541,7 @@ hcd_fill_interface(hcd_reg1 * buf, int len, hcd_interface * i, int num)
  *    hcd_fill_endpoint                                                      *
  *===========================================================================*/
 static int
-hcd_fill_endpoint(hcd_reg1 * buf, int len, hcd_endpoint * e, int num)
+hcd_fill_endpoint(hcd_reg1 * buf, int len, hcd_endpoint * e)
 {
 	hcd_endpoint_descriptor * desc;
 
@@ -549,7 +549,7 @@ hcd_fill_endpoint(hcd_reg1 * buf, int len, hcd_endpoint * e, int num)
 
 	desc = (hcd_endpoint_descriptor *)buf;
 
-	USB_DBG("Endpoint #%d", num);
+	USB_DBG("Endpoint #%d", UE_GET_ADDR(desc->bEndpointAddress));
 
 	if (UDESC_ENDPOINT != desc->bDescriptorType)
 		return EXIT_FAILURE;
diff --git a/drivers/usbd/hcd/musb/musb_am335x.c b/drivers/usbd/hcd/musb/musb_am335x.c
index 411b76b12..61a379318 100755
--- a/drivers/usbd/hcd/musb/musb_am335x.c
+++ b/drivers/usbd/hcd/musb/musb_am335x.c
@@ -631,7 +631,7 @@ musb_am335x_usbx_isr(void * data)
 static int
 musb_am335x_irqstat0_to_ep(int irqstat0)
 {
-	int ep;
+	hcd_reg1 ep;
 
 	DEBUG_DUMP;
 
@@ -640,16 +640,20 @@ musb_am335x_irqstat0_to_ep(int irqstat0)
 	while (0 == (irqstat0 & 0x01)) {
 		irqstat0 >>= 1;
 		ep++;
-		USB_ASSERT(ep < 32, "Invalid IRQSTAT0 supplied (1)");
+		/* Must be within two consecutive EP sets */
+		USB_ASSERT(ep < (2 * HCD_TOTAL_EP),
+			"Invalid IRQSTAT0 supplied (1)");
 	}
 
 	/* Convert RX interrupt to EP number */
-	if (ep >= 16) {
-		ep -= 16;
-		USB_ASSERT(ep != 0, "Invalid IRQSTAT0 supplied (2)");
+	if (ep >= HCD_TOTAL_EP) {
+		ep -= HCD_TOTAL_EP;
+		/* Must not be control EP */
+		USB_ASSERT(ep != HCD_DEFAULT_EP,
+			"Invalid IRQSTAT0 supplied (2)");
 	}
 
-	return ep;
+	return (int)ep;
 }
 
 
diff --git a/drivers/usbd/hcd/musb/musb_core.c b/drivers/usbd/hcd/musb/musb_core.c
index 3a29a8043..eb895d6c3 100755
--- a/drivers/usbd/hcd/musb/musb_core.c
+++ b/drivers/usbd/hcd/musb/musb_core.c
@@ -16,13 +16,13 @@
  *    Local prototypes                                                       *
  *===========================================================================*/
 static void musb_set_state(musb_core_config *);
-static int musb_check_rxpktrdy(void *, int);
-static void musb_in_stage_cleanup(void *, int);
-static void musb_clear_rxpktrdy(void *, int);
+static int musb_check_rxpktrdy(void *, hcd_reg1);
+static void musb_in_stage_cleanup(void *, hcd_reg1);
+static void musb_clear_rxpktrdy(void *, hcd_reg1);
 static void musb_clear_statuspkt(void *);
 static int musb_get_count(void *);
-static void musb_read_fifo(void *, void *, int, int);
-static void musb_write_fifo(void *, void *, int, int);
+static void musb_read_fifo(void *, void *, int, hcd_reg1);
+static void musb_write_fifo(void *, void *, int, hcd_reg1);
 
 
 /*===========================================================================*
@@ -43,8 +43,8 @@ musb_set_state(musb_core_config * cfg)
 
 	r = cfg->regs;
 
-	USB_ASSERT(cfg->ep <= 15, "Invalid EP supplied");
-	USB_ASSERT(cfg->addr <= 127, "Invalid device address supplied");
+	USB_ASSERT(cfg->ep <= HCD_LAST_EP, "Invalid EP supplied");
+	USB_ASSERT(cfg->addr <= HCD_LAST_ADDR, "Invalid address supplied");
 
 	/* Set EP and address to be used in next MUSB command */
 
@@ -62,7 +62,7 @@ musb_set_state(musb_core_config * cfg)
  *    musb_check_rxpktrdy                                                    *
  *===========================================================================*/
 static int
-musb_check_rxpktrdy(void * cfg, int ep_num)
+musb_check_rxpktrdy(void * cfg, hcd_reg1 ep_num)
 {
 	void * r;
 
@@ -74,7 +74,7 @@ musb_check_rxpktrdy(void * cfg, int ep_num)
 	musb_set_state((musb_core_config *)cfg);
 
 	/* Check for RXPKTRDY */
-	if (0 == ep_num) {
+	if (HCD_DEFAULT_EP == ep_num) {
 		/* Get control status register for EP 0 */
 		if (HCD_RD2(r, MUSB_REG_HOST_CSR0) &
 			MUSB_VAL_HOST_CSR0_RXPKTRDY)
@@ -94,14 +94,14 @@ musb_check_rxpktrdy(void * cfg, int ep_num)
  *    musb_in_stage_cleanup                                                  *
  *===========================================================================*/
 static void
-musb_in_stage_cleanup(void * cfg, int ep_num)
+musb_in_stage_cleanup(void * cfg, hcd_reg1 ep_num)
 {
 	DEBUG_DUMP;
 
 	musb_clear_rxpktrdy(cfg, ep_num);
 
 	/* For control EP 0 also clear STATUSPKT */
-	if (0 == ep_num)
+	if (HCD_DEFAULT_EP == ep_num)
 		musb_clear_statuspkt(cfg);
 }
 
@@ -110,7 +110,7 @@ musb_in_stage_cleanup(void * cfg, int ep_num)
  *    musb_clear_rxpktrdy                                                    *
  *===========================================================================*/
 static void
-musb_clear_rxpktrdy(void * cfg, int ep_num)
+musb_clear_rxpktrdy(void * cfg, hcd_reg1 ep_num)
 {
 	void * r;
 	hcd_reg2 host_csr;
@@ -123,7 +123,7 @@ musb_clear_rxpktrdy(void * cfg, int ep_num)
 	musb_set_state((musb_core_config *)cfg);
 
 	/* Check for RXPKTRDY */
-	if (0 == ep_num) {
+	if (HCD_DEFAULT_EP == ep_num) {
 		/* Get control status register for EP 0 */
 		host_csr = HCD_RD2(r, MUSB_REG_HOST_CSR0);
 
@@ -191,7 +191,7 @@ musb_get_count(void * cfg)
  *    musb_read_fifo                                                         *
  *===========================================================================*/
 static void
-musb_read_fifo(void * cfg, void * output, int size, int fifo_num)
+musb_read_fifo(void * cfg, void * output, int size, hcd_reg1 fifo_num)
 {
 	void * r;
 
@@ -202,7 +202,7 @@ musb_read_fifo(void * cfg, void * output, int size, int fifo_num)
 
 	DEBUG_DUMP;
 
-	USB_ASSERT((fifo_num >= 0) && (fifo_num <= 15), "Wrong FIFO number");
+	USB_ASSERT(fifo_num <= HCD_LAST_EP, "Invalid FIFO number");
 
 	r = ((musb_core_config *)cfg)->regs;
 	fifo_addr = MUSB_REG_FIFO0 + (fifo_num * MUSB_REG_FIFO_LEN);
@@ -216,7 +216,7 @@ musb_read_fifo(void * cfg, void * output, int size, int fifo_num)
 	output_w = (hcd_reg4 *)output;
 
 	/* Try and copy aligned words */
-	if (0 == ((hcd_addr)output_w % 4)) {
+	if (0 == ((hcd_addr)output_w % sizeof(hcd_addr))) {
 
 		while (size > (int)(sizeof(*output_w) - 1)) {
 			*output_w++ = HCD_RD4(r, fifo_addr);
@@ -238,7 +238,7 @@ musb_read_fifo(void * cfg, void * output, int size, int fifo_num)
  *    musb_write_fifo                                                         *
  *===========================================================================*/
 static void
-musb_write_fifo(void * cfg, void * input, int size, int fifo_num)
+musb_write_fifo(void * cfg, void * input, int size, hcd_reg1 fifo_num)
 {
 	void * r;
 
@@ -249,7 +249,7 @@ musb_write_fifo(void * cfg, void * input, int size, int fifo_num)
 
 	DEBUG_DUMP;
 
-	USB_ASSERT((fifo_num >= 0) && (fifo_num <= 15), "Wrong FIFO number");
+	USB_ASSERT(fifo_num <= HCD_LAST_EP, "Invalid FIFO number");
 
 	r = ((musb_core_config *)cfg)->regs;
 	fifo_addr = MUSB_REG_FIFO0 + (fifo_num * MUSB_REG_FIFO_LEN);
@@ -263,7 +263,7 @@ musb_write_fifo(void * cfg, void * input, int size, int fifo_num)
 	input_w = (hcd_reg4 *)input;
 
 	/* Try and copy aligned words */
-	if (0 == ((hcd_addr)input_w % 4)) {
+	if (0 == ((hcd_addr)input_w % sizeof(hcd_addr))) {
 
 		while (size > (int)(sizeof(*input_w) - 1)) {
 			HCD_WR4(r, fifo_addr, *input_w++);
@@ -461,9 +461,6 @@ void
 musb_rx_stage(void * cfg, hcd_datarequest * request)
 {
 	musb_core_config * core;
-#if 0
-	hcd_reg2 intrrxe;
-#endif
 	hcd_reg2 host_rxcsr;
 	hcd_reg1 host_rxtype;
 	void * r;
@@ -475,7 +472,7 @@ musb_rx_stage(void * cfg, hcd_datarequest * request)
 
 	USB_ASSERT(request->max_packet_size <= 1024,
 			"Invalid wMaxPacketSize");
-	USB_ASSERT((core->ep <= 15) && (core->ep > 0),
+	USB_ASSERT((core->ep <= HCD_LAST_EP) && (core->ep > HCD_DEFAULT_EP),
 			"Invalid bulk EP supplied");
 
 	/* Set EP and device address to be used in this command */
@@ -513,12 +510,17 @@ musb_rx_stage(void * cfg, hcd_datarequest * request)
 	else if (HCD_TRANSFER_INTERRUPT == request->type)
 		HCD_WR1(r, MUSB_REG_HOST_RXINTERVAL, request->interval);
 
-	/* Not required in some MUSB implementations */
 #if 0
-	/* Enable this interrupt */
-	intrrxe = HCD_RD2(r, MUSB_REG_INTRRXE);
-	HCD_SET(intrrxe, HCD_BIT(core->ep));
-	HCD_WR2(r, MUSB_REG_INTRRXE, intrrxe);
+	{
+		/* Not required by all MUSB implementations, but
+		 * left here just in case */
+		hcd_reg2 intrrxe;
+
+		/* Enable this interrupt */
+		intrrxe = HCD_RD2(r, MUSB_REG_INTRRXE);
+		HCD_SET(intrrxe, HCD_BIT(core->ep));
+		HCD_WR2(r, MUSB_REG_INTRRXE, intrrxe);
+	}
 #endif
 
 	/* TODO: One reusable FIFO, no double buffering */
@@ -529,21 +531,15 @@ musb_rx_stage(void * cfg, hcd_datarequest * request)
 	HCD_WR2(r, MUSB_REG_RXFIFOADDR, MUSB_VAL_XXFIFOADDR_EP0_END);
 	HCD_WR1(r, MUSB_REG_RXFIFOSZ, MUSB_VAL_XXFIFOSZ_4096);
 
-	/* TODO: decide which is better (or working at all when we use more
-	 * than one transfer for bulk data in single device) */
-#if 0
 	/* Make controller reconfigure */
 	host_rxcsr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
-	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_CLRDATATOG);
+	if (MUSB_DATATOG_UNKNOWN == core->datatog_rx[core->ep]) {
+		/* Reset DATA toggle on first transfer */
+		HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_CLRDATATOG);
+		core->datatog_rx[core->ep] = MUSB_DATATOG_INIT;
+	}
 	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_FLUSHFIFO);
 	HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_rxcsr);
-#else
-	/* Reset and flush */
-	host_rxcsr = 0;
-	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_CLRDATATOG);
-	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_FLUSHFIFO);
-	HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_rxcsr);
-#endif
 
 	/* Request packet */
 	host_rxcsr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
@@ -559,9 +555,6 @@ void
 musb_tx_stage(void * cfg, hcd_datarequest * request)
 {
 	musb_core_config * core;
-#if 0
-	hcd_reg2 intrtxe;
-#endif
 	hcd_reg2 host_txcsr;
 	hcd_reg1 host_txtype;
 	void * r;
@@ -573,7 +566,7 @@ musb_tx_stage(void * cfg, hcd_datarequest * request)
 
 	USB_ASSERT(request->max_packet_size <= 1024,
 			"Invalid wMaxPacketSize");
-	USB_ASSERT((core->ep <= 15) && (core->ep > 0),
+	USB_ASSERT((core->ep <= HCD_LAST_EP) && (core->ep > HCD_DEFAULT_EP),
 			"Invalid bulk EP supplied");
 
 	/* Set EP and device address to be used in this command */
@@ -611,12 +604,17 @@ musb_tx_stage(void * cfg, hcd_datarequest * request)
 	else if (HCD_TRANSFER_INTERRUPT == request->type)
 		HCD_WR1(r, MUSB_REG_HOST_TXINTERVAL, request->interval);
 
-	/* Not required in some MUSB implementations */
 #if 0
-	/* Enable this interrupt */
-	intrtxe = HCD_RD2(r, MUSB_REG_INTRTXE);
-	HCD_SET(intrtxe, HCD_BIT(core->ep));
-	HCD_WR2(r, MUSB_REG_INTRTXE, intrtxe);
+	{
+		/* Not required by all MUSB implementations, but
+		 * left here just in case */
+		hcd_reg2 intrtxe;
+
+		/* Enable this interrupt */
+		intrtxe = HCD_RD2(r, MUSB_REG_INTRTXE);
+		HCD_SET(intrtxe, HCD_BIT(core->ep));
+		HCD_WR2(r, MUSB_REG_INTRTXE, intrtxe);
+	}
 #endif
 
 	/* TODO: One reusable FIFO, no double buffering */
@@ -627,9 +625,6 @@ musb_tx_stage(void * cfg, hcd_datarequest * request)
 	HCD_WR2(r, MUSB_REG_TXFIFOADDR, MUSB_VAL_XXFIFOADDR_EP0_END);
 	HCD_WR1(r, MUSB_REG_TXFIFOSZ, MUSB_VAL_XXFIFOSZ_4096);
 
-	/* TODO: decide which is better (or working at all when we use more
-	 * than one transfer for bulk data in single device) */
-#if 0
 	/* Make controller reconfigure */
 	host_txcsr = HCD_RD2(r, MUSB_REG_HOST_TXCSR);
 	HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_DMAMODE);
@@ -638,20 +633,16 @@ musb_tx_stage(void * cfg, hcd_datarequest * request)
 	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_MODE);
 	HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_ISO);
 	HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_AUTOSET);
-	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_CLRDATATOG);
+	if (MUSB_DATATOG_UNKNOWN == core->datatog_tx[core->ep]) {
+		/* Reset DATA toggle on first transfer */
+		HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_CLRDATATOG);
+		core->datatog_tx[core->ep] = MUSB_DATATOG_INIT;
+	}
 	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_FLUSHFIFO);
 	HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_txcsr);
-#else
-	/* Reset and flush */
-	host_txcsr = 0;
-	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_MODE);
-	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_CLRDATATOG);
-	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_FLUSHFIFO);
-	HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_txcsr);
-#endif
 
 	/* Put data in FIFO */
-	musb_write_fifo(cfg, request->data, request->size, core->ep);
+	musb_write_fifo(cfg, request->data, request->data_left, core->ep);
 
 	/* Request packet */
 	host_txcsr = HCD_RD2(r, MUSB_REG_HOST_TXCSR);
@@ -760,7 +751,7 @@ musb_out_status_stage(void * cfg)
  *    musb_read_data                                                         *
  *===========================================================================*/
 int
-musb_read_data(void * cfg, hcd_reg1 * buffer, int ep_num)
+musb_read_data(void * cfg, hcd_reg1 * buffer, hcd_reg1 ep_num)
 {
 	int count;
 
diff --git a/drivers/usbd/hcd/musb/musb_core.h b/drivers/usbd/hcd/musb/musb_core.h
index e60713f9e..899f6eea9 100755
--- a/drivers/usbd/hcd/musb/musb_core.h
+++ b/drivers/usbd/hcd/musb/musb_core.h
@@ -11,6 +11,15 @@
 /*===========================================================================*
  *    Types and constants                                                    *
  *===========================================================================*/
+/* Holds info on DATA toggle (DATA0/DATA1) initialization,
+ * required by bulk transfers */
+typedef enum {
+
+	MUSB_DATATOG_UNKNOWN = 0,	/* Default with memset 0 */
+	MUSB_DATATOG_INIT
+}
+musb_datatog;
+
 /* Structure to hold Mentor USB core configuration
  * May be more than one on a single chip
  * Should be initialized by MUSB's variant specific code (like AM335x) */
@@ -19,6 +28,8 @@ typedef struct {
 	void * regs;	/* Points to beginning of memory mapped registers */
 	hcd_reg1 ep;	/* Currently used endpoint */
 	hcd_reg1 addr;	/* Currently used address */
+	musb_datatog datatog_tx[HCD_TOTAL_EP];
+	musb_datatog datatog_rx[HCD_TOTAL_EP];
 }
 musb_core_config;
 
@@ -41,7 +52,7 @@ void musb_in_data_stage(void *);
 void musb_out_data_stage(void *);
 void musb_in_status_stage(void *);
 void musb_out_status_stage(void *);
-int musb_read_data(void *, hcd_reg1 *, int);
+int musb_read_data(void *, hcd_reg1 *, hcd_reg1);
 int musb_check_error(void *, hcd_transfer, hcd_direction);
 
 
diff --git a/drivers/usbd/include/usb/hcd_common.h b/drivers/usbd/include/usb/hcd_common.h
index f8a38b498..7b127b151 100755
--- a/drivers/usbd/include/usb/hcd_common.h
+++ b/drivers/usbd/include/usb/hcd_common.h
@@ -194,7 +194,7 @@ hcd_event;
 struct hcd_datarequest {
 
 	char * data;
-	int size;
+	int data_left;
 	int endpoint;
 	int direction;
 	unsigned int max_packet_size;
@@ -218,9 +218,12 @@ typedef struct hcd_datarequest		hcd_datarequest;
 #define HCD_NANOSLEEP_USEC(usec)	((usec) * HCD_MILI)
 
 /* Default USB communication parameters */
-#define HCD_DEFAULT_EP		0x00
-#define HCD_DEFAULT_ADDR	0x00
-#define HCD_DEFAULT_CONFIG	0x00
+#define HCD_DEFAULT_EP		0x00u
+#define HCD_DEFAULT_ADDR	0x00u
+#define HCD_DEFAULT_CONFIG	0x00u
+#define HCD_LAST_ADDR		0x7Fu
+#define HCD_LAST_EP		0x0Fu
+#define HCD_TOTAL_EP		0x10u
 
 /* TODO: One device only */
 #define HCD_ATTACHED_ADDR	0x01
@@ -228,6 +231,10 @@ typedef struct hcd_datarequest		hcd_datarequest;
 /* Translates configuration number for 'set configuration' */
 #define HCD_SET_CONFIG_NUM(num)	((num)+0x01u)
 
+/* Default MaxPacketSize for control transfer */
+#define HCD_LS_MAXPACKETSIZE	8u
+#define HCD_HS_MAXPACKETSIZE	64u
+
 
 /*===========================================================================*
  *    Operating system specific                                              *
diff --git a/drivers/usbd/include/usb/hcd_interface.h b/drivers/usbd/include/usb/hcd_interface.h
index 3d20f1c4f..d8a4170a5 100755
--- a/drivers/usbd/include/usb/hcd_interface.h
+++ b/drivers/usbd/include/usb/hcd_interface.h
@@ -32,7 +32,7 @@ struct hcd_driver_state {
 	void	(*out_data_stage)	(void *);
 	void	(*in_status_stage)	(void *);
 	void	(*out_status_stage)	(void *);
-	int	(*read_data)		(void *, hcd_reg1 *, int);
+	int	(*read_data)		(void *, hcd_reg1 *, hcd_reg1);
 	int	(*check_error)		(void *, hcd_transfer, hcd_direction);
 
 	/* Controller's private data (like mapped registers) */
diff --git a/drivers/usbd/include/usb/usb_common.h b/drivers/usbd/include/usb/usb_common.h
index f4f82f2c5..803699ec7 100755
--- a/drivers/usbd/include/usb/usb_common.h
+++ b/drivers/usbd/include/usb/usb_common.h
@@ -11,8 +11,8 @@
 /* Current printf implementation for dumping important messages */
 #include <stdio.h>
 
-#if 1
-/* TODO: should be elsewhere */
+/* In case of verbose debug output, enable this: */
+#if 0
 #define DEBUG
 #endif
 
@@ -45,8 +45,8 @@
 		} while(0)
 
 #else
-#define DEBUG_DUMP
-#define USB_DBG(fmt, ...)
+#define DEBUG_DUMP		((void)0)
+#define USB_DBG(fmt, ...)	((void)0)
 #endif
 
 
@@ -56,7 +56,8 @@
 #define USB_ASSERT(cond, otherwise)					\
 		do {							\
 			if(!(cond)) {					\
-				USB_MSG("ERROR - "otherwise);		\
+				USB_MSG("ASSERTION ERROR (%s:%d) - "	\
+					otherwise, __func__, __LINE__);	\
 				exit(EXIT_FAILURE);			\
 			}						\
 		} while(0)