STM32F4 USB CMSIS

oid usb_init(void)
{
 u2_init(115200);
 u2_send((uint8_t*)"Start\r\n",7);
 RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
 GPIOA->AFR[1] |= (10 << GPIO_AFRH_AFSEL11_Pos) | (10 << GPIO_AFRH_AFSEL12_Pos);
 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR12 | GPIO_OSPEEDER_OSPEEDR11; //PA12 PA11 Very high speed
 GPIOA->MODER |= GPIO_MODER_MODE12_1 | GPIO_MODER_MODE11_1;
 //core
 while (!(USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL));
 USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
 while (USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_CSRST); 
 USB_OTG_FS->GCCFG|= USB_OTG_GCCFG_PWRDWN | USB_OTG_GCCFG_NOVBUSSENS; // Exit Power Down mode
 USB_OTG_FS->GUSBCFG|= USB_OTG_GUSBCFG_FDMOD | (0x06 << USB_OTG_GUSBCFG_TRDT_Pos); // Set to Device mode
 USB_OTG_FS->GINTMSK|= USB_OTG_GINTMSK_IEPINT  |// Enable USB IN endpoint interrupt
                       USB_OTG_GINTMSK_MMISM   |// mode error interrupt
                       USB_OTG_GINTMSK_RXFLVLM |//USB receiving
                       USB_OTG_GINTMSK_USBRST;  // reset interrupt
 USB_OTG_FS->GAHBCFG|= USB_OTG_GAHBCFG_GINT;    // Turn on USB general interrupt
 //dev
 USB_OTG_DEV->DCFG |= USB_OTG_DCFG_NZLSOHSK | USB_OTG_DCFG_DSPD_1 | USB_OTG_DCFG_DSPD_0; //Full speed, STALL for all OUT requests
 //buf
 USB_OTG_FS->GRXFSIZ = USB_RXFIFO_SIZE / 4; // size is in 32-bit words !
 USB_OTG_FS->DIEPTXF0_HNPTXFSIZ = ((USB_TXFIFO_EP0_SIZE / 4) << USB_OTG_DIEPTXF_INEPTXFD_Pos) | USB_RXFIFO_SIZE; // Set the position and size of the 0th transmit buffer
 USB_OTG_FS->DIEPTXF[0] = ((USB_TXFIFO_EP1_SIZE/4) <<  USB_OTG_DIEPTXF_INEPTXFD_Pos) | (USB_RXFIFO_SIZE + USB_TXFIFO_EP0_SIZE); // Set the position and size of the No. 1 transmit buffer
 NVIC_EnableIRQ(OTG_FS_IRQn);

void OTG_FS_IRQHandler(void)
{
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_USBRST)
   {
        USB_OTG_DEV->DCTL &= ~USB_OTG_DCTL_RWUSIG;//wakeup signal disable
        //// wipe out any pending EP flags
    for(uint8_t i = 0U; i < 4; i++)
       {
        USB_INEP(i)->DIEPINT = 0xFB7FU;
        USB_OUTEP(i)->DOEPINT = 0xFB7FU;        
       }
        USB_OTG_DEV->DOEPMSK |= USB_OTG_DOEPMSK_STUPM |  // enable setup-done irq
                            USB_OTG_DOEPMSK_EPDM  |  // enable EP-disabled irq
                            USB_OTG_DOEPMSK_XFRCM; 
      USB_OTG_DEV->DIEPMSK |= USB_OTG_DIEPMSK_TOM   |  // timeout irq
                            USB_OTG_DIEPMSK_XFRCM |
                            USB_OTG_DIEPMSK_EPDM; 
             
             
        USB_INEP (0)->DIEPTSIZ = 64;
    USB_OUTEP(0)->DOEPTSIZ = (USB_OTG_DOEPTSIZ_PKTCNT & (1 << 19)) |
                             USB_OTG_DOEPTSIZ_STUPCNT | // allow 3 setup pkt
                             (3 * 8);    
        USB_INEP(0)->DIEPCTL  = USB_OTG_DIEPCTL_USBAEP | // active EP (always 1)
                            USB_OTG_DIEPCTL_SNAK ;   // set NAK bit
    USB_OUTEP(0)->DOEPCTL = USB_OTG_DOEPCTL_EPENA  | // enable endpoint
                            USB_OTG_DOEPCTL_USBAEP | // active EP (always 1)
                            USB_OTG_DOEPCTL_CNAK;    // Clear NAK bit
             
        USB_OTG_DEV->DCFG &= ~USB_OTG_DCFG_DAD;  
        USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_USBRST; //Clear flag
    u2_send((uint8_t*)"RES\r\n", 5);
        return;
     }
 
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_RXFLVL)
   {
        u2_send((uint8_t*)"RX\r\n", 4);
        const volatile uint32_t status = USB_OTG_FS->GRXSTSP;
    const uint16_t bytes = (status & USB_OTG_GRXSTSP_BCNT) >> USB_OTG_GRXSTSP_BCNT_Pos;      
        if((status & USB_OTG_GRXSTSP_EPNUM) == 0) //endpoint 0
          {
             if(((status & USB_OTG_GRXSTSP_PKTSTS) >> USB_OTG_GRXSTSP_PKTSTS_Pos) == 0x06) //SETUP data packet received
               {
                  uint16_t wpos = 0;
          uint32_t w = 0;
          for(int rword = 0; rword < bytes/4; rword++)
             {
              w = *USB_FIFO(0);
              setup_buf[wpos++] =  w        & 0xff;
              setup_buf[wpos++] = (w >>  8) & 0xff;
              setup_buf[wpos++] = (w >> 16) & 0xff;
              setup_buf[wpos++] = (w >> 24) & 0xff;
             }  
                  setup_bytes = bytes;
              u2_send((uint8_t*)"SETUP\r\n", 7);        
                 }
             if(((status & USB_OTG_GRXSTSP_PKTSTS) >> USB_OTG_GRXSTSP_PKTSTS_Pos) == 0x04) //SETUP transaction completed
               {
                    u2_send((uint8_t*)"SETUP_END\r\n", 11); 
                    usb_setup_rx(setup_buf, setup_bytes); 
                        
               }
             }
        USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_RXFLVL; //Clear flag
        return;
     }
 
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_OEPINT)
   {
        u2_send((uint8_t*)"OEPI\r\n", 6);
        USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_OEPINT;
    return;      
     }
 
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_ENUMDNE)
   {
    u2_send((uint8_t*)"ENUM\r\n", 6);
    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_ENUMDNE; // clear the flag 
        return;
   } 
 
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_MMIS)
   {
        u2_send((uint8_t*)"MIS\r\n", 6);    
    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_MMIS;
        return;
   }     
}

void usb_setup_rx(uint8_t *buf, const uint32_t len)
{
 const uint8_t  request_type = buf[0];
 const uint8_t  request = buf[1];
 const uint16_t request_value = buf[2] | ((uint16_t)buf[3] << 8);
 const uint16_t request_len   = buf[6] | ((uint16_t)buf[7] << 8);
  
 if(request_type == 0x00 && request == 0x05) // set address
   {
        u2_send((uint8_t*)"Set addr\r\n", 10);
    const uint16_t addr = buf[2];
    USB_OTG_DEV->DCFG |= (addr << USB_OTG_DCFG_DAD_Pos);
    usb_tx(0, NULL, 0); 
   }
 else if(request_type == 0x80 && request == 0x00) // get status
        {
                 u2_send((uint8_t*)"Get stat\r\n", 10);
         uint8_t status[2] = { 0, 0 };
                 usb_tx(0, status, sizeof(status)); // send an empty status packet back 
        }
 else if(request_type == 0x80 && request == 0x06) // get descriptor
        {
         if(request_value == 0x0100) // request device descriptor
           {
                        u2_send((uint8_t*)"Dev desc\r\n", 10);
                        usb_tx(0, device_descriptor, sizeof(device_descriptor));
           }
         else if(request_value == 0x0200) // request configuration descriptor
           {
                         u2_send((uint8_t*)"Con desc\r\n", 10);
             usb_tx(0, config_descriptor, sizeof(config_descriptor));
           }
         else usb_tx_stall(0); // we don't know how to handle this request    
          }
 else if(request_type == 0x00 && request == 0x09) // set configuration
        {
                     u2_send((uint8_t*)"Set conf\r\n", 10);
         if(request_value != 0)
                   {
                      // usb_tx_stall(0);
            return;   
               } 
           // enable the IN endpoint for configuration #0
           //USB_INEP(1)->DIEPTSIZ = (1 << 19) | payload_len; // set outbound pkt size
            //USB_INEP(1)->DIEPINT  = USB_OTG_DIEPINT_TXFE; // interrupt on TXF empty
          USB_INEP(1)->DIEPINT  = USB_OTG_DIEPINT_XFRC; // interrupt on TXF empty
          USB_INEP(1)->DIEPTSIZ = 64; 
          USB_INEP(1)->DIEPCTL = USB_OTG_DIEPCTL_SNAK    | // set NAK bit
                           (1 << 22)               | // use txfifo #1
                           USB_OTG_DIEPCTL_EPTYP_1 | // bulk transfer
                           USB_OTG_DIEPCTL_USBAEP  | // active EP (always 1)
                           USB_OTG_DIEPCTL_SD0PID_SEVNFRM |
                           64; // max packet size
 
       // enable the OUT endpoint for configuration #0
         USB_OUTEP(2)->DOEPTSIZ = (1 << 19) | 64; // buffer one full-length packet
         USB_OUTEP(2)->DOEPCTL = USB_OTG_DOEPCTL_EPENA  | // enable endpoint
                            USB_OTG_DOEPCTL_USBAEP | // active endpoint (always 1)
                            USB_OTG_DOEPCTL_CNAK   | // clear NAK bit
                            USB_OTG_DOEPCTL_SD0PID_SEVNFRM |
                            USB_OTG_DOEPCTL_EPTYP_1 |
                            64; // max packet size = 64 bytes
         USB_OTG_DEV->DAINTMSK = 0x40002; // OUT2 and IN1 IRQ . used to be 0x10001
           // done configuring endpoints; now we'll send an empty status packet back
         //g_usb_config_complete = true;
         uint8_t bogus[64] = {0};
        // usb_tx(1, bogus, sizeof(bogus)); // kick-start the FIFO low interrupt
        // usb_tx(0, NULL, 0);
        } 
}

void usb_tx(const uint8_t ep, const uint8_t *buf, const uint8_t len)
{
 USB_INEP(ep)->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | len; // set outbound txlen
 USB_INEP(ep)->DIEPCTL  |= USB_OTG_DIEPCTL_EPENA | // enable endpoint
                           USB_OTG_DIEPCTL_CNAK; // clear NAK bit
 uint32_t *fifo = USB_FIFO(ep); // memory-mapped magic...
 for(uint16_t word_idx = 0; word_idx < (len + 3) / 4; word_idx++)
      {
     *fifo = *((uint32_t *)&buf[word_idx * 4]); // abomination
        }
}

void usb_setup_rx(uint8_t *buf, const uint32_t len)
{
 const uint8_t  request_type = buf[0];
 const uint8_t  request = buf[1];
 const uint16_t request_value = buf[2] | ((uint16_t)buf[3] << 8);
 const uint16_t request_len   = buf[6] | ((uint16_t)buf[7] << 8);
  
 if(request_type == 0x00 && request == 0x05) // set address
   {
    const uint16_t addr = buf[2];
    USB_OTG_DEV->DCFG |= (addr << USB_OTG_DCFG_DAD_Pos);
    usb_tx(0, NULL, 0);
        u2.send((uint8_t*)"Set addr\r\n", 10); 
   }
 else if(request_type == 0x80 && request == 0x00) // get status
        {
                 u2.send((uint8_t*)"Get stat\r\n", 10);
         uint8_t status[2] = { 0, 0 };
                 usb_tx(0, status, sizeof(status)); // send an empty status packet back 
        }
 else if(request_type == 0x80 && request == 0x06) // get descriptor
        {
         if(request_value == 0x0100) // request device descriptor
           {
                        usb_tx(0, desc_device, sizeof(desc_device));
                        u2.send((uint8_t*)"Dev desc\r\n", 10);
           }
         else if(request_value == 0x0200) // request configuration descriptor
           {
             usb_tx(0, desc_config, sizeof(desc_config));
                         u2.send((uint8_t*)"Con desc\r\n", 10);
           }
         else if(request_value == 0x0300)
         {
             switch(request_value & 0xFF) //request string descriptor
                 {
                 case 0:
                 usb_tx(0, desc_lang, sizeof(desc_lang));   
                 u2.send((uint8_t*)"Str lang\r\n", 10);
                 break;
                 case 1: //Manufacturer
                                 usb_tx(0, desc_vendor, sizeof(desc_vendor)); 
                 u2.send((uint8_t*)"Str vend\r\n", 10);
                                 break;
                                 case 2: //Product
                                 usb_tx(0, desc_product, sizeof(desc_product));
                     u2.send((uint8_t*)"Str prod\r\n", 10);
                                 break;
                                 case 3: //SerialNumber
                                 usb_tx(0, desc_serial, sizeof(desc_serial)); 
                     u2.send((uint8_t*)"Str serl\r\n", 10);
                                 break;
                                 case 4: //Interface
                                 usb_tx(0, desc_config, sizeof(desc_config));
                     u2.send((uint8_t*)"Str intr\r\n", 10);
                                 break;
                                 default:
                     u2.send((uint8_t*)"Str oter\r\n", 10);
                     sprintf(str, "%d",(request_value & 0xFF));
                     u2.send((uint8_t*)"val ", 4);
                     u2.send((uint8_t*)str, strlen(str));
                     u2.send((uint8_t*)"\r\n", 2);
                     break;
                                             } 
                                }
                    else 
                        { 
                                 sprintf(str, "%d",request_value);
                   u2.send((uint8_t*)"req val ", 8);
                   u2.send((uint8_t*)str, strlen(str));
                   u2.send((uint8_t*)"\r\n", 2);
                           usb_tx_stall(0); // we don't know how to handle this request 
                        }                    
          }
 else if(request_type == 0x00 && request == 0x09) // set configuration
        {
                     u2.send((uint8_t*)"Set conf\r\n", 10);
         if(request_value != 0)
                   {
                       usb_tx_stall(0);
            return;   
               } 
          
         usb_tx(0, NULL, 0);
        } 
 else 
 {
    u2.send((uint8_t*)"Other\r\n", 7);
  sprintf(str, "%d",request_type);
    u2.send((uint8_t*)"req val ", 8);
    u2.send((uint8_t*)str, strlen(str));
    u2.send((uint8_t*)"\r\n", 2);
    sprintf(str, "%d",request);
    u2.send((uint8_t*)"reque ", 6);
    u2.send((uint8_t*)str, strlen(str));
    u2.send((uint8_t*)"\r\n", 2); 
 }
}

#include "usb.h"
#include "desc.h"
 
Usb usb;
 
 
extern "C"
void OTG_FS_IRQHandler(void)
{   
 usb.intr();
}
 
void Usb::intr(void)
{
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_RXFLVL)                      // RX level interrupt
   {
        uint32_t status = USB_OTG_FS->GRXSTSP;
    uint16_t len = (status & USB_OTG_GRXSTSP_BCNT) >> 4;                 // Read len
        if(((status & USB_OTG_GRXSTSP_PKTSTS) >> 17) == 0x02)              // Data
          {
           if(len)
               {                         
                  read(status & USB_OTG_GRXSTSP_EPNUM, &bufRx[countRx] , len); // Read data
                    epNumLastRx = status & USB_OTG_GRXSTSP_EPNUM;                // Save last endpoint num
                    countRx += len;                                              // Save all len
                 }
          }
        if(((status & USB_OTG_GRXSTSP_PKTSTS) >> 17) == 0x06)              // Setup data packet received
            {    
             read(status & USB_OTG_GRXSTSP_EPNUM, bufRx, 0x08);                    // Read setup packet
            }   
     }
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_OEPINT) // OUT -> RX endpoint interrupt
   {         
        uint32_t epNum; 
        uint32_t epInt;
      epNum = USB_OTG_DEV->DAINT;
    epNum &= USB_OTG_DEV->DAINTMSK;
        if(epNum & EP0_OUT_INT)                       // EP0 OUT RX Interrupt
          {      
             epInt = USB_OUTEP(0)->DOEPINT;
       epInt &= USB_OTG_DEV->DOEPMSK;
             if(epInt & USB_OTG_DOEPINT_STUP)
               {
                setup(bufRx);                            // Parse setup packet
                 }
             USB_OUTEP(0)->DOEPINT = epInt;              // Clear flag
             USB_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK | 
                                        USB_OTG_DOEPCTL_EPENA; // Enable endpoint, Clear NAK bit
            }   
    if(epNum & EP1_OUT_INT)                        // EP1 OUT RX Interrupt
          {
             epInt = USB_OUTEP(1)->DOEPINT;
       epInt &= USB_OTG_DEV->DOEPMSK;
             USB_OUTEP(1)->DOEPINT = epInt;               // Clear flag
             USB_OUTEP(1)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK | 
                                        USB_OTG_DOEPCTL_EPENA; // Enable endpoint, Clear NAK bit
            }               
     }
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_IEPINT)   // IN -> TX endpoint interrupt
   {     
        uint32_t epNum;
    uint32_t epInt;      
      epNum = USB_OTG_DEV->DAINT;
    epNum &= USB_OTG_DEV->DAINTMSK;
    if(epNum & EP0_IN_INT)                          // EP0 IN TX Interrupt
          {
             epInt = USB_INEP(0)->DIEPINT;
       epInt &= USB_OTG_DEV->DIEPMSK;
             if(epInt & USB_OTG_DIEPINT_XFRC)
               {
                  sendEnd(0);                               // If data left to send
         }
         USB_INEP(0)->DIEPINT = epInt;                // Clear flag              
      }
    if(epNum & EP1_IN_INT)                          // EP1 IN TX Interrupt
          {
             epInt = USB_INEP(1)->DIEPINT;
       epInt &= USB_OTG_DEV->DIEPMSK;
             if(epInt & USB_OTG_DIEPINT_XFRC)
               {
                  sendEnd(1);                               // If data left to send
         }
         USB_INEP(1)->DIEPINT = epInt;                // Clear flag             
      }                 
     }
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_USBRST)   // Reset Int
   {
        USB_OTG_DEV->DCTL &= ~USB_OTG_DCTL_RWUSIG;      // Wakeup signal disable    
        flushTx();                                      // Clear tx buffer
    for(uint8_t i = 0U; i < 4; i++)                 // Clear any pending EP flags
       {
        USB_INEP(i)->DIEPINT = 0xFB7FU;
                USB_INEP(i)->DIEPCTL &= ~USB_OTG_DIEPCTL_STALL;
                USB_INEP(i)->DIEPCTL |= USB_OTG_DIEPCTL_SNAK;
        USB_OUTEP(i)->DOEPINT = 0xFB7FU;
                USB_OUTEP(i)->DOEPCTL &= ~USB_OTG_DOEPCTL_STALL;
        USB_OUTEP(i)->DOEPCTL |= USB_OTG_DOEPCTL_SNAK;               
             }
        USB_OTG_DEV->DAINTMSK |= 0x10001U;              // EP0 OUT, EP0 IN Interupt              
        USB_OTG_DEV->DOEPMSK |= USB_OTG_DOEPMSK_STUPM | // Enable setup-done irq
                            USB_OTG_DOEPMSK_EPDM  | // Enable EP-disabled irq
                            USB_OTG_DOEPMSK_XFRCM;  // Enable tx-done irq                                    
      USB_OTG_DEV->DIEPMSK |= USB_OTG_DIEPMSK_TOM   | // Timeout irq
                            USB_OTG_DIEPMSK_XFRCM |
                            USB_OTG_DIEPMSK_EPDM; 
        /* Setup EP0 to receive SETUP packets */
    USB_OUTEP(0)->DOEPTSIZ = (1 << USB_OTG_DOEPTSIZ_PKTCNT_Pos) |
                             USB_OTG_DOEPTSIZ_STUPCNT | // Allow 3 setup pkt
                             (3 * 8);    
    USB_OUTEP(0)->DOEPCTL = USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;   // Enable endpoint, Clear NAK bit                                  
        USB_OTG_DEV->DCFG &= ~USB_OTG_DCFG_DAD;         // Clear address    
        USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_USBRST;   // Clear flag    
     }
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_ENUMDNE)
   { 
    USB_OTG_FS->GINTSTS |= USB_OTG_GINTSTS_ENUMDNE; // Clear the flag  
   } 
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_MMIS)
   {
    USB_OTG_FS->GINTSTS |= USB_OTG_GINTSTS_MMIS;
   }  
 if(USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_SOF)
   {     
    USB_OTG_FS->GINTSTS |= USB_OTG_GINTSTS_SOF;     // Clear the flag (rc_w1)
   }    
}
 
/* This function is sending data */
void Usb::sendData(const uint8_t ep, const uint8_t *buf, uint8_t len)
{
 while(countTx);
 countTx = len;                                  // Save all len
 if(len > MAX_SIZE)                              // Larger maximum size?
          {                                          // Create queue for send
             len = MAX_SIZE;                           // Set maximum size
             bufTx =(uint8_t*) buf;                    //   Save ptr for new send
            }       
 send(ep, buf, len);                             // Send data           
}
 
 
void Usb::sendEnd(const uint8_t ep) 
{
 if(countTx > MAX_SIZE)                          // If queue for send?
   {
      bufTx = (uint8_t*) (bufTx + MAX_SIZE);       // Calculating ptr for send
    countTx -= MAX_SIZE;                           // Calculating length
    send(ep, bufTx, countTx);                    // Send data        
     }
 else countTx = 0;                               // No queue for send
}   
 
/* This function reads received data from the buffer */
uint16_t Usb::readData(const uint8_t ep, uint8_t *buf)
{
 uint16_t i, len = 0;
 if(ep == epNumLastRx)                                                           
   {
    if(countRx)                                      // If read data
          {
             for(i = 0; i < countRx; i++)
                  {
                     *buf++ = bufRx[i];                        // Copy data
                    }
             len = countRx;
       countRx = 0; 
       epNumLastRx = 0;                 
            }
   }         
 return len;
}
 
 
void Usb::send(const uint8_t ep, const uint8_t *buf, const uint8_t len)
{
 uint16_t i;
 uint32_t *ptr = (uint32_t *)buf;
 USB_INEP(ep)->DIEPTSIZ = (1 << USB_OTG_DIEPTSIZ_PKTCNT_Pos) | len;       // Set outbound txlen
 USB_INEP(ep)->DIEPCTL |= USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK;   // Enable endpoint, clear NAK bit     
 for(i = 0; i < ((len + 3) / 4); i++) 
      {
     USB_FIFO(ep) = __UNALIGNED_UINT32_READ(ptr);                         // Copy data 
         ptr++;
        } 
}
 
 
void Usb::read(const uint8_t ep, const uint8_t *buf, const uint8_t len)
{
 int16_t i; 
 uint32_t   *ptr = (uint32_t *)buf;
 for(i = 0; i < ((len + 3) / 4); i++)
      {
         __UNALIGNED_UINT32_WRITE(ptr, USB_FIFO(ep));         // Read data
         ptr++; 
        }
}
 
 
void Usb::init(void)
{
 /* Enable clock */
 RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
 /* Port config */ 
 GPIOA->AFR[1] |= (10 << GPIO_AFRH_AFSEL12_Pos) | (10 << GPIO_AFRH_AFSEL11_Pos); // PA12 -> OTG_FS_DP, PA11 -> OTG_FS_DM
 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR12 | GPIO_OSPEEDER_OSPEEDR11;            // PA12 PA11 Very high speed
 GPIOA->MODER |= GPIO_MODER_MODE12_1 | GPIO_MODER_MODE11_1;                      // Alternate function
 /* Core */
 while (!(USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL));
 USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
 while (USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_CSRST); 
 USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_PWRDWN | USB_OTG_GCCFG_NOVBUSSENS; // Exit Power Down mode
 USB_OTG_FS->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD | (0x06 << USB_OTG_GUSBCFG_TRDT_Pos) | USB_OTG_GUSBCFG_PHYSEL | (17 << USB_OTG_GUSBCFG_TOCAL_Pos); // Set to Device mode
 USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_IEPINT |   // Enable USB IN TX endpoint interrupt
                          USB_OTG_GINTMSK_OEPINT |   // Enable USB OUT RX endpoint interrupt
                        USB_OTG_GINTMSK_RXFLVLM |  // USB reciving
                        USB_OTG_GINTMSK_USBRST;    // Reset interrupt
 USB_OTG_FS->GAHBCFG = USB_OTG_GAHBCFG_GINT;       // On USB general interrupt
 /* Device */
 USB_OTG_DEV->DCFG |= USB_OTG_DCFG_NZLSOHSK | USB_OTG_DCFG_DSPD_1 | USB_OTG_DCFG_DSPD_0; //Full speed, STALL for all OUT requests
 /* Buffers */
 USB_OTG_FS->GRXFSIZ = RX_FIFO_SIZE; // size is in 32-bit words !
 USB_OTG_FS->DIEPTXF0_HNPTXFSIZ = (TX_FIFO_EP0_SIZE << 16) | RX_FIFO_SIZE;              // Set the position and size of the transmit buffer
 USB_OTG_FS->DIEPTXF[1] = (TX_FIFO_EP1_SIZE << 16) | (RX_FIFO_SIZE + TX_FIFO_EP0_SIZE); // Set the position and size of the transmit buffer     
 USB_OTG_FS->DIEPTXF[2] = (TX_FIFO_EP2_SIZE << 16) | (RX_FIFO_SIZE + TX_FIFO_EP0_SIZE + TX_FIFO_EP1_SIZE);
 /* Interrupt */
 NVIC_SetPriority(OTG_FS_IRQn, 1);
 NVIC_EnableIRQ(OTG_FS_IRQn);   
}
 
 
void Usb::setup(uint8_t *buf)
{
 const uint8_t bmRequestType = buf[0];
 const uint8_t bRequest = buf[1];
 const uint16_t wValue = buf[2] | ((uint16_t)buf[3] << 8);
 const uint16_t wIndex = buf[4] | ((uint16_t)buf[5] << 8);  
 const uint16_t wLength = buf[6] | ((uint16_t)buf[7] << 8); 
 switch(bmRequestType & 0x1FU)
       {
          case REQ_DEVICE: 
                     switch(bmRequestType & REQ_MASK)
                               {
                                  case REQ_CLASS:
                                      case REQ_VENDOR:
                                        break;
                                        case REQ_STANDARD:
                                        switch(bRequest)
                                              {
                                                 case GET_DESCRIPTOR:
                                                        getDesc(wValue, wLength);
                                                 break; 
                                                     case SET_ADDRESS:
                                                          setAddr(buf[2]);                                                     
                                                     break;
                                                     case SET_CONFIGURATION:
                                                            setConfig();
                           break;
                           case GET_CONFIGURATION:
                           break;
                           case GET_STATUS:
                           break;
                           case SET_FEATURE:
                           break;
                           case CLEAR_FEATURE:
                           break;
                           default:
                           break;
                                                    }                                       
                                        break;
                                     }
                break;
            case REQ_INTERFACE:
             uint8_t data[8];
             sendData(0, data, wLength);
             if(bRequest == 34) countRx = 0;                
        break;              
            case REQ_ENDPOINT:  
                break;
             }
} 
 
 
void Usb::getDesc(uint16_t wValue, uint16_t wLength)
{
 uint8_t *pbuf;
 uint8_t len = 0;
 switch(wValue >> 8)    
       {
          case DESC_DEVICE:                  // Request device descriptor
                       pbuf = (uint8_t*)desc_device; 
             len = sizeof(desc_device);             
        break;  
        case DESC_CONFIG:                  // Request configuration descriptor
                       pbuf = (uint8_t*) desc_config;
                     len = sizeof(desc_config);
        break;   
        case DESC_STRING:                  // Request string descriptor
                     switch(wValue & 0xFF)         // Request string descriptor
                                   {
                                      case DESC_STR_LANGID:  // Lang
                                               pbuf = (uint8_t*) desc_lang;
                                                 len = sizeof(desc_lang);   
                    break;                                              
                                        case DESC_STR_MFC:     // Manufacturer
                         pbuf = (uint8_t*) desc_vendor, 
                                             len = sizeof(desc_vendor); 
                    break;
                    case DESC_STR_PRODUCT: // Product
                         pbuf = (uint8_t*) desc_product; 
                                             len = sizeof(desc_product);
                    break;
                    case DESC_STR_SERIAL:  // SerialNumber
                         pbuf = (uint8_t*) desc_serial;
                                     len = sizeof(desc_serial); 
                    break;
                                      case DESC_STR_CONFIG:  // Config
                                                 pbuf = (uint8_t*) desc_config;
                                         len = sizeof(desc_config);
                                      break;
                    case DESC_STR_INTERFACE:// Interface
                    break;
                    default:
                                        break;
                                     }                          
                break;                                      
                case DESC_OTHER_CONFIG:
                       stallTx(0); // we don't know how to handle this request                      
          break;
             }
 if(len)
   {    
      sendData(0, pbuf, MIN(len,wLength));   
     }  
}
 
 
void Usb::setAddr(uint8_t addr)
{
 USB_OTG_DEV->DCFG |= ((uint32_t)addr << 4);
 send(0, 0, 0); 
}
 
 
void Usb::setConfig(void)
{               
 /* Open EP1 IN */
 USB_OTG_DEV->DAINTMSK |=   (1 << 1);                           // Enable Interupt
 USB_INEP(1)->DIEPCTL |= USB_OTG_DIEPCTL_SD0PID_SEVNFRM |     // Set DATA0
                           (1 << USB_OTG_DIEPCTL_TXFNUM_Pos)|   // TxFIFO number
                           USB_OTG_DIEPCTL_EPTYP_1 |            // Endpoint type bulk
                           USB_OTG_DIEPCTL_USBAEP |             // Active endpoint
                           MAX_SIZE;                            // Max packet size
 /* Open EP1 OUT */
 USB_OTG_DEV->DAINTMSK |= (1 << 17);                          // Enable Interupt
 USB_OUTEP(1)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA |             // Endpoint enable
                            USB_OTG_DOEPCTL_SD0PID_SEVNFRM |    // Set DATA0
                            USB_OTG_DOEPCTL_CNAK |              // Clear NAK
                            USB_OTG_DOEPCTL_EPTYP_1 |           // Endpoint type bulk
                            USB_OTG_DOEPCTL_USBAEP |            // Active endpoint 
                            MAX_SIZE;                           // Max packet size
 /* Open Command EP2 IN  */
 USB_OTG_DEV->DAINTMSK |= (1 << 2);                           // Enable Interupt
 USB_INEP(2)->DIEPCTL |= USB_OTG_DIEPCTL_SD0PID_SEVNFRM |     // Set DATA0
                           (1 << USB_OTG_DIEPCTL_TXFNUM_Pos)|   // TxFIFO number
                           USB_OTG_DIEPCTL_EPTYP |              // Endpoint type Interrupt
                           USB_OTG_DIEPCTL_USBAEP |             // Active endpoint
                           MAX_SIZE;                            // Max packet size
 send(0, 0, 0);
}
 
 
void Usb::flushTx(void)
{
 USB_OTG_FS->GRSTCTL = USB_OTG_GRSTCTL_TXFFLSH | (1 << 10);
 while(USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH);
}
 
 
void Usb::flushRx(void)
{
 USB_OTG_FS->GRSTCTL = USB_OTG_GRSTCTL_RXFFLSH;
 while (USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_RXFFLSH);
}
 
 
void Usb::stallTx(uint8_t ep)
{
 USB_INEP(ep)->DIEPCTL |= USB_OTG_DIEPCTL_STALL;
}

#define USB_OTG_DEV  ((USB_OTG_DeviceTypeDef *)((uint32_t)USB_OTG_FS_PERIPH_BASE + USB_OTG_DEVICE_BASE))
#define USB_INEP(i)  ((USB_OTG_INEndpointTypeDef *)(( uint32_t)USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE + (i) * USB_OTG_EP_REG_SIZE))        
#define USB_OUTEP(i) ((USB_OTG_OUTEndpointTypeDef *)((uint32_t)USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE + (i) * USB_OTG_EP_REG_SIZE))
#define USB_FIFO_BASE *(__IO uint32_t *)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE)
#define USB_FIFO(i)  *(__IO uint32_t *)(USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + ((i) * USB_OTG_FIFO_SIZE))
 
    
#define RX_FIFO_SIZE     128
#define TX_FIFO_EP0_SIZE 128
#define TX_FIFO_EP1_SIZE 128
#define TX_FIFO_EP2_SIZE 128
#define TX_FIFO_EP3_SIZE 128
 
#define  DESC_DEVICE               0x01U
#define  DESC_CONFIG               0x02U
#define  DESC_STRING               0x03U
#define  DESC_INTERFACE            0x04U
#define  DESC_ENDPOINT             0x05U
#define  DESC_DEVICE_QUALIFIER     0x06U
#define  DESC_OTHER_CONFIG         0x07U
#define  DESC_IAD                  0x0BU
#define  DESC_BOS                  0x0FU
 
#define  DESC_STR_LANGID           0x00U
#define  DESC_STR_MFC              0x01U
#define  DESC_STR_PRODUCT          0x02U
#define  DESC_STR_SERIAL           0x03U
#define  DESC_STR_CONFIG           0x04U
#define  DESC_STR_INTERFACE        0x05U
#define  REQ_DEVICE                0x00U
#define  REQ_INTERFACE             0x01U
#define  REQ_ENDPOINT              0x02U
 
#define  GET_STATUS                0x00U
#define  CLEAR_FEATURE             0x01U
#define  SET_FEATURE               0x03U
#define  SET_ADDRESS               0x05U
#define  GET_DESCRIPTOR            0x06U
#define  SET_DESCRIPTOR            0x07U
#define  GET_CONFIGURATION         0x08U
#define  SET_CONFIGURATION         0x09U
#define  GET_INTERFACE             0x0AU
#define  SET_INTERFACE             0x0BU
#define  SYNCH_FRAME               0x0CU
 
#define  REQ_STANDARD              0x00U
#define  REQ_CLASS                 0x20U
#define  REQ_VENDOR                0x40U
#define  REQ_MASK                  0x60U
 
#define EP0_OUT_INT                0x00010000
#define EP1_OUT_INT                0x00020000
#define EP2_OUT_INT                0x00040000
 
#define EP0_IN_INT                 0x0001
#define EP1_IN_INT                 0x0002
#define EP2_IN_INT                 0x0004
 
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX_SIZE                   64U
 
     
void usb_tx(const uint8_t ep, const uint8_t *buf, const uint8_t len);
void usb_rx(const uint8_t ep, const uint8_t *buf, const uint8_t len);
void usb_setup(uint8_t *buf);
void usb_start_tx(const uint8_t ep, const uint8_t *buf, uint8_t len);
void usb_end_tx(const uint8_t ep);
void usb_tx_stall(uint8_t ep);
void usb_rx_internal(uint8_t ep, uint8_t *data, uint8_t len);
void usb_flush_txfifo(void);
void usb_flush_rxfifo(void);
void getDesc(uint16_t wValue, uint16_t wLength);
void setConfig(void);
void setAddress(uint8_t addr);
 
class Usb
{
 private:
   uint8_t bufRx[72];
   uint8_t *bufTx;
   uint8_t epNumLastRx = 0;
   volatile uint16_t countTx = 0;
   volatile uint16_t countRx = 0;
   void sendEnd(const uint8_t ep);
   void send(const uint8_t ep, const uint8_t *buf, const uint8_t len);
   void read(const uint8_t ep, const uint8_t *buf, const uint8_t len);
   void setup(uint8_t *buf);
   void setAddr(uint8_t addr);
   void getDesc(uint16_t wValue, uint16_t wLength);
   void setConfig(void);
   void flushTx(void);
   void flushRx(void);
   void stallTx(uint8_t ep);
 public:
     void init(void);
   void intr(void); // Interrupt
   void sendData(const uint8_t ep, const uint8_t *buf, uint8_t len); // Sending data   
   uint16_t readData(const uint8_t ep, uint8_t *buf);                // Reciv data
};
 
extern Usb usb;