xref: /rk3399_rockchip-uboot/drivers/usb/gadget/f_sdp.c (revision 5661f08a71446ab0ffb5bcf595709027e68f10fa)
1 /*
2  * f_sdp.c -- USB HID Serial Download Protocol
3  *
4  * Copyright (C) 2017 Toradex
5  * Author: Stefan Agner <stefan.agner@toradex.com>
6  *
7  * This file implements the Serial Download Protocol (SDP) as specified in
8  * the i.MX 6 Reference Manual. The SDP is a USB HID based protocol and
9  * allows to download images directly to memory. The implementation
10  * works with the imx_loader (imx_usb) USB client software on host side.
11  *
12  * Not all commands are implemented, e.g. WRITE_REGISTER, DCD_WRITE and
13  * SKIP_DCD_HEADER are only stubs.
14  *
15  * Parts of the implementation are based on f_dfu and f_thor.
16  *
17  * SPDX-License-Identifier:	GPL-2.0+
18  */
19 
20 #include <errno.h>
21 #include <common.h>
22 #include <console.h>
23 #include <malloc.h>
24 
25 #include <linux/usb/ch9.h>
26 #include <linux/usb/gadget.h>
27 #include <linux/usb/composite.h>
28 
29 #include <asm/io.h>
30 #include <g_dnl.h>
31 #include <sdp.h>
32 #include <imximage.h>
33 
34 #define HID_REPORT_ID_MASK	0x000000ff
35 
36 /*
37  * HID class requests
38  */
39 #define HID_REQ_GET_REPORT		0x01
40 #define HID_REQ_GET_IDLE		0x02
41 #define HID_REQ_GET_PROTOCOL		0x03
42 #define HID_REQ_SET_REPORT		0x09
43 #define HID_REQ_SET_IDLE		0x0A
44 #define HID_REQ_SET_PROTOCOL		0x0B
45 
46 #define HID_USAGE_PAGE_LEN		76
47 
48 struct hid_report {
49 	u8 usage_page[HID_USAGE_PAGE_LEN];
50 } __packed;
51 
52 #define SDP_READ_REGISTER	0x0101
53 #define SDP_WRITE_REGISTER	0x0202
54 #define SDP_WRITE_FILE		0x0404
55 #define SDP_ERROR_STATUS	0x0505
56 #define SDP_DCD_WRITE		0x0a0a
57 #define SDP_JUMP_ADDRESS	0x0b0b
58 #define SDP_SKIP_DCD_HEADER	0x0c0c
59 
60 #define SDP_SECURITY_CLOSED		0x12343412
61 #define SDP_SECURITY_OPEN		0x56787856
62 
63 #define SDP_WRITE_FILE_COMPLETE		0x88888888
64 #define SDP_WRITE_REGISTER_COMPLETE	0x128A8A12
65 #define SDP_SKIP_DCD_HEADER_COMPLETE	0x900DD009
66 #define SDP_ERROR_IMXHEADER		0x000a0533
67 
68 #define SDP_COMMAND_LEN		16
69 
70 struct sdp_command {
71 	u16 cmd;
72 	u32 addr;
73 	u8 format;
74 	u32 cnt;
75 	u32 data;
76 	u8 rsvd;
77 } __packed;
78 
79 enum sdp_state {
80 	SDP_STATE_IDLE,
81 	SDP_STATE_RX_DCD_DATA,
82 	SDP_STATE_RX_FILE_DATA,
83 	SDP_STATE_TX_SEC_CONF,
84 	SDP_STATE_TX_SEC_CONF_BUSY,
85 	SDP_STATE_TX_REGISTER,
86 	SDP_STATE_TX_REGISTER_BUSY,
87 	SDP_STATE_TX_STATUS,
88 	SDP_STATE_TX_STATUS_BUSY,
89 	SDP_STATE_JUMP,
90 };
91 
92 struct f_sdp {
93 	struct usb_function		usb_function;
94 
95 	struct usb_descriptor_header	**function;
96 
97 	u8				altsetting;
98 	enum sdp_state			state;
99 	enum sdp_state			next_state;
100 	u32				dnl_address;
101 	u32				dnl_bytes_remaining;
102 	u32				jmp_address;
103 	bool				always_send_status;
104 	u32				error_status;
105 
106 	/* EP0 request */
107 	struct usb_request		*req;
108 
109 	/* EP1 IN */
110 	struct usb_ep			*in_ep;
111 	struct usb_request		*in_req;
112 
113 	bool				configuration_done;
114 };
115 
116 static struct f_sdp *sdp_func;
117 
118 static inline struct f_sdp *func_to_sdp(struct usb_function *f)
119 {
120 	return container_of(f, struct f_sdp, usb_function);
121 }
122 
123 static struct usb_interface_descriptor sdp_intf_runtime = {
124 	.bLength =		sizeof(sdp_intf_runtime),
125 	.bDescriptorType =	USB_DT_INTERFACE,
126 	.bAlternateSetting =	0,
127 	.bNumEndpoints =	1,
128 	.bInterfaceClass =	USB_CLASS_HID,
129 	.bInterfaceSubClass =	0,
130 	.bInterfaceProtocol =	0,
131 	/* .iInterface = DYNAMIC */
132 };
133 
134 /* HID configuration */
135 static struct usb_class_hid_descriptor sdp_hid_desc = {
136 	.bLength =		sizeof(sdp_hid_desc),
137 	.bDescriptorType =	USB_DT_CS_DEVICE,
138 
139 	.bcdCDC =		__constant_cpu_to_le16(0x0110),
140 	.bCountryCode =		0,
141 	.bNumDescriptors =	1,
142 
143 	.bDescriptorType0	= USB_DT_HID_REPORT,
144 	.wDescriptorLength0	= HID_USAGE_PAGE_LEN,
145 };
146 
147 static struct usb_endpoint_descriptor in_desc = {
148 	.bLength =		USB_DT_ENDPOINT_SIZE,
149 	.bDescriptorType =	USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
150 
151 	.bEndpointAddress =	1 | USB_DIR_IN,
152 	.bmAttributes =	USB_ENDPOINT_XFER_INT,
153 	.wMaxPacketSize =	64,
154 	.bInterval =		1,
155 };
156 
157 static struct usb_descriptor_header *sdp_runtime_descs[] = {
158 	(struct usb_descriptor_header *)&sdp_intf_runtime,
159 	(struct usb_descriptor_header *)&sdp_hid_desc,
160 	(struct usb_descriptor_header *)&in_desc,
161 	NULL,
162 };
163 
164 /* This is synchronized with what the SoC implementation reports */
165 static struct hid_report sdp_hid_report = {
166 	.usage_page = {
167 		0x06, 0x00, 0xff, /* Usage Page */
168 		0x09, 0x01, /* Usage (Pointer?) */
169 		0xa1, 0x01, /* Collection */
170 
171 		0x85, 0x01, /* Report ID */
172 		0x19, 0x01, /* Usage Minimum */
173 		0x29, 0x01, /* Usage Maximum */
174 		0x15, 0x00, /* Local Minimum */
175 		0x26, 0xFF, 0x00, /* Local Maximum? */
176 		0x75, 0x08, /* Report Size */
177 		0x95, 0x10, /* Report Count */
178 		0x91, 0x02, /* Output Data */
179 
180 		0x85, 0x02, /* Report ID */
181 		0x19, 0x01, /* Usage Minimum */
182 		0x29, 0x01, /* Usage Maximum */
183 		0x15, 0x00, /* Local Minimum */
184 		0x26, 0xFF, 0x00, /* Local Maximum? */
185 		0x75, 0x80, /* Report Size 128 */
186 		0x95, 0x40, /* Report Count */
187 		0x91, 0x02, /* Output Data */
188 
189 		0x85, 0x03, /* Report ID */
190 		0x19, 0x01, /* Usage Minimum */
191 		0x29, 0x01, /* Usage Maximum */
192 		0x15, 0x00, /* Local Minimum */
193 		0x26, 0xFF, 0x00, /* Local Maximum? */
194 		0x75, 0x08, /* Report Size 8 */
195 		0x95, 0x04, /* Report Count */
196 		0x81, 0x02, /* Input Data */
197 
198 		0x85, 0x04, /* Report ID */
199 		0x19, 0x01, /* Usage Minimum */
200 		0x29, 0x01, /* Usage Maximum */
201 		0x15, 0x00, /* Local Minimum */
202 		0x26, 0xFF, 0x00, /* Local Maximum? */
203 		0x75, 0x08, /* Report Size 8 */
204 		0x95, 0x40, /* Report Count */
205 		0x81, 0x02, /* Input Data */
206 		0xc0
207 	},
208 };
209 
210 static const char sdp_name[] = "Serial Downloader Protocol";
211 
212 /*
213  * static strings, in UTF-8
214  */
215 static struct usb_string strings_sdp_generic[] = {
216 	[0].s = sdp_name,
217 	{  }			/* end of list */
218 };
219 
220 static struct usb_gadget_strings stringtab_sdp_generic = {
221 	.language	= 0x0409,	/* en-us */
222 	.strings	= strings_sdp_generic,
223 };
224 
225 static struct usb_gadget_strings *sdp_generic_strings[] = {
226 	&stringtab_sdp_generic,
227 	NULL,
228 };
229 
230 static void sdp_rx_command_complete(struct usb_ep *ep, struct usb_request *req)
231 {
232 	struct f_sdp *sdp = req->context;
233 	int status = req->status;
234 	u8 *data = req->buf;
235 	u8 report = data[0];
236 
237 	if (status != 0) {
238 		error("Status: %d", status);
239 		return;
240 	}
241 
242 	if (report != 1) {
243 		error("Unexpected report %d", report);
244 		return;
245 	}
246 
247 	struct sdp_command *cmd = req->buf + 1;
248 
249 	debug("%s: command: %04x, addr: %08x, cnt: %u\n",
250 	      __func__, be16_to_cpu(cmd->cmd),
251 	      be32_to_cpu(cmd->addr), be32_to_cpu(cmd->cnt));
252 
253 	switch (be16_to_cpu(cmd->cmd)) {
254 	case SDP_READ_REGISTER:
255 		sdp->always_send_status = false;
256 		sdp->error_status = 0x0;
257 
258 		sdp->state = SDP_STATE_TX_SEC_CONF;
259 		sdp->dnl_address = be32_to_cpu(cmd->addr);
260 		sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
261 		sdp->next_state = SDP_STATE_TX_REGISTER;
262 		printf("Reading %d registers at 0x%08x... ",
263 		       sdp->dnl_bytes_remaining, sdp->dnl_address);
264 		break;
265 	case SDP_WRITE_FILE:
266 		sdp->always_send_status = true;
267 		sdp->error_status = SDP_WRITE_FILE_COMPLETE;
268 
269 		sdp->state = SDP_STATE_RX_FILE_DATA;
270 		sdp->dnl_address = be32_to_cpu(cmd->addr);
271 		sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
272 		sdp->next_state = SDP_STATE_IDLE;
273 
274 		printf("Downloading file of size %d to 0x%08x... ",
275 		       sdp->dnl_bytes_remaining, sdp->dnl_address);
276 
277 		break;
278 	case SDP_ERROR_STATUS:
279 		sdp->always_send_status = true;
280 		sdp->error_status = 0;
281 
282 		sdp->state = SDP_STATE_TX_SEC_CONF;
283 		sdp->next_state = SDP_STATE_IDLE;
284 		break;
285 	case SDP_DCD_WRITE:
286 		sdp->always_send_status = true;
287 		sdp->error_status = SDP_WRITE_REGISTER_COMPLETE;
288 
289 		sdp->state = SDP_STATE_RX_DCD_DATA;
290 		sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
291 		sdp->next_state = SDP_STATE_IDLE;
292 		break;
293 	case SDP_JUMP_ADDRESS:
294 		sdp->always_send_status = false;
295 		sdp->error_status = 0;
296 
297 		sdp->jmp_address = be32_to_cpu(cmd->addr);
298 		sdp->state = SDP_STATE_TX_SEC_CONF;
299 		sdp->next_state = SDP_STATE_JUMP;
300 		break;
301 	case SDP_SKIP_DCD_HEADER:
302 		sdp->always_send_status = true;
303 		sdp->error_status = SDP_SKIP_DCD_HEADER_COMPLETE;
304 
305 		/* Ignore command, DCD not supported anyway */
306 		sdp->state = SDP_STATE_TX_SEC_CONF;
307 		sdp->next_state = SDP_STATE_IDLE;
308 		break;
309 	default:
310 		error("Unknown command: %04x\n", be16_to_cpu(cmd->cmd));
311 	}
312 }
313 
314 static void sdp_rx_data_complete(struct usb_ep *ep, struct usb_request *req)
315 {
316 	struct f_sdp *sdp = req->context;
317 	int status = req->status;
318 	u8 *data = req->buf;
319 	u8 report = data[0];
320 	int datalen = req->length - 1;
321 
322 	if (status != 0) {
323 		error("Status: %d", status);
324 		return;
325 	}
326 
327 	if (report != 2) {
328 		error("Unexpected report %d", report);
329 		return;
330 	}
331 
332 	if (sdp->dnl_bytes_remaining < datalen) {
333 		/*
334 		 * Some USB stacks require to send a complete buffer as
335 		 * specified in the HID descriptor. This leads to longer
336 		 * transfers than the file length, no problem for us.
337 		 */
338 		sdp->dnl_bytes_remaining = 0;
339 	} else {
340 		sdp->dnl_bytes_remaining -= datalen;
341 	}
342 
343 	if (sdp->state == SDP_STATE_RX_FILE_DATA) {
344 		memcpy((void *)sdp->dnl_address, req->buf + 1, datalen);
345 		sdp->dnl_address += datalen;
346 	}
347 
348 	if (sdp->dnl_bytes_remaining)
349 		return;
350 
351 	printf("done\n");
352 
353 	switch (sdp->state) {
354 	case SDP_STATE_RX_FILE_DATA:
355 		sdp->state = SDP_STATE_TX_SEC_CONF;
356 		break;
357 	case SDP_STATE_RX_DCD_DATA:
358 		sdp->state = SDP_STATE_TX_SEC_CONF;
359 		break;
360 	default:
361 		error("Invalid state: %d", sdp->state);
362 	}
363 }
364 
365 static void sdp_tx_complete(struct usb_ep *ep, struct usb_request *req)
366 {
367 	struct f_sdp *sdp = req->context;
368 	int status = req->status;
369 
370 	if (status != 0) {
371 		error("Status: %d", status);
372 		return;
373 	}
374 
375 	switch (sdp->state) {
376 	case SDP_STATE_TX_SEC_CONF_BUSY:
377 		/* Not all commands require status report */
378 		if (sdp->always_send_status || sdp->error_status)
379 			sdp->state = SDP_STATE_TX_STATUS;
380 		else
381 			sdp->state = sdp->next_state;
382 
383 		break;
384 	case SDP_STATE_TX_STATUS_BUSY:
385 		sdp->state = sdp->next_state;
386 		break;
387 	case SDP_STATE_TX_REGISTER_BUSY:
388 		if (sdp->dnl_bytes_remaining)
389 			sdp->state = SDP_STATE_TX_REGISTER;
390 		else
391 			sdp->state = SDP_STATE_IDLE;
392 		break;
393 	default:
394 		error("Wrong State: %d", sdp->state);
395 		sdp->state = SDP_STATE_IDLE;
396 		break;
397 	}
398 	debug("%s complete --> %d, %d/%d\n", ep->name,
399 	      status, req->actual, req->length);
400 }
401 
402 static int sdp_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
403 {
404 	struct usb_gadget *gadget = f->config->cdev->gadget;
405 	struct usb_request *req = f->config->cdev->req;
406 	struct f_sdp *sdp = f->config->cdev->req->context;
407 	u16 len = le16_to_cpu(ctrl->wLength);
408 	u16 w_value = le16_to_cpu(ctrl->wValue);
409 	int value = 0;
410 	u8 req_type = ctrl->bRequestType & USB_TYPE_MASK;
411 
412 	debug("w_value: 0x%04x len: 0x%04x\n", w_value, len);
413 	debug("req_type: 0x%02x ctrl->bRequest: 0x%02x sdp->state: %d\n",
414 	      req_type, ctrl->bRequest, sdp->state);
415 
416 	if (req_type == USB_TYPE_STANDARD) {
417 		if (ctrl->bRequest == USB_REQ_GET_DESCRIPTOR) {
418 			/* Send HID report descriptor */
419 			value = min(len, (u16) sizeof(sdp_hid_report));
420 			memcpy(req->buf, &sdp_hid_report, value);
421 			sdp->configuration_done = true;
422 		}
423 	}
424 
425 	if (req_type == USB_TYPE_CLASS) {
426 		int report = w_value & HID_REPORT_ID_MASK;
427 
428 		/* HID (SDP) request */
429 		switch (ctrl->bRequest) {
430 		case HID_REQ_SET_REPORT:
431 			switch (report) {
432 			case 1:
433 				value = SDP_COMMAND_LEN + 1;
434 				req->complete = sdp_rx_command_complete;
435 				break;
436 			case 2:
437 				value = len;
438 				req->complete = sdp_rx_data_complete;
439 				break;
440 			}
441 		}
442 	}
443 
444 	if (value >= 0) {
445 		req->length = value;
446 		req->zero = value < len;
447 		value = usb_ep_queue(gadget->ep0, req, 0);
448 		if (value < 0) {
449 			debug("ep_queue --> %d\n", value);
450 			req->status = 0;
451 		}
452 	}
453 
454 	return value;
455 }
456 
457 static int sdp_bind(struct usb_configuration *c, struct usb_function *f)
458 {
459 	struct usb_gadget *gadget = c->cdev->gadget;
460 	struct usb_composite_dev *cdev = c->cdev;
461 	struct f_sdp *sdp = func_to_sdp(f);
462 	int rv = 0, id;
463 
464 	id = usb_interface_id(c, f);
465 	if (id < 0)
466 		return id;
467 	sdp_intf_runtime.bInterfaceNumber = id;
468 
469 	struct usb_ep *ep;
470 
471 	/* allocate instance-specific endpoints */
472 	ep = usb_ep_autoconfig(gadget, &in_desc);
473 	if (!ep) {
474 		rv = -ENODEV;
475 		goto error;
476 	}
477 
478 	sdp->in_ep = ep; /* Store IN EP for enabling @ setup */
479 
480 	cdev->req->context = sdp;
481 
482 error:
483 	return rv;
484 }
485 
486 static void sdp_unbind(struct usb_configuration *c, struct usb_function *f)
487 {
488 	free(sdp_func);
489 	sdp_func = NULL;
490 }
491 
492 static struct usb_request *alloc_ep_req(struct usb_ep *ep, unsigned length)
493 {
494 	struct usb_request *req;
495 
496 	req = usb_ep_alloc_request(ep, 0);
497 	if (!req)
498 		return req;
499 
500 	req->length = length;
501 	req->buf = memalign(CONFIG_SYS_CACHELINE_SIZE, length);
502 	if (!req->buf) {
503 		usb_ep_free_request(ep, req);
504 		req = NULL;
505 	}
506 
507 	return req;
508 }
509 
510 
511 static struct usb_request *sdp_start_ep(struct usb_ep *ep)
512 {
513 	struct usb_request *req;
514 
515 	req = alloc_ep_req(ep, 64);
516 	debug("%s: ep:%p req:%p\n", __func__, ep, req);
517 
518 	if (!req)
519 		return NULL;
520 
521 	memset(req->buf, 0, req->length);
522 	req->complete = sdp_tx_complete;
523 
524 	return req;
525 }
526 static int sdp_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
527 {
528 	struct f_sdp *sdp = func_to_sdp(f);
529 	struct usb_composite_dev *cdev = f->config->cdev;
530 	int result;
531 
532 	debug("%s: intf: %d alt: %d\n", __func__, intf, alt);
533 
534 	result = usb_ep_enable(sdp->in_ep, &in_desc);
535 	if (result)
536 		return result;
537 	sdp->in_req = sdp_start_ep(sdp->in_ep);
538 	sdp->in_req->context = sdp;
539 
540 	sdp->in_ep->driver_data = cdev; /* claim */
541 
542 	sdp->altsetting = alt;
543 	sdp->state = SDP_STATE_IDLE;
544 
545 	return 0;
546 }
547 
548 static int sdp_get_alt(struct usb_function *f, unsigned intf)
549 {
550 	struct f_sdp *sdp = func_to_sdp(f);
551 
552 	return sdp->altsetting;
553 }
554 
555 static void sdp_disable(struct usb_function *f)
556 {
557 	struct f_sdp *sdp = func_to_sdp(f);
558 
559 	usb_ep_disable(sdp->in_ep);
560 
561 	if (sdp->in_req) {
562 		free(sdp->in_req);
563 		sdp->in_req = NULL;
564 	}
565 }
566 
567 static int sdp_bind_config(struct usb_configuration *c)
568 {
569 	int status;
570 
571 	if (!sdp_func) {
572 		sdp_func = memalign(CONFIG_SYS_CACHELINE_SIZE, sizeof(*sdp_func));
573 		if (!sdp_func)
574 			return -ENOMEM;
575 	}
576 
577 	memset(sdp_func, 0, sizeof(*sdp_func));
578 
579 	sdp_func->usb_function.name = "sdp";
580 	sdp_func->usb_function.hs_descriptors = sdp_runtime_descs;
581 	sdp_func->usb_function.descriptors = sdp_runtime_descs;
582 	sdp_func->usb_function.bind = sdp_bind;
583 	sdp_func->usb_function.unbind = sdp_unbind;
584 	sdp_func->usb_function.set_alt = sdp_set_alt;
585 	sdp_func->usb_function.get_alt = sdp_get_alt;
586 	sdp_func->usb_function.disable = sdp_disable;
587 	sdp_func->usb_function.strings = sdp_generic_strings;
588 	sdp_func->usb_function.setup = sdp_setup;
589 
590 	status = usb_add_function(c, &sdp_func->usb_function);
591 
592 	return status;
593 }
594 
595 int sdp_init(int controller_index)
596 {
597 	printf("SDP: initialize...\n");
598 	while (!sdp_func->configuration_done) {
599 		if (ctrlc()) {
600 			puts("\rCTRL+C - Operation aborted.\n");
601 			return 1;
602 		}
603 		usb_gadget_handle_interrupts(controller_index);
604 	}
605 
606 	return 0;
607 }
608 
609 static u32 sdp_jump_imxheader(void *address)
610 {
611 	flash_header_v2_t *headerv2 = address;
612 	ulong (*entry)(void);
613 
614 	if (headerv2->header.tag != IVT_HEADER_TAG) {
615 		printf("Header Tag is not an IMX image\n");
616 		return SDP_ERROR_IMXHEADER;
617 	}
618 
619 	printf("Jumping to 0x%08x\n", headerv2->entry);
620 	entry = (void *)headerv2->entry;
621 	entry();
622 
623 	/* The image probably never returns hence we won't reach that point */
624 	return 0;
625 }
626 
627 static void sdp_handle_in_ep(void)
628 {
629 	u8 *data = sdp_func->in_req->buf;
630 	u32 status;
631 	int datalen;
632 
633 	switch (sdp_func->state) {
634 	case SDP_STATE_TX_SEC_CONF:
635 		debug("Report 3: HAB security\n");
636 		data[0] = 3;
637 
638 		status = SDP_SECURITY_OPEN;
639 		memcpy(&data[1], &status, 4);
640 		sdp_func->in_req->length = 5;
641 		usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
642 		sdp_func->state = SDP_STATE_TX_SEC_CONF_BUSY;
643 		break;
644 
645 	case SDP_STATE_TX_STATUS:
646 		debug("Report 4: Status\n");
647 		data[0] = 4;
648 
649 		memcpy(&data[1], &sdp_func->error_status, 4);
650 		sdp_func->in_req->length = 65;
651 		usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
652 		sdp_func->state = SDP_STATE_TX_STATUS_BUSY;
653 		break;
654 	case SDP_STATE_TX_REGISTER:
655 		debug("Report 4: Register Values\n");
656 		data[0] = 4;
657 
658 		datalen = sdp_func->dnl_bytes_remaining;
659 
660 		if (datalen > 64)
661 			datalen = 64;
662 
663 		memcpy(&data[1], (void *)sdp_func->dnl_address, datalen);
664 		sdp_func->in_req->length = 65;
665 
666 		sdp_func->dnl_bytes_remaining -= datalen;
667 		sdp_func->dnl_address += datalen;
668 
669 		usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
670 		sdp_func->state = SDP_STATE_TX_REGISTER_BUSY;
671 		break;
672 	case SDP_STATE_JUMP:
673 		printf("Checking imxheader at 0x%08x\n", f_sdp->jmp_address);
674 		status = sdp_jump_imxheader((void *)f_sdp->jmp_address);
675 
676 		sdp_func->next_state = SDP_STATE_IDLE;
677 		sdp_func->error_status = status;
678 
679 		/* Only send Report 4 if there was an error */
680 		if (status)
681 			sdp_func->state = SDP_STATE_TX_STATUS;
682 		else
683 			sdp_func->state = SDP_STATE_IDLE;
684 		break;
685 	default:
686 		break;
687 	};
688 }
689 
690 void sdp_handle(int controller_index)
691 {
692 	printf("SDP: handle requests...\n");
693 	while (1) {
694 		if (ctrlc()) {
695 			puts("\rCTRL+C - Operation aborted.\n");
696 			return;
697 		}
698 
699 		usb_gadget_handle_interrupts(controller_index);
700 
701 		sdp_handle_in_ep();
702 	}
703 }
704 
705 int sdp_add(struct usb_configuration *c)
706 {
707 	int id;
708 
709 	id = usb_string_id(c->cdev);
710 	if (id < 0)
711 		return id;
712 	strings_sdp_generic[0].id = id;
713 	sdp_intf_runtime.iInterface = id;
714 
715 	debug("%s: cdev: %p gadget: %p gadget->ep0: %p\n", __func__,
716 	      c->cdev, c->cdev->gadget, c->cdev->gadget->ep0);
717 
718 	return sdp_bind_config(c);
719 }
720 
721 DECLARE_GADGET_BIND_CALLBACK(usb_dnl_sdp, sdp_add);
722