Cleanup of USB drivers for AT32F4 (#12441)
[betaflight.git] / lib / main / AT32F43x / middlewares / usb_drivers / src / usbd_core.c
blob7ad409bfa7d9e9e4a1b0287c346f35016953fb06
1 /**
2 **************************************************************************
3 * @file usbd_core.c
4 * @brief usb device driver
5 **************************************************************************
6 * Copyright notice & Disclaimer
8 * The software Board Support Package (BSP) that is made available to
9 * download from Artery official website is the copyrighted work of Artery.
10 * Artery authorizes customers to use, copy, and distribute the BSP
11 * software and its related documentation for the purpose of design and
12 * development in conjunction with Artery microcontrollers. Use of the
13 * software is governed by this copyright notice and the following disclaimer.
15 * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES,
16 * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS,
17 * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR
18 * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS,
19 * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
20 * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT.
22 **************************************************************************
25 #include "usb_core.h"
26 #include "usbd_core.h"
27 #include "usbd_sdr.h"
29 /** @addtogroup AT32F435_437_middlewares_usbd_drivers
30 * @{
33 /** @defgroup USBD_drivers_core
34 * @brief usb device drivers core
35 * @{
38 /** @defgroup USBD_core_private_functions
39 * @{
42 /**
43 * @brief usb core in transfer complete handler
44 * @param udev: to the structure of usbd_core_type
45 * @param ept_addr: endpoint number
46 * @retval none
48 void usbd_core_in_handler(usbd_core_type *udev, uint8_t ept_addr)
50 /* get endpoint info*/
51 usb_ept_info *ept_info = &udev->ept_in[ept_addr & 0x7F];
53 if(ept_addr == 0)
55 if(udev->ept0_sts == USB_EPT0_DATA_IN)
57 if(ept_info->rem0_len > ept_info->maxpacket)
59 ept_info->rem0_len -= ept_info->maxpacket;
60 usbd_ept_send(udev, 0, ept_info->trans_buf,
61 MIN(ept_info->rem0_len, ept_info->maxpacket));
63 /* endpoint 0 */
64 else if(ept_info->last_len == ept_info->maxpacket
65 && ept_info->ept0_slen >= ept_info->maxpacket
66 && ept_info->ept0_slen < udev->ept0_wlength)
68 ept_info->last_len = 0;
69 usbd_ept_send(udev, 0, 0, 0);
70 usbd_ept_recv(udev, ept_addr, 0, 0);
72 else
75 if(udev->class_handler->ept0_tx_handler != 0 &&
76 udev->conn_state == USB_CONN_STATE_CONFIGURED)
78 udev->class_handler->ept0_tx_handler(udev);
80 usbd_ctrl_recv_status(udev);
85 else if(udev->class_handler->in_handler != 0 &&
86 udev->conn_state == USB_CONN_STATE_CONFIGURED)
88 /* other user define endpoint */
89 udev->class_handler->in_handler(udev, ept_addr);
93 /**
94 * @brief usb core out transfer complete handler
95 * @param udev: to the structure of usbd_core_type
96 * @param ept_addr: endpoint number
97 * @retval none
99 void usbd_core_out_handler(usbd_core_type *udev, uint8_t ept_addr)
101 /* get endpoint info*/
102 usb_ept_info *ept_info = &udev->ept_out[ept_addr & 0x7F];
104 if(ept_addr == 0)
106 /* endpoint 0 */
107 if(udev->ept0_sts == USB_EPT0_DATA_OUT)
109 if(ept_info->rem0_len > ept_info->maxpacket)
111 ept_info->rem0_len -= ept_info->maxpacket;
112 usbd_ept_recv(udev, ept_addr, ept_info->trans_buf,
113 MIN(ept_info->rem0_len, ept_info->maxpacket));
115 else
117 if(udev->class_handler->ept0_rx_handler != 0)
119 udev->class_handler->ept0_rx_handler(udev);
121 usbd_ctrl_send_status(udev);
125 else if(udev->class_handler->out_handler != 0 &&
126 udev->conn_state == USB_CONN_STATE_CONFIGURED)
128 /* other user define endpoint */
129 udev->class_handler->out_handler(udev, ept_addr);
134 * @brief usb core setup transfer complete handler
135 * @param udev: to the structure of usbd_core_type
136 * @param ept_addr: endpoint number
137 * @retval none
139 void usbd_core_setup_handler(usbd_core_type *udev, uint8_t ept_num)
141 UNUSED(ept_num);
142 /* setup parse */
143 usbd_setup_request_parse(&udev->setup, udev->setup_buffer);
145 /* set ept0 status */
146 udev->ept0_sts = USB_EPT0_SETUP;
147 udev->ept0_wlength = udev->setup.wLength;
149 switch(udev->setup.bmRequestType & USB_REQ_RECIPIENT_MASK)
151 case USB_REQ_RECIPIENT_DEVICE:
152 /* recipient device request */
153 usbd_device_request(udev);
154 break;
155 case USB_REQ_RECIPIENT_INTERFACE:
156 /* recipient interface request */
157 usbd_interface_request(udev);
158 break;
159 case USB_REQ_RECIPIENT_ENDPOINT:
160 /* recipient endpoint request */
161 usbd_endpoint_request(udev);
162 break;
163 default:
164 break;
169 * @brief usb control endpoint send data
170 * @param udev: to the structure of usbd_core_type
171 * @param ept_addr: endpoint number
172 * @param buffer: send data buffer
173 * @param len: send data length
174 * @retval none
176 void usbd_ctrl_send(usbd_core_type *udev, uint8_t *buffer, uint16_t len)
178 usb_ept_info *ept_info = &udev->ept_in[0];
180 ept_info->ept0_slen = len;
181 ept_info->rem0_len = len;
182 udev->ept0_sts = USB_EPT0_DATA_IN;
184 usbd_ept_send(udev, 0, buffer, len);
188 * @brief usb control endpoint recv data
189 * @param udev: to the structure of usbd_core_type
190 * @param ept_addr: endpoint number
191 * @param buffer: recv data buffer
192 * @param len: recv data length
193 * @retval none
195 void usbd_ctrl_recv(usbd_core_type *udev, uint8_t *buffer, uint16_t len)
197 usb_ept_info *ept_info = &udev->ept_out[0];
199 ept_info->ept0_slen = len;
200 ept_info->rem0_len = len;
201 udev->ept0_sts = USB_EPT0_DATA_OUT;
203 usbd_ept_recv(udev, 0, buffer, len);
207 * @brief usb control endpoint send in status
208 * @param udev: to the structure of usbd_core_type
209 * @retval none
211 void usbd_ctrl_send_status(usbd_core_type *udev)
213 udev->ept0_sts = USB_EPT0_STATUS_IN;
215 usbd_ept_send(udev, 0, 0, 0);
219 * @brief usb control endpoint send out status
220 * @param udev: to the structure of usbd_core_type
221 * @retval none
223 void usbd_ctrl_recv_status(usbd_core_type *udev)
225 udev->ept0_sts = USB_EPT0_STATUS_OUT;
227 usbd_ept_recv(udev, 0, 0, 0);
231 * @brief clear endpoint stall
232 * @param udev: to the structure of usbd_core_type
233 * @param ept_addr: endpoint number
234 * @retval none
236 void usbd_clear_stall(usbd_core_type *udev, uint8_t ept_addr)
238 usb_ept_info *ept_info;
239 usb_reg_type *usbx = udev->usb_reg;
241 if(ept_addr & 0x80)
243 /* in endpoint */
244 ept_info = &udev->ept_in[ept_addr & 0x7F];
246 else
248 /* out endpoint */
249 ept_info = &udev->ept_out[ept_addr & 0x7F];
251 usb_ept_clear_stall(usbx, ept_info);
252 ept_info->stall = 0;
256 * @brief usb set endpoint to stall status
257 * @param udev: to the structure of usbd_core_type
258 * @param ept_addr: endpoint number
259 * @retval none
261 void usbd_set_stall(usbd_core_type *udev, uint8_t ept_addr)
263 usb_ept_info *ept_info;
264 usb_reg_type *usbx = udev->usb_reg;
266 if(ept_addr & 0x80)
268 /* in endpoint */
269 ept_info = &udev->ept_in[ept_addr & 0x7F];
271 else
273 /* out endpoint */
274 ept_info = &udev->ept_out[ept_addr & 0x7F];
276 usb_ept_stall(usbx, ept_info);
278 ept_info->stall = 1;
282 * @brief un-support device request
283 * @param udev: to the structure of usbd_core_type
284 * @retval none
286 void usbd_ctrl_unsupport(usbd_core_type *udev)
288 /* return stall status */
289 usbd_set_stall(udev, 0x00);
290 usbd_set_stall(udev, 0x80);
294 * @brief get endpoint receive data length
295 * @param udev: to the structure of usbd_core_type
296 * @param ept_addr: endpoint number
297 * @retval data receive len
299 uint32_t usbd_get_recv_len(usbd_core_type *udev, uint8_t ept_addr)
301 usb_ept_info *ept = &udev->ept_out[ept_addr & 0x7F];
302 return ept->trans_len;
306 * @brief usb open endpoint
307 * @param udev: to the structure of usbd_core_type
308 * @param ept_addr: endpoint number
309 * @param ept_type: endpoint type
310 * @param maxpacket: endpoint support max buffer size
311 * @retval none
313 void usbd_ept_open(usbd_core_type *udev, uint8_t ept_addr, uint8_t ept_type, uint16_t maxpacket)
315 usb_reg_type *usbx = udev->usb_reg;
316 usb_ept_info *ept_info;
318 if((ept_addr & 0x80) == 0)
320 /* out endpoint info */
321 ept_info = &udev->ept_out[ept_addr & 0x7F];
322 ept_info->inout = EPT_DIR_OUT;
324 else
326 /* in endpoint info */
327 ept_info = &udev->ept_in[ept_addr & 0x7F];
328 ept_info->inout = EPT_DIR_IN;
331 /* set endpoint maxpacket and type */
332 ept_info->maxpacket = maxpacket;
333 ept_info->trans_type = ept_type;
335 /* open endpoint */
336 usb_ept_open(usbx, ept_info);
340 * @brief usb close endpoint
341 * @param udev: to the structure of usbd_core_type
342 * @param ept_addr: endpoint number
343 * @retval none
345 void usbd_ept_close(usbd_core_type *udev, uint8_t ept_addr)
347 usb_ept_info *ept_info;
348 if(ept_addr & 0x80)
350 /* in endpoint */
351 ept_info = &udev->ept_in[ept_addr & 0x7F];
353 else
355 /* out endpoint */
356 ept_info = &udev->ept_out[ept_addr & 0x7F];
359 /* close endpoint */
360 usb_ept_close(udev->usb_reg, ept_info);
364 * @brief usb device connect to host
365 * @param udev: to the structure of usbd_core_type
366 * @retval none
368 void usbd_connect(usbd_core_type *udev)
370 usb_connect(udev->usb_reg);
374 * @brief usb device disconnect to host
375 * @param udev: to the structure of usbd_core_type
376 * @retval none
378 void usbd_disconnect(usbd_core_type *udev)
380 usb_disconnect(udev->usb_reg);
384 * @brief usb device set device address.
385 * @param udev: to the structure of usbd_core_type
386 * @param address: host assignment address
387 * @retval none
389 void usbd_set_device_addr(usbd_core_type *udev, uint8_t address)
391 usb_set_address(udev->usb_reg, address);
395 * @brief usb endpoint structure initialization
396 * @param udev: to the structure of usbd_core_type
397 * @retval none
399 void usb_ept_default_init(usbd_core_type *udev)
401 uint8_t i_index = 0;
402 /* init in endpoint info structure */
403 for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++)
405 udev->ept_in[i_index].eptn = i_index;
406 udev->ept_in[i_index].ept_address = i_index;
407 udev->ept_in[i_index].inout = EPT_DIR_IN;
408 udev->ept_in[i_index].maxpacket = 0;
409 udev->ept_in[i_index].trans_buf = 0;
410 udev->ept_in[i_index].total_len = 0;
413 /* init out endpoint info structure */
414 for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++)
416 udev->ept_out[i_index].eptn = i_index;
417 udev->ept_out[i_index].ept_address = i_index;
418 udev->ept_out[i_index].inout = EPT_DIR_OUT;
419 udev->ept_out[i_index].maxpacket = 0;
420 udev->ept_out[i_index].trans_buf = 0;
421 udev->ept_out[i_index].total_len = 0;
426 * @brief endpoint send data
427 * @param udev: to the structure of usbd_core_type
428 * @param ept_addr: endpoint number
429 * @param buffer: send data buffer
430 * @param len: send data length
431 * @retval none
433 void usbd_ept_send(usbd_core_type *udev, uint8_t ept_addr, uint8_t *buffer, uint16_t len)
435 /* get endpoint info struct and register */
436 usb_reg_type *usbx = udev->usb_reg;
437 usb_ept_info *ept_info = &udev->ept_in[ept_addr & 0x7F];
438 otg_eptin_type *ept_in = USB_INEPT(usbx, ept_info->eptn);
439 otg_device_type *dev = OTG_DEVICE(usbx);
440 uint32_t pktcnt;
442 /* set send data buffer and length */
443 ept_info->trans_buf = buffer;
444 ept_info->total_len = len;
445 ept_info->trans_len = 0;
447 /* transfer data len is zero */
448 if(ept_info->total_len == 0)
450 ept_in->dieptsiz_bit.pktcnt = 1;
451 ept_in->dieptsiz_bit.xfersize = 0;
453 else
455 if((ept_addr & 0x7F) == 0) // endpoint 0
457 /* endpoint 0 */
458 if(ept_info->total_len > ept_info->maxpacket)
460 ept_info->total_len = ept_info->maxpacket;
463 /* set transfer size */
464 ept_in->dieptsiz_bit.xfersize = ept_info->total_len;
466 /* set packet count */
467 ept_in->dieptsiz_bit.pktcnt = 1;
469 ept_info->last_len = ept_info->total_len;
471 else
473 /* other endpoint */
475 /* packet count */
476 pktcnt = (ept_info->total_len + ept_info->maxpacket - 1) / ept_info->maxpacket;
478 /* set transfer size */
479 ept_in->dieptsiz_bit.xfersize = ept_info->total_len;
481 /* set packet count */
482 ept_in->dieptsiz_bit.pktcnt = pktcnt;
484 if(ept_info->trans_type == EPT_ISO_TYPE)
486 ept_in->dieptsiz_bit.mc = 1;
491 if(ept_info->trans_type != EPT_ISO_TYPE)
493 if(ept_info->total_len > 0)
495 /* set in endpoint tx fifo empty interrupt mask */
496 dev->diepempmsk |= 1 << ept_info->eptn;
500 if(ept_info->trans_type == EPT_ISO_TYPE)
502 if((dev->dsts_bit.soffn & 0x1) == 0)
504 ept_in->diepctl_bit.setd1pid = TRUE;
506 else
508 ept_in->diepctl_bit.setd0pid = TRUE;
512 /* clear endpoint nak */
513 ept_in->diepctl_bit.cnak = TRUE;
515 /* endpoint enable */
516 ept_in->diepctl_bit.eptena = TRUE;
518 if(ept_info->trans_type == EPT_ISO_TYPE)
520 /* write data to fifo */
521 usb_write_packet(usbx, ept_info->trans_buf, ept_info->eptn, ept_info->total_len);
526 * @brief endpoint receive data
527 * @param udev: to the structure of usbd_core_type
528 * @param ept_addr: endpoint number
529 * @param buffer: receive data buffer
530 * @param len: receive data length
531 * @retval none
533 void usbd_ept_recv(usbd_core_type *udev, uint8_t ept_addr, uint8_t *buffer, uint16_t len)
535 /* get endpoint info struct and register */
536 usb_reg_type *usbx = udev->usb_reg;
537 usb_ept_info *ept_info = &udev->ept_out[ept_addr & 0x7F];
538 otg_eptout_type *ept_out = USB_OUTEPT(usbx, ept_info->eptn);
539 otg_device_type *dev = OTG_DEVICE(usbx);
540 uint32_t pktcnt;
542 /* set receive data buffer and length */
543 ept_info->trans_buf = buffer;
544 ept_info->total_len = len;
545 ept_info->trans_len = 0;
547 if((ept_addr & 0x7F) == 0)
549 /* endpoint 0 */
550 ept_info->total_len = ept_info->maxpacket;
553 if(ept_info->total_len == 0 || ((ept_addr & 0x7F) == 0))
555 /* set transfer size */
556 ept_out->doeptsiz_bit.xfersize = ept_info->maxpacket;
558 /* set packet count */
559 ept_out->doeptsiz_bit.pktcnt = 1;
561 else
563 pktcnt = (ept_info->total_len + ept_info->maxpacket - 1) / ept_info->maxpacket;
565 /* set transfer size */
566 ept_out->doeptsiz_bit.xfersize = ept_info->maxpacket * pktcnt;
568 /* set packet count */
569 ept_out->doeptsiz_bit.pktcnt = pktcnt;
572 if(ept_info->trans_type == EPT_ISO_TYPE)
574 if((dev->dsts_bit.soffn & 0x01) == 0)
576 ept_out->doepctl_bit.setd1pid = TRUE;
578 else
580 ept_out->doepctl_bit.setd0pid = TRUE;
584 /* clear endpoint nak */
585 ept_out->doepctl_bit.cnak = TRUE;
587 /* endpoint enable */
588 ept_out->doepctl_bit.eptena = TRUE;
592 * @brief get usb connect state
593 * @param udev: to the structure of usbd_core_type
594 * @retval usb connect state
596 usbd_conn_state usbd_connect_state_get(usbd_core_type *udev)
598 return udev->conn_state;
602 * @brief usb device remote wakeup
603 * @param udev: to the structure of usbd_core_type
604 * @retval none
606 void usbd_remote_wakeup(usbd_core_type *udev)
608 /* check device is in suspend mode */
609 if(usb_suspend_status_get(udev->usb_reg) == 1)
611 /* set connect state */
612 udev->conn_state = udev->old_conn_state;
614 /* open phy clock */
615 usb_open_phy_clk(udev->usb_reg);
617 /* set remote wakeup */
618 usb_remote_wkup_set(udev->usb_reg);
620 /* delay 10 ms */
621 usb_delay_ms(10);
623 /* clear remote wakup */
624 usb_remote_wkup_clear(udev->usb_reg);
629 * @brief usb device enter suspend mode
630 * @param udev: to the structure of usbd_core_type
631 * @retval none
633 void usbd_enter_suspend(usbd_core_type *udev)
635 /* check device is in suspend mode */
636 if(usb_suspend_status_get(udev->usb_reg) == 1)
638 /* stop phy clk */
639 usb_stop_phy_clk(udev->usb_reg);
644 * @brief usb device flush in endpoint fifo
645 * @param udev: to the structure of usbd_core_type
646 * @param ept_num: endpoint number
647 * @retval none
649 void usbd_flush_tx_fifo(usbd_core_type *udev, uint8_t ept_num)
651 /* flush endpoint tx fifo */
652 usb_flush_tx_fifo(udev->usb_reg, ept_num & 0x1F);
656 * @brief usb device endpoint fifo alloc
657 * @param udev: to the structure of usbd_core_type
658 * @retval none
660 void usbd_fifo_alloc(usbd_core_type *udev)
662 usb_reg_type *usbx = udev->usb_reg;
664 if(usbx == OTG1_GLOBAL)
666 /* set receive fifo size */
667 usb_set_rx_fifo(usbx, USBD_RX_SIZE);
669 /* set endpoint0 tx fifo size */
670 usb_set_tx_fifo(usbx, USB_EPT0, USBD_EP0_TX_SIZE);
672 /* set endpoint1 tx fifo size */
673 usb_set_tx_fifo(usbx, USB_EPT1, USBD_EP1_TX_SIZE);
675 /* set endpoint2 tx fifo size */
676 usb_set_tx_fifo(usbx, USB_EPT2, USBD_EP2_TX_SIZE);
678 /* set endpoint3 tx fifo size */
679 usb_set_tx_fifo(usbx, USB_EPT3, USBD_EP3_TX_SIZE);
681 if(USB_EPT_MAX_NUM == 8)
683 /* set endpoint4 tx fifo size */
684 usb_set_tx_fifo(usbx, USB_EPT4, USBD_EP4_TX_SIZE);
686 /* set endpoint5 tx fifo size */
687 usb_set_tx_fifo(usbx, USB_EPT5, USBD_EP5_TX_SIZE);
689 /* set endpoint6 tx fifo size */
690 usb_set_tx_fifo(usbx, USB_EPT6, USBD_EP6_TX_SIZE);
692 /* set endpoint7 tx fifo size */
693 usb_set_tx_fifo(usbx, USB_EPT7, USBD_EP7_TX_SIZE);
696 #ifdef OTG2_GLOBAL
697 if(usbx == OTG2_GLOBAL)
699 /* set receive fifo size */
700 usb_set_rx_fifo(usbx, USBD2_RX_SIZE);
702 /* set endpoint0 tx fifo size */
703 usb_set_tx_fifo(usbx, USB_EPT0, USBD2_EP0_TX_SIZE);
705 /* set endpoint1 tx fifo size */
706 usb_set_tx_fifo(usbx, USB_EPT1, USBD2_EP1_TX_SIZE);
708 /* set endpoint2 tx fifo size */
709 usb_set_tx_fifo(usbx, USB_EPT2, USBD2_EP2_TX_SIZE);
711 /* set endpoint3 tx fifo size */
712 usb_set_tx_fifo(usbx, USB_EPT3, USBD2_EP3_TX_SIZE);
714 if(USB_EPT_MAX_NUM == 8)
716 /* set endpoint4 tx fifo size */
717 usb_set_tx_fifo(usbx, USB_EPT4, USBD2_EP4_TX_SIZE);
719 /* set endpoint5 tx fifo size */
720 usb_set_tx_fifo(usbx, USB_EPT5, USBD2_EP5_TX_SIZE);
722 /* set endpoint6 tx fifo size */
723 usb_set_tx_fifo(usbx, USB_EPT6, USBD2_EP6_TX_SIZE);
725 /* set endpoint7 tx fifo size */
726 usb_set_tx_fifo(usbx, USB_EPT7, USBD2_EP7_TX_SIZE);
729 #endif
733 * @brief usb device core initialization
734 * @param udev: to the structure of usbd_core_type
735 * @param usb_reg: usb otgfs peripheral global register
736 * this parameter can be one of the following values:
737 * OTG1_GLOBAL , OTG2_GLOBAL
738 * @param class_handler: usb class handler
739 * @param desc_handler: device config handler
740 * @param core_id: usb core id number
741 * @retval usb_sts_type
743 usb_sts_type usbd_core_init(usbd_core_type *udev,
744 usb_reg_type *usb_reg,
745 usbd_class_handler *class_handler,
746 usbd_desc_handler *desc_handler,
747 uint8_t core_id)
749 UNUSED(core_id);
750 usb_reg_type *usbx;
751 otg_device_type *dev;
752 otg_eptin_type *ept_in;
753 otg_eptout_type *ept_out;
754 uint32_t i_index;
756 udev->usb_reg = usb_reg;
757 usbx = usb_reg;
758 dev = OTG_DEVICE(usbx);
760 /* set connect state */
761 udev->conn_state = USB_CONN_STATE_DEFAULT;
763 /* device class config */
764 udev->device_addr = 0;
765 udev->class_handler = class_handler;
766 udev->desc_handler = desc_handler;
767 /* set device disconnect */
768 usbd_disconnect(udev);
770 /* set endpoint to default status */
771 usb_ept_default_init(udev);
773 /* disable usb global interrupt */
774 usb_interrupt_disable(usbx);
776 /* init global register */
777 usb_global_init(usbx);
779 /* set device mode */
780 usb_global_set_mode(usbx, OTG_DEVICE_MODE);
782 /* open phy clock */
783 usb_open_phy_clk(udev->usb_reg);
785 /* set periodic frame interval */
786 dev->dcfg_bit.perfrint = DCFG_PERFRINT_80;
788 /* set device speed to full-speed */
789 dev->dcfg_bit.devspd = USB_DCFG_FULL_SPEED;
791 /* flush all tx fifo */
792 usb_flush_tx_fifo(usbx, 16);
794 /* flush share rx fifo */
795 usb_flush_rx_fifo(usbx);
797 /* clear all endpoint interrupt flag and mask */
798 dev->daint = 0xFFFFFFFF;
799 dev->daintmsk = 0;
800 dev->diepmsk = 0;
801 dev->doepmsk = 0;
803 for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++)
805 usbx->dieptxfn[i_index] = 0;
808 /* endpoint fifo alloc */
809 usbd_fifo_alloc(udev);
811 /* disable all in endpoint */
812 for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++)
814 ept_in = USB_INEPT(usbx, i_index);
815 if(ept_in->diepctl_bit.eptena)
817 ept_in->diepctl = 0;
818 ept_in->diepctl_bit.eptdis = TRUE;
819 ept_in->diepctl_bit.snak = TRUE;
821 else
823 ept_in->diepctl = 0;
825 ept_in->dieptsiz = 0;
826 ept_in->diepint = 0xFF;
829 /* disable all out endpoint */
830 for(i_index = 0; i_index < USB_EPT_MAX_NUM; i_index ++)
832 ept_out = USB_OUTEPT(usbx, i_index);
833 if(ept_out->doepctl_bit.eptena)
835 ept_out->doepctl = 0;
836 ept_out->doepctl_bit.eptdis = TRUE;
837 ept_out->doepctl_bit.snak = TRUE;
839 else
841 ept_out->doepctl = 0;
843 ept_out->doeptsiz = 0;
844 ept_out->doepint = 0xFF;
846 dev->diepmsk_bit.txfifoudrmsk = TRUE;
848 /* clear global interrupt and mask */
849 usbx->gintmsk = 0;
850 usbx->gintsts = 0xBFFFFFFF;
852 /* enable global interrupt mask */
853 usbx->gintmsk = USB_OTG_SOF_INT | USB_OTG_RXFLVL_INT |
854 USB_OTG_USBSUSP_INT | USB_OTG_USBRST_INT |
855 USB_OTG_ENUMDONE_INT | USB_OTG_IEPT_INT |
856 USB_OTG_OEPT_INT | USB_OTG_INCOMISOIN_INT |
857 USB_OTG_INCOMPIP_INCOMPISOOUT_INT | USB_OTG_WKUP_INT |
858 USB_OTG_OTGINT_INT;
860 /* usb connect */
861 usbd_connect(udev);
863 /* enable global interrupt */
864 usb_interrupt_enable(usbx);
866 return USB_OK;
871 * @}
875 * @}
879 * @}