void USB_Init (void) {
  DWORD val = 0;

  PCONP |= 0x80000000;                      /* Turn On USB PCLK */

  // Configure 48MHz USB Clock;  FOsc = 12MHz, M = 4, P = 2
  PLL1CFG  = 0x23;                         /* M = 4, P = 2 */
  PLL1CON  = PLLCON_PLLE;                  /* PLL Enable */
  PLL1FEED = 0xAA;                         /* Feed Sequence 1 */
  PLL1FEED = 0x55;                         /* Feed Sequence 2 */
    
  while ((PLL1STAT & PLLSTAT_PLOCK) == 0); /* Wait for PLL Lock */

  PLL1CON  = PLLCON_PLLE | PLLCON_PLLC;    /* PLL Enable & Connect */
  PLL1FEED = 0xAA;                         /* Feed Sequence 1 */
  PLL1FEED = 0x55;                         /* Feed Sequence 2 */

  PINSEL1 = 0x80000000;                     /* select LED USB */

  VICVectAddr0 = (unsigned long)USB_ISR;    /* USB Interrupt -> Vector 0 */
  VICVectCntl0 = 0x20 | 22;                 /* USB Interrupt -> IRQ Slot 0 */
  VICIntEnable |= 1 << 22;                   /* Enable USB Interrupt */

  USBDevIntEn = (1<<DEV_STAT);                /* Enable Device Status Interrupt */

  USB_Reset();
  USB_SetAddress(0);
}

void USB_Reset (void) {

  USBEpInd = 0;
  USBMaxPSize = USB_MAX_PACKET0;
  USBEpInd = 1;
  USBMaxPSize = USB_MAX_PACKET0;
  while ((USBDevIntSt & EP_RLZED_INT) == 0);

  USBEpIntClr  = 0xFFFFFFFF;
  USBEpIntEn   = 0xFFFFFFFF;
  USBEpIntClr   = 0xFFFFFFFF;
  USBDevIntClr = 0xFFFFFFFF;
  USBDevIntEn  = DEV_STAT_INT    | EP_SLOW_INT    |
               (USB_SOF_EVENT   ? FRAME_INT : 0) | ERR_INT;
}