1 /*
2  * Driver for the Atmel USBA high speed USB device controller
3  * [Original from Linux kernel: drivers/usb/gadget/atmel_usba_udc.c]
4  *
5  * Copyright (C) 2005-2013 Atmel Corporation
6  *			   Bo Shen <voice.shen@atmel.com>
7  *
8  * SPDX-License-Identifier:     GPL-2.0+
9  */
10 
11 #include <common.h>
12 #include <asm/errno.h>
13 #include <asm/gpio.h>
14 #include <asm/hardware.h>
15 #include <linux/list.h>
16 #include <linux/usb/ch9.h>
17 #include <linux/usb/gadget.h>
18 #include <linux/usb/atmel_usba_udc.h>
19 #include <malloc.h>
20 #include <usb/lin_gadget_compat.h>
21 
22 #include "atmel_usba_udc.h"
23 
24 static int vbus_is_present(struct usba_udc *udc)
25 {
26 	/* No Vbus detection: Assume always present */
27 	return 1;
28 }
29 
30 static void next_fifo_transaction(struct usba_ep *ep, struct usba_request *req)
31 {
32 	unsigned int transaction_len;
33 
34 	transaction_len = req->req.length - req->req.actual;
35 	req->last_transaction = 1;
36 	if (transaction_len > ep->ep.maxpacket) {
37 		transaction_len = ep->ep.maxpacket;
38 		req->last_transaction = 0;
39 	} else if (transaction_len == ep->ep.maxpacket && req->req.zero) {
40 			req->last_transaction = 0;
41 	}
42 
43 	DBG(DBG_QUEUE, "%s: submit_transaction, req %p (length %d)%s\n",
44 	    ep->ep.name, req, transaction_len,
45 	    req->last_transaction ? ", done" : "");
46 
47 	memcpy(ep->fifo, req->req.buf + req->req.actual, transaction_len);
48 	usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
49 	req->req.actual += transaction_len;
50 }
51 
52 static void submit_request(struct usba_ep *ep, struct usba_request *req)
53 {
54 	DBG(DBG_QUEUE, "%s: submit_request: req %p (length %d), dma: %d\n",
55 	    ep->ep.name, req, req->req.length, req->using_dma);
56 
57 	req->req.actual = 0;
58 	req->submitted = 1;
59 
60 	next_fifo_transaction(ep, req);
61 	if (req->last_transaction) {
62 		usba_ep_writel(ep, CTL_DIS, USBA_TX_PK_RDY);
63 		usba_ep_writel(ep, CTL_ENB, USBA_TX_COMPLETE);
64 	} else {
65 		usba_ep_writel(ep, CTL_DIS, USBA_TX_COMPLETE);
66 		usba_ep_writel(ep, CTL_ENB, USBA_TX_PK_RDY);
67 	}
68 }
69 
70 static void submit_next_request(struct usba_ep *ep)
71 {
72 	struct usba_request *req;
73 
74 	if (list_empty(&ep->queue)) {
75 		usba_ep_writel(ep, CTL_DIS, USBA_TX_PK_RDY | USBA_RX_BK_RDY);
76 		return;
77 	}
78 
79 	req = list_entry(ep->queue.next, struct usba_request, queue);
80 	if (!req->submitted)
81 		submit_request(ep, req);
82 }
83 
84 static void send_status(struct usba_udc *udc, struct usba_ep *ep)
85 {
86 	ep->state = STATUS_STAGE_IN;
87 	usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
88 	usba_ep_writel(ep, CTL_ENB, USBA_TX_COMPLETE);
89 }
90 
91 static void receive_data(struct usba_ep *ep)
92 {
93 	struct usba_udc *udc = ep->udc;
94 	struct usba_request *req;
95 	unsigned long status;
96 	unsigned int bytecount, nr_busy;
97 	int is_complete = 0;
98 
99 	status = usba_ep_readl(ep, STA);
100 	nr_busy = USBA_BFEXT(BUSY_BANKS, status);
101 
102 	DBG(DBG_QUEUE, "receive data: nr_busy=%u\n", nr_busy);
103 
104 	while (nr_busy > 0) {
105 		if (list_empty(&ep->queue)) {
106 			usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
107 			break;
108 		}
109 		req = list_entry(ep->queue.next,
110 				 struct usba_request, queue);
111 
112 		bytecount = USBA_BFEXT(BYTE_COUNT, status);
113 
114 		if (status & USBA_SHORT_PACKET)
115 			is_complete = 1;
116 		if (req->req.actual + bytecount >= req->req.length) {
117 			is_complete = 1;
118 			bytecount = req->req.length - req->req.actual;
119 		}
120 
121 		memcpy(req->req.buf + req->req.actual, ep->fifo, bytecount);
122 		req->req.actual += bytecount;
123 
124 		usba_ep_writel(ep, CLR_STA, USBA_RX_BK_RDY);
125 
126 		if (is_complete) {
127 			DBG(DBG_QUEUE, "%s: request done\n", ep->ep.name);
128 			req->req.status = 0;
129 			list_del_init(&req->queue);
130 			usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
131 			spin_lock(&udc->lock);
132 			req->req.complete(&ep->ep, &req->req);
133 			spin_unlock(&udc->lock);
134 		}
135 
136 		status = usba_ep_readl(ep, STA);
137 		nr_busy = USBA_BFEXT(BUSY_BANKS, status);
138 
139 		if (is_complete && ep_is_control(ep)) {
140 			send_status(udc, ep);
141 			break;
142 		}
143 	}
144 }
145 
146 static void
147 request_complete(struct usba_ep *ep, struct usba_request *req, int status)
148 {
149 	if (req->req.status == -EINPROGRESS)
150 		req->req.status = status;
151 
152 	DBG(DBG_GADGET | DBG_REQ, "%s: req %p complete: status %d, actual %u\n",
153 	    ep->ep.name, req, req->req.status, req->req.actual);
154 
155 	req->req.complete(&ep->ep, &req->req);
156 }
157 
158 static void
159 request_complete_list(struct usba_ep *ep, struct list_head *list, int status)
160 {
161 	struct usba_request *req, *tmp_req;
162 
163 	list_for_each_entry_safe(req, tmp_req, list, queue) {
164 		list_del_init(&req->queue);
165 		request_complete(ep, req, status);
166 	}
167 }
168 
169 static int
170 usba_ep_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
171 {
172 	struct usba_ep *ep = to_usba_ep(_ep);
173 	struct usba_udc *udc = ep->udc;
174 	unsigned long flags = 0, ept_cfg, maxpacket;
175 	unsigned int nr_trans;
176 
177 	DBG(DBG_GADGET, "%s: ep_enable: desc=%p\n", ep->ep.name, desc);
178 
179 	maxpacket = usb_endpoint_maxp(desc) & 0x7ff;
180 
181 	if (((desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK)
182 	      != ep->index) ||
183 	      ep->index == 0 ||
184 	      desc->bDescriptorType != USB_DT_ENDPOINT ||
185 	      maxpacket == 0 ||
186 	      maxpacket > ep->fifo_size) {
187 		DBG(DBG_ERR, "ep_enable: Invalid argument");
188 		return -EINVAL;
189 	}
190 
191 	ep->is_isoc = 0;
192 	ep->is_in = 0;
193 
194 	if (maxpacket <= 8)
195 		ept_cfg = USBA_BF(EPT_SIZE, USBA_EPT_SIZE_8);
196 	else
197 		/* LSB is bit 1, not 0 */
198 		ept_cfg = USBA_BF(EPT_SIZE, fls(maxpacket - 1) - 3);
199 
200 	DBG(DBG_HW, "%s: EPT_SIZE = %lu (maxpacket = %lu)\n",
201 	    ep->ep.name, ept_cfg, maxpacket);
202 
203 	if (usb_endpoint_dir_in(desc)) {
204 		ep->is_in = 1;
205 		ept_cfg |= USBA_EPT_DIR_IN;
206 	}
207 
208 	switch (usb_endpoint_type(desc)) {
209 	case USB_ENDPOINT_XFER_CONTROL:
210 		ept_cfg |= USBA_BF(EPT_TYPE, USBA_EPT_TYPE_CONTROL);
211 		ept_cfg |= USBA_BF(BK_NUMBER, USBA_BK_NUMBER_ONE);
212 		break;
213 	case USB_ENDPOINT_XFER_ISOC:
214 		if (!ep->can_isoc) {
215 			DBG(DBG_ERR, "ep_enable: %s is not isoc capable\n",
216 			    ep->ep.name);
217 			return -EINVAL;
218 		}
219 
220 		/*
221 		 * Bits 11:12 specify number of _additional_
222 		 * transactions per microframe.
223 		 */
224 		nr_trans = ((usb_endpoint_maxp(desc) >> 11) & 3) + 1;
225 		if (nr_trans > 3)
226 			return -EINVAL;
227 
228 		ep->is_isoc = 1;
229 		ept_cfg |= USBA_BF(EPT_TYPE, USBA_EPT_TYPE_ISO);
230 
231 		/*
232 		 * Do triple-buffering on high-bandwidth iso endpoints.
233 		 */
234 		if (nr_trans > 1 && ep->nr_banks == 3)
235 			ept_cfg |= USBA_BF(BK_NUMBER, USBA_BK_NUMBER_TRIPLE);
236 		else
237 			ept_cfg |= USBA_BF(BK_NUMBER, USBA_BK_NUMBER_DOUBLE);
238 		ept_cfg |= USBA_BF(NB_TRANS, nr_trans);
239 		break;
240 	case USB_ENDPOINT_XFER_BULK:
241 		ept_cfg |= USBA_BF(EPT_TYPE, USBA_EPT_TYPE_BULK);
242 		ept_cfg |= USBA_BF(BK_NUMBER, USBA_BK_NUMBER_ONE);
243 		break;
244 	case USB_ENDPOINT_XFER_INT:
245 		ept_cfg |= USBA_BF(EPT_TYPE, USBA_EPT_TYPE_INT);
246 		ept_cfg |= USBA_BF(BK_NUMBER, USBA_BK_NUMBER_ONE);
247 		break;
248 	}
249 
250 	spin_lock_irqsave(&ep->udc->lock, flags);
251 
252 	ep->desc = desc;
253 	ep->ep.maxpacket = maxpacket;
254 
255 	usba_ep_writel(ep, CFG, ept_cfg);
256 	usba_ep_writel(ep, CTL_ENB, USBA_EPT_ENABLE);
257 
258 	usba_writel(udc, INT_ENB,
259 		    (usba_readl(udc, INT_ENB)
260 		     | USBA_BF(EPT_INT, 1 << ep->index)));
261 
262 	spin_unlock_irqrestore(&udc->lock, flags);
263 
264 	DBG(DBG_HW, "EPT_CFG%d after init: %#08lx\n", ep->index,
265 	    (unsigned long)usba_ep_readl(ep, CFG));
266 	DBG(DBG_HW, "INT_ENB after init: %#08lx\n",
267 	    (unsigned long)usba_readl(udc, INT_ENB));
268 
269 	return 0;
270 }
271 
272 static int usba_ep_disable(struct usb_ep *_ep)
273 {
274 	struct usba_ep *ep = to_usba_ep(_ep);
275 	struct usba_udc *udc = ep->udc;
276 	LIST_HEAD(req_list);
277 	unsigned long flags = 0;
278 
279 	DBG(DBG_GADGET, "ep_disable: %s\n", ep->ep.name);
280 
281 	spin_lock_irqsave(&udc->lock, flags);
282 
283 	if (!ep->desc) {
284 		spin_unlock_irqrestore(&udc->lock, flags);
285 		/* REVISIT because this driver disables endpoints in
286 		 * reset_all_endpoints() before calling disconnect(),
287 		 * most gadget drivers would trigger this non-error ...
288 		 */
289 		if (udc->gadget.speed != USB_SPEED_UNKNOWN)
290 			DBG(DBG_ERR, "ep_disable: %s not enabled\n",
291 			    ep->ep.name);
292 		return -EINVAL;
293 	}
294 	ep->desc = NULL;
295 
296 	list_splice_init(&ep->queue, &req_list);
297 	usba_ep_writel(ep, CFG, 0);
298 	usba_ep_writel(ep, CTL_DIS, USBA_EPT_ENABLE);
299 	usba_writel(udc, INT_ENB,
300 		    usba_readl(udc, INT_ENB) &
301 		    ~USBA_BF(EPT_INT, 1 << ep->index));
302 
303 	request_complete_list(ep, &req_list, -ESHUTDOWN);
304 
305 	spin_unlock_irqrestore(&udc->lock, flags);
306 
307 	return 0;
308 }
309 
310 static struct usb_request *
311 usba_ep_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
312 {
313 	struct usba_request *req;
314 
315 	DBG(DBG_GADGET, "ep_alloc_request: %p, 0x%x\n", _ep, gfp_flags);
316 
317 	req = calloc(1, sizeof(struct usba_request));
318 	if (!req)
319 		return NULL;
320 
321 	INIT_LIST_HEAD(&req->queue);
322 
323 	return &req->req;
324 }
325 
326 static void
327 usba_ep_free_request(struct usb_ep *_ep, struct usb_request *_req)
328 {
329 	struct usba_request *req = to_usba_req(_req);
330 
331 	DBG(DBG_GADGET, "ep_free_request: %p, %p\n", _ep, _req);
332 
333 	free(req);
334 }
335 
336 static int
337 usba_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
338 {
339 	struct usba_request *req = to_usba_req(_req);
340 	struct usba_ep *ep = to_usba_ep(_ep);
341 	struct usba_udc *udc = ep->udc;
342 	unsigned long flags = 0;
343 	int ret;
344 
345 	DBG(DBG_GADGET | DBG_QUEUE | DBG_REQ, "%s: queue req %p, len %u\n",
346 	    ep->ep.name, req, _req->length);
347 
348 	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN ||
349 	    !ep->desc)
350 		return -ESHUTDOWN;
351 
352 	req->submitted = 0;
353 	req->using_dma = 0;
354 	req->last_transaction = 0;
355 
356 	_req->status = -EINPROGRESS;
357 	_req->actual = 0;
358 
359 	/* May have received a reset since last time we checked */
360 	ret = -ESHUTDOWN;
361 	spin_lock_irqsave(&udc->lock, flags);
362 	if (ep->desc) {
363 		list_add_tail(&req->queue, &ep->queue);
364 
365 		if ((!ep_is_control(ep) && ep->is_in) ||
366 		    (ep_is_control(ep) && (ep->state == DATA_STAGE_IN ||
367 		    ep->state == STATUS_STAGE_IN)))
368 			usba_ep_writel(ep, CTL_ENB, USBA_TX_PK_RDY);
369 		else
370 			usba_ep_writel(ep, CTL_ENB, USBA_RX_BK_RDY);
371 
372 		ret = 0;
373 	}
374 	spin_unlock_irqrestore(&udc->lock, flags);
375 
376 	return ret;
377 }
378 
379 static int usba_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
380 {
381 	struct usba_ep *ep = to_usba_ep(_ep);
382 	struct usba_request *req = to_usba_req(_req);
383 
384 	DBG(DBG_GADGET | DBG_QUEUE, "ep_dequeue: %s, req %p\n",
385 	    ep->ep.name, req);
386 
387 	/*
388 	 * Errors should stop the queue from advancing until the
389 	 * completion function returns.
390 	 */
391 	list_del_init(&req->queue);
392 
393 	request_complete(ep, req, -ECONNRESET);
394 
395 	/* Process the next request if any */
396 	submit_next_request(ep);
397 
398 	return 0;
399 }
400 
401 static int usba_ep_set_halt(struct usb_ep *_ep, int value)
402 {
403 	struct usba_ep *ep = to_usba_ep(_ep);
404 	unsigned long flags = 0;
405 	int ret = 0;
406 
407 	DBG(DBG_GADGET, "endpoint %s: %s HALT\n", ep->ep.name,
408 	    value ? "set" : "clear");
409 
410 	if (!ep->desc) {
411 		DBG(DBG_ERR, "Attempted to halt uninitialized ep %s\n",
412 		    ep->ep.name);
413 		return -ENODEV;
414 	}
415 
416 	if (ep->is_isoc) {
417 		DBG(DBG_ERR, "Attempted to halt isochronous ep %s\n",
418 		    ep->ep.name);
419 		return -ENOTTY;
420 	}
421 
422 	spin_lock_irqsave(&udc->lock, flags);
423 
424 	/*
425 	 * We can't halt IN endpoints while there are still data to be
426 	 * transferred
427 	 */
428 	if (!list_empty(&ep->queue) ||
429 	    ((value && ep->is_in && (usba_ep_readl(ep, STA) &
430 	    USBA_BF(BUSY_BANKS, -1L))))) {
431 		ret = -EAGAIN;
432 	} else {
433 		if (value)
434 			usba_ep_writel(ep, SET_STA, USBA_FORCE_STALL);
435 		else
436 			usba_ep_writel(ep, CLR_STA,
437 				       USBA_FORCE_STALL | USBA_TOGGLE_CLR);
438 		usba_ep_readl(ep, STA);
439 	}
440 
441 	spin_unlock_irqrestore(&udc->lock, flags);
442 
443 	return ret;
444 }
445 
446 static int usba_ep_fifo_status(struct usb_ep *_ep)
447 {
448 	struct usba_ep *ep = to_usba_ep(_ep);
449 
450 	return USBA_BFEXT(BYTE_COUNT, usba_ep_readl(ep, STA));
451 }
452 
453 static void usba_ep_fifo_flush(struct usb_ep *_ep)
454 {
455 	struct usba_ep *ep = to_usba_ep(_ep);
456 	struct usba_udc *udc = ep->udc;
457 
458 	usba_writel(udc, EPT_RST, 1 << ep->index);
459 }
460 
461 static const struct usb_ep_ops usba_ep_ops = {
462 	.enable		= usba_ep_enable,
463 	.disable	= usba_ep_disable,
464 	.alloc_request	= usba_ep_alloc_request,
465 	.free_request	= usba_ep_free_request,
466 	.queue		= usba_ep_queue,
467 	.dequeue	= usba_ep_dequeue,
468 	.set_halt	= usba_ep_set_halt,
469 	.fifo_status	= usba_ep_fifo_status,
470 	.fifo_flush	= usba_ep_fifo_flush,
471 };
472 
473 static int usba_udc_get_frame(struct usb_gadget *gadget)
474 {
475 	struct usba_udc *udc = to_usba_udc(gadget);
476 
477 	return USBA_BFEXT(FRAME_NUMBER, usba_readl(udc, FNUM));
478 }
479 
480 static int usba_udc_wakeup(struct usb_gadget *gadget)
481 {
482 	struct usba_udc *udc = to_usba_udc(gadget);
483 	unsigned long flags = 0;
484 	u32 ctrl;
485 	int ret = -EINVAL;
486 
487 	spin_lock_irqsave(&udc->lock, flags);
488 	if (udc->devstatus & (1 << USB_DEVICE_REMOTE_WAKEUP)) {
489 		ctrl = usba_readl(udc, CTRL);
490 		usba_writel(udc, CTRL, ctrl | USBA_REMOTE_WAKE_UP);
491 		ret = 0;
492 	}
493 	spin_unlock_irqrestore(&udc->lock, flags);
494 
495 	return ret;
496 }
497 
498 static int
499 usba_udc_set_selfpowered(struct usb_gadget *gadget, int is_selfpowered)
500 {
501 	struct usba_udc *udc = to_usba_udc(gadget);
502 	unsigned long flags = 0;
503 
504 	spin_lock_irqsave(&udc->lock, flags);
505 	if (is_selfpowered)
506 		udc->devstatus |= 1 << USB_DEVICE_SELF_POWERED;
507 	else
508 		udc->devstatus &= ~(1 << USB_DEVICE_SELF_POWERED);
509 	spin_unlock_irqrestore(&udc->lock, flags);
510 
511 	return 0;
512 }
513 
514 static const struct usb_gadget_ops usba_udc_ops = {
515 	.get_frame		= usba_udc_get_frame,
516 	.wakeup			= usba_udc_wakeup,
517 	.set_selfpowered	= usba_udc_set_selfpowered,
518 };
519 
520 static struct usb_endpoint_descriptor usba_ep0_desc = {
521 	.bLength = USB_DT_ENDPOINT_SIZE,
522 	.bDescriptorType = USB_DT_ENDPOINT,
523 	.bEndpointAddress = 0,
524 	.bmAttributes = USB_ENDPOINT_XFER_CONTROL,
525 	.wMaxPacketSize = cpu_to_le16(64),
526 	/* FIXME: I have no idea what to put here */
527 	.bInterval = 1,
528 };
529 
530 /*
531  * Called with interrupts disabled and udc->lock held.
532  */
533 static void reset_all_endpoints(struct usba_udc *udc)
534 {
535 	struct usba_ep *ep;
536 	struct usba_request *req, *tmp_req;
537 
538 	usba_writel(udc, EPT_RST, ~0UL);
539 
540 	ep = to_usba_ep(udc->gadget.ep0);
541 	list_for_each_entry_safe(req, tmp_req, &ep->queue, queue) {
542 		list_del_init(&req->queue);
543 		request_complete(ep, req, -ECONNRESET);
544 	}
545 
546 	/* NOTE:  normally, the next call to the gadget driver is in
547 	 * charge of disabling endpoints... usually disconnect().
548 	 * The exception would be entering a high speed test mode.
549 	 *
550 	 * FIXME remove this code ... and retest thoroughly.
551 	 */
552 	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
553 		if (ep->desc) {
554 			spin_unlock(&udc->lock);
555 			usba_ep_disable(&ep->ep);
556 			spin_lock(&udc->lock);
557 		}
558 	}
559 }
560 
561 static struct usba_ep *get_ep_by_addr(struct usba_udc *udc, u16 wIndex)
562 {
563 	struct usba_ep *ep;
564 
565 	if ((wIndex & USB_ENDPOINT_NUMBER_MASK) == 0)
566 		return to_usba_ep(udc->gadget.ep0);
567 
568 	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
569 		u8 bEndpointAddress;
570 
571 		if (!ep->desc)
572 			continue;
573 		bEndpointAddress = ep->desc->bEndpointAddress;
574 		if ((wIndex ^ bEndpointAddress) & USB_DIR_IN)
575 			continue;
576 		if ((bEndpointAddress & USB_ENDPOINT_NUMBER_MASK)
577 				== (wIndex & USB_ENDPOINT_NUMBER_MASK))
578 			return ep;
579 	}
580 
581 	return NULL;
582 }
583 
584 /* Called with interrupts disabled and udc->lock held */
585 static inline void set_protocol_stall(struct usba_udc *udc, struct usba_ep *ep)
586 {
587 	usba_ep_writel(ep, SET_STA, USBA_FORCE_STALL);
588 	ep->state = WAIT_FOR_SETUP;
589 }
590 
591 static inline int is_stalled(struct usba_udc *udc, struct usba_ep *ep)
592 {
593 	if (usba_ep_readl(ep, STA) & USBA_FORCE_STALL)
594 		return 1;
595 	return 0;
596 }
597 
598 static inline void set_address(struct usba_udc *udc, unsigned int addr)
599 {
600 	u32 regval;
601 
602 	DBG(DBG_BUS, "setting address %u...\n", addr);
603 	regval = usba_readl(udc, CTRL);
604 	regval = USBA_BFINS(DEV_ADDR, addr, regval);
605 	usba_writel(udc, CTRL, regval);
606 }
607 
608 static int do_test_mode(struct usba_udc *udc)
609 {
610 	static const char test_packet_buffer[] = {
611 		/* JKJKJKJK * 9 */
612 		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
613 		/* JJKKJJKK * 8 */
614 		0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
615 		/* JJKKJJKK * 8 */
616 		0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE,
617 		/* JJJJJJJKKKKKKK * 8 */
618 		0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
619 		0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
620 		/* JJJJJJJK * 8 */
621 		0x7F, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB, 0xFD,
622 		/* {JKKKKKKK * 10}, JK */
623 		0xFC, 0x7E, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB, 0xFD, 0x7E
624 	};
625 	struct usba_ep *ep;
626 	int test_mode;
627 
628 	test_mode = udc->test_mode;
629 
630 	/* Start from a clean slate */
631 	reset_all_endpoints(udc);
632 
633 	switch (test_mode) {
634 	case 0x0100:
635 		/* Test_J */
636 		usba_writel(udc, TST, USBA_TST_J_MODE);
637 		DBG(DBG_ALL, "Entering Test_J mode...\n");
638 		break;
639 	case 0x0200:
640 		/* Test_K */
641 		usba_writel(udc, TST, USBA_TST_K_MODE);
642 		DBG(DBG_ALL, "Entering Test_K mode...\n");
643 		break;
644 	case 0x0300:
645 		/*
646 		 * Test_SE0_NAK: Force high-speed mode and set up ep0
647 		 * for Bulk IN transfers
648 		 */
649 		ep = &udc->usba_ep[0];
650 		usba_writel(udc, TST,
651 			    USBA_BF(SPEED_CFG, USBA_SPEED_CFG_FORCE_HIGH));
652 		usba_ep_writel(ep, CFG,
653 			       USBA_BF(EPT_SIZE, USBA_EPT_SIZE_64)
654 			       | USBA_EPT_DIR_IN
655 			       | USBA_BF(EPT_TYPE, USBA_EPT_TYPE_BULK)
656 			       | USBA_BF(BK_NUMBER, 1));
657 		if (!(usba_ep_readl(ep, CFG) & USBA_EPT_MAPPED)) {
658 			set_protocol_stall(udc, ep);
659 			DBG(DBG_ALL, "Test_SE0_NAK: ep0 not mapped\n");
660 		} else {
661 			usba_ep_writel(ep, CTL_ENB, USBA_EPT_ENABLE);
662 			DBG(DBG_ALL, "Entering Test_SE0_NAK mode...\n");
663 		}
664 		break;
665 	case 0x0400:
666 		/* Test_Packet */
667 		ep = &udc->usba_ep[0];
668 		usba_ep_writel(ep, CFG,
669 			       USBA_BF(EPT_SIZE, USBA_EPT_SIZE_64)
670 			       | USBA_EPT_DIR_IN
671 			       | USBA_BF(EPT_TYPE, USBA_EPT_TYPE_BULK)
672 			       | USBA_BF(BK_NUMBER, 1));
673 		if (!(usba_ep_readl(ep, CFG) & USBA_EPT_MAPPED)) {
674 			set_protocol_stall(udc, ep);
675 			DBG(DBG_ALL, "Test_Packet: ep0 not mapped\n");
676 		} else {
677 			usba_ep_writel(ep, CTL_ENB, USBA_EPT_ENABLE);
678 			usba_writel(udc, TST, USBA_TST_PKT_MODE);
679 			memcpy(ep->fifo, test_packet_buffer,
680 			       sizeof(test_packet_buffer));
681 			usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
682 			DBG(DBG_ALL, "Entering Test_Packet mode...\n");
683 		}
684 		break;
685 	default:
686 		DBG(DBG_ERR, "Invalid test mode: 0x%04x\n", test_mode);
687 		return -EINVAL;
688 	}
689 
690 	return 0;
691 }
692 
693 /* Avoid overly long expressions */
694 static inline bool feature_is_dev_remote_wakeup(struct usb_ctrlrequest *crq)
695 {
696 	if (crq->wValue == cpu_to_le16(USB_DEVICE_REMOTE_WAKEUP))
697 		return true;
698 	return false;
699 }
700 
701 static inline bool feature_is_dev_test_mode(struct usb_ctrlrequest *crq)
702 {
703 	if (crq->wValue == cpu_to_le16(USB_DEVICE_TEST_MODE))
704 		return true;
705 	return false;
706 }
707 
708 static inline bool feature_is_ep_halt(struct usb_ctrlrequest *crq)
709 {
710 	if (crq->wValue == cpu_to_le16(USB_ENDPOINT_HALT))
711 		return true;
712 	return false;
713 }
714 
715 static int handle_ep0_setup(struct usba_udc *udc, struct usba_ep *ep,
716 		struct usb_ctrlrequest *crq)
717 {
718 	int retval = 0;
719 
720 	switch (crq->bRequest) {
721 	case USB_REQ_GET_STATUS: {
722 		u16 status;
723 
724 		if (crq->bRequestType == (USB_DIR_IN | USB_RECIP_DEVICE)) {
725 			status = cpu_to_le16(udc->devstatus);
726 		} else if (crq->bRequestType
727 				== (USB_DIR_IN | USB_RECIP_INTERFACE)) {
728 			status = cpu_to_le16(0);
729 		} else if (crq->bRequestType
730 				== (USB_DIR_IN | USB_RECIP_ENDPOINT)) {
731 			struct usba_ep *target;
732 
733 			target = get_ep_by_addr(udc, le16_to_cpu(crq->wIndex));
734 			if (!target)
735 				goto stall;
736 
737 			status = 0;
738 			if (is_stalled(udc, target))
739 				status |= cpu_to_le16(1);
740 		} else {
741 			goto delegate;
742 		}
743 
744 		/* Write directly to the FIFO. No queueing is done. */
745 		if (crq->wLength != cpu_to_le16(sizeof(status)))
746 			goto stall;
747 		ep->state = DATA_STAGE_IN;
748 		__raw_writew(status, ep->fifo);
749 		usba_ep_writel(ep, SET_STA, USBA_TX_PK_RDY);
750 		break;
751 	}
752 
753 	case USB_REQ_CLEAR_FEATURE: {
754 		if (crq->bRequestType == USB_RECIP_DEVICE) {
755 			if (feature_is_dev_remote_wakeup(crq))
756 				udc->devstatus
757 					&= ~(1 << USB_DEVICE_REMOTE_WAKEUP);
758 			else
759 				/* Can't CLEAR_FEATURE TEST_MODE */
760 				goto stall;
761 		} else if (crq->bRequestType == USB_RECIP_ENDPOINT) {
762 			struct usba_ep *target;
763 
764 			if (crq->wLength != cpu_to_le16(0) ||
765 			    !feature_is_ep_halt(crq))
766 				goto stall;
767 			target = get_ep_by_addr(udc, le16_to_cpu(crq->wIndex));
768 			if (!target)
769 				goto stall;
770 
771 			usba_ep_writel(target, CLR_STA, USBA_FORCE_STALL);
772 			if (target->index != 0)
773 				usba_ep_writel(target, CLR_STA,
774 					       USBA_TOGGLE_CLR);
775 		} else {
776 			goto delegate;
777 		}
778 
779 		send_status(udc, ep);
780 		break;
781 	}
782 
783 	case USB_REQ_SET_FEATURE: {
784 		if (crq->bRequestType == USB_RECIP_DEVICE) {
785 			if (feature_is_dev_test_mode(crq)) {
786 				send_status(udc, ep);
787 				ep->state = STATUS_STAGE_TEST;
788 				udc->test_mode = le16_to_cpu(crq->wIndex);
789 				return 0;
790 			} else if (feature_is_dev_remote_wakeup(crq)) {
791 				udc->devstatus |= 1 << USB_DEVICE_REMOTE_WAKEUP;
792 			} else {
793 				goto stall;
794 			}
795 		} else if (crq->bRequestType == USB_RECIP_ENDPOINT) {
796 			struct usba_ep *target;
797 
798 			if (crq->wLength != cpu_to_le16(0) ||
799 			    !feature_is_ep_halt(crq))
800 				goto stall;
801 
802 			target = get_ep_by_addr(udc, le16_to_cpu(crq->wIndex));
803 			if (!target)
804 				goto stall;
805 
806 			usba_ep_writel(target, SET_STA, USBA_FORCE_STALL);
807 		} else {
808 			goto delegate;
809 		}
810 
811 		send_status(udc, ep);
812 		break;
813 	}
814 
815 	case USB_REQ_SET_ADDRESS:
816 		if (crq->bRequestType != (USB_DIR_OUT | USB_RECIP_DEVICE))
817 			goto delegate;
818 
819 		set_address(udc, le16_to_cpu(crq->wValue));
820 		send_status(udc, ep);
821 		ep->state = STATUS_STAGE_ADDR;
822 		break;
823 
824 	default:
825 delegate:
826 		spin_unlock(&udc->lock);
827 		retval = udc->driver->setup(&udc->gadget, crq);
828 		spin_lock(&udc->lock);
829 	}
830 
831 	return retval;
832 
833 stall:
834 	DBG(DBG_ALL, "%s: Invalid setup request: %02x.%02x v%04x i%04x l%d\n",
835 	    ep->ep.name, crq->bRequestType, crq->bRequest,
836 	    le16_to_cpu(crq->wValue), le16_to_cpu(crq->wIndex),
837 	    le16_to_cpu(crq->wLength));
838 	set_protocol_stall(udc, ep);
839 
840 	return -1;
841 }
842 
843 static void usba_control_irq(struct usba_udc *udc, struct usba_ep *ep)
844 {
845 	struct usba_request *req;
846 	u32 epstatus;
847 	u32 epctrl;
848 
849 restart:
850 	epstatus = usba_ep_readl(ep, STA);
851 	epctrl = usba_ep_readl(ep, CTL);
852 
853 	DBG(DBG_INT, "%s [%d]: s/%08x c/%08x\n",
854 	    ep->ep.name, ep->state, epstatus, epctrl);
855 
856 	req = NULL;
857 	if (!list_empty(&ep->queue))
858 		req = list_entry(ep->queue.next,
859 				 struct usba_request, queue);
860 
861 	if ((epctrl & USBA_TX_PK_RDY) && !(epstatus & USBA_TX_PK_RDY)) {
862 		if (req->submitted)
863 			next_fifo_transaction(ep, req);
864 		else
865 			submit_request(ep, req);
866 
867 		if (req->last_transaction) {
868 			usba_ep_writel(ep, CTL_DIS, USBA_TX_PK_RDY);
869 			usba_ep_writel(ep, CTL_ENB, USBA_TX_COMPLETE);
870 		}
871 		goto restart;
872 	}
873 	if ((epstatus & epctrl) & USBA_TX_COMPLETE) {
874 		usba_ep_writel(ep, CLR_STA, USBA_TX_COMPLETE);
875 
876 		switch (ep->state) {
877 		case DATA_STAGE_IN:
878 			usba_ep_writel(ep, CTL_ENB, USBA_RX_BK_RDY);
879 			usba_ep_writel(ep, CTL_DIS, USBA_TX_COMPLETE);
880 			ep->state = STATUS_STAGE_OUT;
881 			break;
882 		case STATUS_STAGE_ADDR:
883 			/* Activate our new address */
884 			usba_writel(udc, CTRL, (usba_readl(udc, CTRL)
885 						| USBA_FADDR_EN));
886 			usba_ep_writel(ep, CTL_DIS, USBA_TX_COMPLETE);
887 			ep->state = WAIT_FOR_SETUP;
888 			break;
889 		case STATUS_STAGE_IN:
890 			if (req) {
891 				list_del_init(&req->queue);
892 				request_complete(ep, req, 0);
893 				submit_next_request(ep);
894 			}
895 			usba_ep_writel(ep, CTL_DIS, USBA_TX_COMPLETE);
896 			ep->state = WAIT_FOR_SETUP;
897 			break;
898 		case STATUS_STAGE_TEST:
899 			usba_ep_writel(ep, CTL_DIS, USBA_TX_COMPLETE);
900 			ep->state = WAIT_FOR_SETUP;
901 			if (do_test_mode(udc))
902 				set_protocol_stall(udc, ep);
903 			break;
904 		default:
905 			DBG(DBG_ALL, "%s: TXCOMP: Invalid endpoint state %d\n",
906 			    ep->ep.name, ep->state);
907 			set_protocol_stall(udc, ep);
908 			break;
909 		}
910 
911 		goto restart;
912 	}
913 	if ((epstatus & epctrl) & USBA_RX_BK_RDY) {
914 		switch (ep->state) {
915 		case STATUS_STAGE_OUT:
916 			usba_ep_writel(ep, CLR_STA, USBA_RX_BK_RDY);
917 			usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
918 
919 			if (req) {
920 				list_del_init(&req->queue);
921 				request_complete(ep, req, 0);
922 			}
923 			ep->state = WAIT_FOR_SETUP;
924 			break;
925 
926 		case DATA_STAGE_OUT:
927 			receive_data(ep);
928 			break;
929 
930 		default:
931 			usba_ep_writel(ep, CLR_STA, USBA_RX_BK_RDY);
932 			usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
933 			DBG(DBG_ALL, "%s: RXRDY: Invalid endpoint state %d\n",
934 			    ep->ep.name, ep->state);
935 			set_protocol_stall(udc, ep);
936 			break;
937 		}
938 
939 		goto restart;
940 	}
941 	if (epstatus & USBA_RX_SETUP) {
942 		union {
943 			struct usb_ctrlrequest crq;
944 			unsigned long data[2];
945 		} crq;
946 		unsigned int pkt_len;
947 		int ret;
948 
949 		if (ep->state != WAIT_FOR_SETUP) {
950 			/*
951 			 * Didn't expect a SETUP packet at this
952 			 * point. Clean up any pending requests (which
953 			 * may be successful).
954 			 */
955 			int status = -EPROTO;
956 
957 			/*
958 			 * RXRDY and TXCOMP are dropped when SETUP
959 			 * packets arrive.  Just pretend we received
960 			 * the status packet.
961 			 */
962 			if (ep->state == STATUS_STAGE_OUT ||
963 			    ep->state == STATUS_STAGE_IN) {
964 				usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
965 				status = 0;
966 			}
967 
968 			if (req) {
969 				list_del_init(&req->queue);
970 				request_complete(ep, req, status);
971 			}
972 		}
973 
974 		pkt_len = USBA_BFEXT(BYTE_COUNT, usba_ep_readl(ep, STA));
975 		DBG(DBG_HW, "Packet length: %u\n", pkt_len);
976 		if (pkt_len != sizeof(crq)) {
977 			DBG(DBG_ALL, "udc: Invalid length %u (expected %zu)\n",
978 			    pkt_len, sizeof(crq));
979 			set_protocol_stall(udc, ep);
980 			return;
981 		}
982 
983 		DBG(DBG_FIFO, "Copying ctrl request from 0x%p:\n", ep->fifo);
984 		memcpy(crq.data, ep->fifo, sizeof(crq));
985 
986 		/* Free up one bank in the FIFO so that we can
987 		 * generate or receive a reply right away. */
988 		usba_ep_writel(ep, CLR_STA, USBA_RX_SETUP);
989 
990 		if (crq.crq.bRequestType & USB_DIR_IN) {
991 			/*
992 			 * The USB 2.0 spec states that "if wLength is
993 			 * zero, there is no data transfer phase."
994 			 * However, testusb #14 seems to actually
995 			 * expect a data phase even if wLength = 0...
996 			 */
997 			ep->state = DATA_STAGE_IN;
998 		} else {
999 			if (crq.crq.wLength != cpu_to_le16(0))
1000 				ep->state = DATA_STAGE_OUT;
1001 			else
1002 				ep->state = STATUS_STAGE_IN;
1003 		}
1004 
1005 		ret = -1;
1006 		if (ep->index == 0) {
1007 			ret = handle_ep0_setup(udc, ep, &crq.crq);
1008 		} else {
1009 			spin_unlock(&udc->lock);
1010 			ret = udc->driver->setup(&udc->gadget, &crq.crq);
1011 			spin_lock(&udc->lock);
1012 		}
1013 
1014 		DBG(DBG_BUS, "req %02x.%02x, length %d, state %d, ret %d\n",
1015 		    crq.crq.bRequestType, crq.crq.bRequest,
1016 		    le16_to_cpu(crq.crq.wLength), ep->state, ret);
1017 
1018 		if (ret < 0) {
1019 			/* Let the host know that we failed */
1020 			set_protocol_stall(udc, ep);
1021 		}
1022 	}
1023 }
1024 
1025 static void usba_ep_irq(struct usba_udc *udc, struct usba_ep *ep)
1026 {
1027 	struct usba_request *req;
1028 	u32 epstatus;
1029 	u32 epctrl;
1030 
1031 	epstatus = usba_ep_readl(ep, STA);
1032 	epctrl = usba_ep_readl(ep, CTL);
1033 
1034 	DBG(DBG_INT, "%s: interrupt, status: 0x%08x\n", ep->ep.name, epstatus);
1035 
1036 	while ((epctrl & USBA_TX_PK_RDY) && !(epstatus & USBA_TX_PK_RDY)) {
1037 		DBG(DBG_BUS, "%s: TX PK ready\n", ep->ep.name);
1038 
1039 		if (list_empty(&ep->queue)) {
1040 			DBG(DBG_INT, "ep_irq: queue empty\n");
1041 			usba_ep_writel(ep, CTL_DIS, USBA_TX_PK_RDY);
1042 			return;
1043 		}
1044 
1045 		req = list_entry(ep->queue.next, struct usba_request, queue);
1046 
1047 		if (req->submitted)
1048 			next_fifo_transaction(ep, req);
1049 		else
1050 			submit_request(ep, req);
1051 
1052 		if (req->last_transaction) {
1053 			list_del_init(&req->queue);
1054 			submit_next_request(ep);
1055 			request_complete(ep, req, 0);
1056 		}
1057 
1058 		epstatus = usba_ep_readl(ep, STA);
1059 		epctrl = usba_ep_readl(ep, CTL);
1060 	}
1061 
1062 	if ((epstatus & epctrl) & USBA_RX_BK_RDY) {
1063 		DBG(DBG_BUS, "%s: RX data ready\n", ep->ep.name);
1064 		receive_data(ep);
1065 	}
1066 }
1067 
1068 static int usba_udc_irq(struct usba_udc *udc)
1069 {
1070 	u32 status, ep_status;
1071 
1072 	spin_lock(&udc->lock);
1073 
1074 	status = usba_readl(udc, INT_STA);
1075 	DBG(DBG_INT, "irq, status=%#08x\n", status);
1076 
1077 	if (status & USBA_DET_SUSPEND) {
1078 		usba_writel(udc, INT_CLR, USBA_DET_SUSPEND);
1079 		DBG(DBG_BUS, "Suspend detected\n");
1080 		if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
1081 		    udc->driver && udc->driver->suspend) {
1082 			spin_unlock(&udc->lock);
1083 			udc->driver->suspend(&udc->gadget);
1084 			spin_lock(&udc->lock);
1085 		}
1086 	}
1087 
1088 	if (status & USBA_WAKE_UP) {
1089 		usba_writel(udc, INT_CLR, USBA_WAKE_UP);
1090 		DBG(DBG_BUS, "Wake Up CPU detected\n");
1091 	}
1092 
1093 	if (status & USBA_END_OF_RESUME) {
1094 		usba_writel(udc, INT_CLR, USBA_END_OF_RESUME);
1095 		DBG(DBG_BUS, "Resume detected\n");
1096 		if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
1097 		    udc->driver && udc->driver->resume) {
1098 			spin_unlock(&udc->lock);
1099 			udc->driver->resume(&udc->gadget);
1100 			spin_lock(&udc->lock);
1101 		}
1102 	}
1103 
1104 	ep_status = USBA_BFEXT(EPT_INT, status);
1105 	if (ep_status) {
1106 		int i;
1107 
1108 		for (i = 0; i < USBA_NR_ENDPOINTS; i++)
1109 			if (ep_status & (1 << i)) {
1110 				if (ep_is_control(&udc->usba_ep[i]))
1111 					usba_control_irq(udc, &udc->usba_ep[i]);
1112 				else
1113 					usba_ep_irq(udc, &udc->usba_ep[i]);
1114 			}
1115 	}
1116 
1117 	if (status & USBA_END_OF_RESET) {
1118 		struct usba_ep *ep0;
1119 
1120 		usba_writel(udc, INT_CLR, USBA_END_OF_RESET);
1121 		reset_all_endpoints(udc);
1122 
1123 		if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
1124 		    udc->driver->disconnect) {
1125 			udc->gadget.speed = USB_SPEED_UNKNOWN;
1126 			spin_unlock(&udc->lock);
1127 			udc->driver->disconnect(&udc->gadget);
1128 			spin_lock(&udc->lock);
1129 		}
1130 
1131 		if (status & USBA_HIGH_SPEED)
1132 			udc->gadget.speed = USB_SPEED_HIGH;
1133 		else
1134 			udc->gadget.speed = USB_SPEED_FULL;
1135 
1136 		ep0 = &udc->usba_ep[0];
1137 		ep0->desc = &usba_ep0_desc;
1138 		ep0->state = WAIT_FOR_SETUP;
1139 		usba_ep_writel(ep0, CFG,
1140 			       (USBA_BF(EPT_SIZE, EP0_EPT_SIZE)
1141 				| USBA_BF(EPT_TYPE, USBA_EPT_TYPE_CONTROL)
1142 				| USBA_BF(BK_NUMBER, USBA_BK_NUMBER_ONE)));
1143 		usba_ep_writel(ep0, CTL_ENB,
1144 			       USBA_EPT_ENABLE | USBA_RX_SETUP);
1145 		usba_writel(udc, INT_ENB,
1146 			    (usba_readl(udc, INT_ENB)
1147 			     | USBA_BF(EPT_INT, 1)
1148 			     | USBA_DET_SUSPEND
1149 			     | USBA_END_OF_RESUME));
1150 
1151 		/*
1152 		 * Unclear why we hit this irregularly, e.g. in usbtest,
1153 		 * but it's clearly harmless...
1154 		 */
1155 		if (!(usba_ep_readl(ep0, CFG) & USBA_EPT_MAPPED))
1156 			DBG(DBG_ALL, "ODD: EP0 configuration is invalid!\n");
1157 	}
1158 
1159 	spin_unlock(&udc->lock);
1160 
1161 	return 0;
1162 }
1163 
1164 static int atmel_usba_start(struct usba_udc *udc)
1165 {
1166 	udc->devstatus = 1 << USB_DEVICE_SELF_POWERED;
1167 
1168 	udc->vbus_prev = 0;
1169 
1170 	/* If Vbus is present, enable the controller and wait for reset */
1171 	if (vbus_is_present(udc) && udc->vbus_prev == 0) {
1172 		usba_writel(udc, CTRL, USBA_ENABLE_MASK);
1173 		usba_writel(udc, INT_ENB, USBA_END_OF_RESET);
1174 	}
1175 
1176 	return 0;
1177 }
1178 
1179 static int atmel_usba_stop(struct usba_udc *udc)
1180 {
1181 	udc->gadget.speed = USB_SPEED_UNKNOWN;
1182 	reset_all_endpoints(udc);
1183 
1184 	/* This will also disable the DP pullup */
1185 	usba_writel(udc, CTRL, USBA_DISABLE_MASK);
1186 
1187 	return 0;
1188 }
1189 
1190 static struct usba_udc controller = {
1191 	.regs = (unsigned *)ATMEL_BASE_UDPHS,
1192 	.fifo = (unsigned *)ATMEL_BASE_UDPHS_FIFO,
1193 	.gadget = {
1194 		.ops		= &usba_udc_ops,
1195 		.ep_list	= LIST_HEAD_INIT(controller.gadget.ep_list),
1196 		.speed		= USB_SPEED_HIGH,
1197 		.is_dualspeed	= 1,
1198 		.name		= "atmel_usba_udc",
1199 	},
1200 };
1201 
1202 int usb_gadget_handle_interrupts(int index)
1203 {
1204 	struct usba_udc *udc = &controller;
1205 
1206 	return usba_udc_irq(udc);
1207 }
1208 
1209 
1210 int usb_gadget_register_driver(struct usb_gadget_driver *driver)
1211 {
1212 	struct usba_udc *udc = &controller;
1213 	int ret;
1214 
1215 	if (!driver || !driver->bind || !driver->setup) {
1216 		printf("bad paramter\n");
1217 		return -EINVAL;
1218 	}
1219 
1220 	if (udc->driver) {
1221 		printf("UDC already has a gadget driver\n");
1222 		return -EBUSY;
1223 	}
1224 
1225 	atmel_usba_start(udc);
1226 
1227 	udc->driver = driver;
1228 
1229 	ret = driver->bind(&udc->gadget);
1230 	if (ret) {
1231 		error("driver->bind() returned %d\n", ret);
1232 		udc->driver = NULL;
1233 	}
1234 
1235 	return ret;
1236 }
1237 
1238 int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
1239 {
1240 	struct usba_udc *udc = &controller;
1241 
1242 	if (!driver || !driver->unbind || !driver->disconnect) {
1243 		error("bad paramter\n");
1244 		return -EINVAL;
1245 	}
1246 
1247 	driver->disconnect(&udc->gadget);
1248 	driver->unbind(&udc->gadget);
1249 	udc->driver = NULL;
1250 
1251 	atmel_usba_stop(udc);
1252 
1253 	return 0;
1254 }
1255 
1256 static struct usba_ep *usba_udc_pdata(struct usba_platform_data *pdata,
1257 				      struct usba_udc *udc)
1258 {
1259 	struct usba_ep *eps;
1260 	int i;
1261 
1262 	eps = malloc(sizeof(struct usba_ep) * pdata->num_ep);
1263 	if (!eps) {
1264 		error("failed to alloc eps\n");
1265 		return NULL;
1266 	}
1267 
1268 	udc->gadget.ep0 = &eps[0].ep;
1269 
1270 	INIT_LIST_HEAD(&udc->gadget.ep_list);
1271 	INIT_LIST_HEAD(&eps[0].ep.ep_list);
1272 
1273 	for (i = 0; i < pdata->num_ep; i++) {
1274 		struct usba_ep *ep = &eps[i];
1275 
1276 		ep->ep_regs = udc->regs + USBA_EPT_BASE(i);
1277 		ep->dma_regs = udc->regs + USBA_DMA_BASE(i);
1278 		ep->fifo = udc->fifo + USBA_FIFO_BASE(i);
1279 		ep->ep.ops = &usba_ep_ops;
1280 		ep->ep.name = pdata->ep[i].name;
1281 		ep->ep.maxpacket = pdata->ep[i].fifo_size;
1282 		ep->fifo_size = ep->ep.maxpacket;
1283 		ep->udc = udc;
1284 		INIT_LIST_HEAD(&ep->queue);
1285 		ep->nr_banks = pdata->ep[i].nr_banks;
1286 		ep->index = pdata->ep[i].index;
1287 		ep->can_dma = pdata->ep[i].can_dma;
1288 		ep->can_isoc = pdata->ep[i].can_isoc;
1289 		if (i)
1290 			list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
1291 	};
1292 
1293 	return eps;
1294 }
1295 
1296 int usba_udc_probe(struct usba_platform_data *pdata)
1297 {
1298 	struct usba_udc *udc;
1299 
1300 	udc = &controller;
1301 
1302 	udc->usba_ep = usba_udc_pdata(pdata, udc);
1303 
1304 	return 0;
1305 }
1306