1 files changed, 191 insertions, 28 deletions

diff --git a/drivers/usb/gadget/f_sdp.c b/drivers/usb/gadget/f_sdp.c
index f2fe89d2a6..e48aa2f90d 100644
--- a/drivers/usb/gadget/f_sdp.c
+++ b/drivers/usb/gadget/f_sdp.c
@@ -71,6 +71,10 @@ struct hid_report {
 
 #define SDP_COMMAND_LEN		16
 
+#define SDP_HID_PACKET_SIZE_EP1 1024
+
+#define SDP_EXIT 1
+
 struct sdp_command {
 	u16 cmd;
 	u32 addr;
@@ -82,8 +86,10 @@ struct sdp_command {
 
 enum sdp_state {
 	SDP_STATE_IDLE,
+	SDP_STATE_RX_CMD,
 	SDP_STATE_RX_DCD_DATA,
 	SDP_STATE_RX_FILE_DATA,
+	SDP_STATE_RX_FILE_DATA_BUSY,
 	SDP_STATE_TX_SEC_CONF,
 	SDP_STATE_TX_SEC_CONF_BUSY,
 	SDP_STATE_TX_REGISTER,
@@ -114,8 +120,12 @@ struct f_sdp {
 	/* EP1 IN */
 	struct usb_ep			*in_ep;
 	struct usb_request		*in_req;
+	/* EP1 OUT */
+	struct usb_ep			*out_ep;
+	struct usb_request		*out_req;
 
 	bool				configuration_done;
+	bool				ep_int_enable;
 };
 
 static struct f_sdp *sdp_func;
@@ -129,7 +139,7 @@ static struct usb_interface_descriptor sdp_intf_runtime = {
 	.bLength =		sizeof(sdp_intf_runtime),
 	.bDescriptorType =	USB_DT_INTERFACE,
 	.bAlternateSetting =	0,
-	.bNumEndpoints =	1,
+	.bNumEndpoints =	2,
 	.bInterfaceClass =	USB_CLASS_HID,
 	.bInterfaceSubClass =	0,
 	.bInterfaceProtocol =	0,
@@ -159,10 +169,49 @@ static struct usb_endpoint_descriptor in_desc = {
 	.bInterval =		1,
 };
 
+static struct usb_endpoint_descriptor out_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
+
+	.bEndpointAddress =	1 | USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	64,
+	.bInterval =		1,
+};
+
+static struct usb_endpoint_descriptor in_hs_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
+
+	.bEndpointAddress =	1 | USB_DIR_IN,
+	.bmAttributes =	USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	512,
+	.bInterval =		3,
+};
+
+static struct usb_endpoint_descriptor out_hs_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
+
+	.bEndpointAddress =	1 | USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	SDP_HID_PACKET_SIZE_EP1,
+	.bInterval =		3,
+};
+
 static struct usb_descriptor_header *sdp_runtime_descs[] = {
 	(struct usb_descriptor_header *)&sdp_intf_runtime,
 	(struct usb_descriptor_header *)&sdp_hid_desc,
 	(struct usb_descriptor_header *)&in_desc,
+	(struct usb_descriptor_header *)&out_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *sdp_runtime_hs_descs[] = {
+	(struct usb_descriptor_header *)&sdp_intf_runtime,
+	(struct usb_descriptor_header *)&sdp_hid_desc,
+	(struct usb_descriptor_header *)&in_hs_desc,
+	(struct usb_descriptor_header *)&out_hs_desc,
 	NULL,
 };
 
@@ -328,7 +377,7 @@ static void sdp_rx_data_complete(struct usb_ep *ep, struct usb_request *req)
 	int status = req->status;
 	u8 *data = req->buf;
 	u8 report = data[0];
-	int datalen = req->length - 1;
+	int datalen = req->actual - 1;
 
 	if (status != 0) {
 		pr_err("Status: %d\n", status);
@@ -351,13 +400,15 @@ static void sdp_rx_data_complete(struct usb_ep *ep, struct usb_request *req)
 		sdp->dnl_bytes_remaining -= datalen;
 	}
 
-	if (sdp->state == SDP_STATE_RX_FILE_DATA) {
+	if (sdp->state == SDP_STATE_RX_FILE_DATA_BUSY) {
 		memcpy(sdp_ptr(sdp->dnl_address), req->buf + 1, datalen);
 		sdp->dnl_address += datalen;
 	}
 
-	if (sdp->dnl_bytes_remaining)
+	if (sdp->dnl_bytes_remaining) {
+		sdp->state = SDP_STATE_RX_FILE_DATA;
 		return;
+	}
 
 #ifndef CONFIG_SPL_BUILD
 	env_set_hex("filesize", sdp->dnl_bytes);
@@ -365,7 +416,7 @@ static void sdp_rx_data_complete(struct usb_ep *ep, struct usb_request *req)
 	printf("done\n");
 
 	switch (sdp->state) {
-	case SDP_STATE_RX_FILE_DATA:
+	case SDP_STATE_RX_FILE_DATA_BUSY:
 		sdp->state = SDP_STATE_TX_SEC_CONF;
 		break;
 	case SDP_STATE_RX_DCD_DATA:
@@ -446,10 +497,12 @@ static int sdp_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 			case 1:
 				value = SDP_COMMAND_LEN + 1;
 				req->complete = sdp_rx_command_complete;
+				sdp_func->ep_int_enable = false;
 				break;
 			case 2:
 				value = len;
 				req->complete = sdp_rx_data_complete;
+				sdp_func->state = SDP_STATE_RX_FILE_DATA_BUSY;
 				break;
 			}
 		}
@@ -480,16 +533,29 @@ static int sdp_bind(struct usb_configuration *c, struct usb_function *f)
 		return id;
 	sdp_intf_runtime.bInterfaceNumber = id;
 
-	struct usb_ep *ep;
+	struct usb_ep *ep_in, *ep_out;
 
 	/* allocate instance-specific endpoints */
-	ep = usb_ep_autoconfig(gadget, &in_desc);
-	if (!ep) {
+	ep_in = usb_ep_autoconfig(gadget, &in_desc);
+	if (!ep_in) {
+		rv = -ENODEV;
+		goto error;
+	}
+
+	ep_out = usb_ep_autoconfig(gadget, &out_desc);
+	if (!ep_out) {
 		rv = -ENODEV;
 		goto error;
 	}
 
-	sdp->in_ep = ep; /* Store IN EP for enabling @ setup */
+	if (gadget_is_dualspeed(gadget)) {
+		/* Assume endpoint addresses are the same for both speeds */
+		in_hs_desc.bEndpointAddress = in_desc.bEndpointAddress;
+		out_hs_desc.bEndpointAddress = out_desc.bEndpointAddress;
+	}
+
+	sdp->in_ep = ep_in; /* Store IN EP for enabling @ setup */
+	sdp->out_ep = ep_out;
 
 	cdev->req->context = sdp;
 
@@ -522,18 +588,29 @@ static struct usb_request *alloc_ep_req(struct usb_ep *ep, unsigned length)
 }
 
 
-static struct usb_request *sdp_start_ep(struct usb_ep *ep)
+static struct usb_request *sdp_start_ep(struct usb_ep *ep, bool in)
 {
 	struct usb_request *req;
 
-	req = alloc_ep_req(ep, 64);
+	if (in)
+		req = alloc_ep_req(ep, 65);
+	else
+		req = alloc_ep_req(ep, 2048);
+/*
+ * OUT endpoint request length should be an integral multiple of
+ * maxpacket size 1024, else we break on certain controllers like
+ * DWC3 that expect bulk OUT requests to be divisible by maxpacket size.
+ */
 	debug("%s: ep:%p req:%p\n", __func__, ep, req);
 
 	if (!req)
 		return NULL;
 
 	memset(req->buf, 0, req->length);
-	req->complete = sdp_tx_complete;
+	if (in)
+		req->complete = sdp_tx_complete;
+	else
+		req->complete = sdp_rx_command_complete;
 
 	return req;
 }
@@ -541,20 +618,32 @@ static int sdp_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
 	struct f_sdp *sdp = func_to_sdp(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_gadget *gadget = cdev->gadget;
 	int result;
 
 	debug("%s: intf: %d alt: %d\n", __func__, intf, alt);
 
-	result = usb_ep_enable(sdp->in_ep, &in_desc);
+	if (gadget_is_dualspeed(gadget) && gadget->speed == USB_SPEED_HIGH) {
+		result = usb_ep_enable(sdp->in_ep, &in_hs_desc);
+		result |= usb_ep_enable(sdp->out_ep, &out_hs_desc);
+	} else {
+		result = usb_ep_enable(sdp->in_ep, &in_desc);
+		result |= usb_ep_enable(sdp->out_ep, &out_desc);
+	}
 	if (result)
 		return result;
-	sdp->in_req = sdp_start_ep(sdp->in_ep);
+
+	sdp->in_req = sdp_start_ep(sdp->in_ep, true);
 	sdp->in_req->context = sdp;
+	sdp->out_req = sdp_start_ep(sdp->out_ep, false);
+	sdp->out_req->context = sdp;
 
 	sdp->in_ep->driver_data = cdev; /* claim */
+	sdp->out_ep->driver_data = cdev; /* claim */
 
 	sdp->altsetting = alt;
 	sdp->state = SDP_STATE_IDLE;
+	sdp->ep_int_enable = true;
 
 	return 0;
 }
@@ -571,11 +660,18 @@ static void sdp_disable(struct usb_function *f)
 	struct f_sdp *sdp = func_to_sdp(f);
 
 	usb_ep_disable(sdp->in_ep);
+	usb_ep_disable(sdp->out_ep);
 
 	if (sdp->in_req) {
-		free(sdp->in_req);
+		free(sdp->in_req->buf);
+		usb_ep_free_request(sdp->in_ep, sdp->in_req);
 		sdp->in_req = NULL;
 	}
+	if (sdp->out_req) {
+		free(sdp->out_req->buf);
+		usb_ep_free_request(sdp->out_ep, sdp->out_req);
+		sdp->out_req = NULL;
+	}
 }
 
 static int sdp_bind_config(struct usb_configuration *c)
@@ -591,7 +687,7 @@ static int sdp_bind_config(struct usb_configuration *c)
 	memset(sdp_func, 0, sizeof(*sdp_func));
 
 	sdp_func->usb_function.name = "sdp";
-	sdp_func->usb_function.hs_descriptors = sdp_runtime_descs;
+	sdp_func->usb_function.hs_descriptors = sdp_runtime_hs_descs;
 	sdp_func->usb_function.descriptors = sdp_runtime_descs;
 	sdp_func->usb_function.bind = sdp_bind;
 	sdp_func->usb_function.unbind = sdp_unbind;
@@ -641,19 +737,43 @@ static u32 sdp_jump_imxheader(void *address)
 }
 
 #ifdef CONFIG_SPL_BUILD
-#ifdef CONFIG_SPL_LOAD_FIT
-static ulong sdp_fit_read(struct spl_load_info *load, ulong sector,
-			  ulong count, void *buf)
+static ulong sdp_load_read(struct spl_load_info *load, ulong sector,
+			   ulong count, void *buf)
 {
 	debug("%s: sector %lx, count %lx, buf %lx\n",
 	      __func__, sector, count, (ulong)buf);
 	memcpy(buf, (void *)(load->dev + sector), count);
 	return count;
 }
-#endif
+
+static ulong search_fit_header(ulong p, int size)
+{
+	int i;
+
+	for (i = 0; i < size; i += 4) {
+		if (genimg_get_format((const void *)(p + i)) == IMAGE_FORMAT_FIT)
+			return p + i;
+	}
+
+	return 0;
+}
+
+static ulong search_container_header(ulong p, int size)
+{
+	int i;
+	u8 *hdr;
+
+	for (i = 0; i < size; i += 4) {
+		hdr = (u8 *)(p + i);
+		if (*(hdr + 3) == 0x87 && *hdr == 0)
+			if (*(hdr + 1) != 0 || *(hdr + 2) != 0)
+				return p + i;
+	}
+	return 0;
+}
 #endif
 
-static void sdp_handle_in_ep(struct spl_image_info *spl_image)
+static int sdp_handle_in_ep(struct spl_image_info *spl_image)
 {
 	u8 *data = sdp_func->in_req->buf;
 	u32 status;
@@ -705,6 +825,15 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 		/* If imx header fails, try some U-Boot specific headers */
 		if (status) {
 #ifdef CONFIG_SPL_BUILD
+			if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER))
+				sdp_func->jmp_address = (u32)search_container_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
+			else if (IS_ENABLED(CONFIG_SPL_LOAD_FIT))
+				sdp_func->jmp_address = (u32)search_fit_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
+			if (sdp_func->jmp_address == 0)
+				panic("Error in search header, failed to jump\n");
+
+			printf("Found header at 0x%08x\n", sdp_func->jmp_address);
+
 			image_header_t *header =
 				sdp_ptr(sdp_func->jmp_address);
 #ifdef CONFIG_SPL_LOAD_FIT
@@ -714,13 +843,23 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 				debug("Found FIT\n");
 				load.dev = header;
 				load.bl_len = 1;
-				load.read = sdp_fit_read;
+				load.read = sdp_load_read;
 				spl_load_simple_fit(spl_image, &load, 0,
 						    header);
 
-				return;
+				return SDP_EXIT;
 			}
 #endif
+			if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER)) {
+				struct spl_load_info load;
+
+				load.dev = header;
+				load.bl_len = 1;
+				load.read = sdp_load_read;
+				spl_load_imx_container(spl_image, &load, 0);
+				return SDP_EXIT;
+			}
+
 			/* In SPL, allow jumps to U-Boot images */
 			struct spl_image_info spl_image = {};
 			spl_parse_image_header(&spl_image, header);
@@ -743,6 +882,29 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 	default:
 		break;
 	};
+
+	return 0;
+}
+
+static void sdp_handle_out_ep(void)
+{
+	int rc;
+
+	if (sdp_func->state == SDP_STATE_IDLE) {
+		sdp_func->out_req->complete = sdp_rx_command_complete;
+		rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
+		if (rc)
+			printf("error in submission: %s\n",
+			       sdp_func->out_ep->name);
+		sdp_func->state = SDP_STATE_RX_CMD;
+	} else if (sdp_func->state == SDP_STATE_RX_FILE_DATA) {
+		sdp_func->out_req->complete = sdp_rx_data_complete;
+		rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
+		if (rc)
+			printf("error in submission: %s\n",
+			       sdp_func->out_ep->name);
+		sdp_func->state = SDP_STATE_RX_FILE_DATA_BUSY;
+	}
 }
 
 #ifndef CONFIG_SPL_BUILD
@@ -751,6 +913,7 @@ int sdp_handle(int controller_index)
 int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image)
 #endif
 {
+	int flag = 0;
 	printf("SDP: handle requests...\n");
 	while (1) {
 		if (ctrlc()) {
@@ -758,19 +921,19 @@ int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image)
 			return -EINVAL;
 		}
 
-#ifdef CONFIG_SPL_BUILD
-		if (spl_image->flags & SPL_FIT_FOUND)
+		if (flag == SDP_EXIT)
 			return 0;
-#endif
 
 		WATCHDOG_RESET();
 		usb_gadget_handle_interrupts(controller_index);
 
 #ifdef CONFIG_SPL_BUILD
-		sdp_handle_in_ep(spl_image);
+		flag = sdp_handle_in_ep(spl_image);
 #else
-		sdp_handle_in_ep(NULL);
+		flag = sdp_handle_in_ep(NULL);
 #endif
+		if (sdp_func->ep_int_enable)
+			sdp_handle_out_ep();
 	}
 }