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