git.zerfleddert.de Git - proxmark3-svn/blob - common/usb_cdc.c

   1 /*
   2  * at91sam7s USB CDC device implementation
   3  *
   4  * Copyright (c) 2012, Roel Verdult
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  * notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  * notice, this list of conditions and the following disclaimer in the
  13  * documentation and/or other materials provided with the distribution.
  14  * 3. Neither the name of the copyright holders nor the
  15  * names of its contributors may be used to endorse or promote products
  16  * derived from this software without specific prior written permission.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
  19  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  20  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  21  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
  22  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  23  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  24  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  25  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  26  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  27  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  28  *
  29  * based on the "Basic USB Example" from ATMEL (doc6123.pdf)
  30  *
  31  * @file usb_cdc.c
  32  * @brief
  33  */
  34
  35 #include "usb_cdc.h"
  36 #include "config_gpio.h"
  37
  38 #define MIN(a, b) (((a) < (b)) ? (a) : (b))
  39 #define MAX(a, b) (((a) > (b)) ? (a) : (b))
  40 #define AT91C_EP_IN_SIZE  0x40
  41 #define AT91C_EP_OUT         1
  42 #define AT91C_EP_OUT_SIZE 0x40
  43 #define AT91C_EP_IN          2
  44
  45 const char devDescriptor[] = {
  46         /* Device descriptor */
  47         0x12,      // bLength
  48         0x01,      // bDescriptorType
  49         //0x10,0x01, // Complies with USB Spec. Release (0110h = release 1.10)
  50         0x00,0x02, // Complies with USB Spec. Release (0110h = release 2.00)
  51         0x02,      // bDeviceClass:    CDC class code
  52         0x00,      // bDeviceSubclass: CDC class sub code
  53         0x00,      // bDeviceProtocol: CDC Device protocol
  54         0x08,      // bMaxPacketSize0
  55         0x2d,0x2d, // Vendor ID (--)
  56         0x4d,0x50, // Product ID (PM), transmitted in reverse
  57         0x01,0x00, // Device release number (0001)
  58         0x01,      // iManufacturer    // 0x01
  59         0x00,      // iProduct
  60         0x00,      // SerialNumber
  61         0x01       // bNumConfigs
  62 };
  63
  64 const char cfgDescriptor[] = {
  65         /* ============== CONFIGURATION 1 =========== */
  66         /* Configuration 1 descriptor */
  67         0x09,   // CbLength
  68         0x02,   // CbDescriptorType
  69         0x43,   // CwTotalLength 2 EP + Control
  70         0x00,
  71         0x02,   // CbNumInterfaces
  72         0x01,   // CbConfigurationValue
  73         0x00,   // CiConfiguration
  74         0xC0,   // CbmAttributes 0xA0
  75         0x00,   // CMaxPower
  76
  77         /* Communication Class Interface Descriptor Requirement */
  78         0x09, // bLength
  79         0x04, // bDescriptorType
  80         0x00, // bInterfaceNumber
  81         0x00, // bAlternateSetting
  82         0x01, // bNumEndpoints
  83         0x02, // bInterfaceClass
  84         0x02, // bInterfaceSubclass
  85         0x00, // bInterfaceProtocol
  86         0x00, // iInterface
  87
  88         /* Header Functional Descriptor */
  89         0x05, // bFunction Length
  90         0x24, // bDescriptor type: CS_INTERFACE
  91         0x00, // bDescriptor subtype: Header Func Desc
  92         0x10, // bcdCDC:1.1
  93         0x01,
  94
  95         /* ACM Functional Descriptor */
  96         0x04, // bFunctionLength
  97         0x24, // bDescriptor Type: CS_INTERFACE
  98         0x02, // bDescriptor Subtype: ACM Func Desc
  99         0x00, // bmCapabilities
 100
 101         /* Union Functional Descriptor */
 102         0x05, // bFunctionLength
 103         0x24, // bDescriptorType: CS_INTERFACE
 104         0x06, // bDescriptor Subtype: Union Func Desc
 105         0x00, // bMasterInterface: Communication Class Interface
 106         0x01, // bSlaveInterface0: Data Class Interface
 107
 108         /* Call Management Functional Descriptor */
 109         0x05, // bFunctionLength
 110         0x24, // bDescriptor Type: CS_INTERFACE
 111         0x01, // bDescriptor Subtype: Call Management Func Desc
 112         0x00, // bmCapabilities: D1 + D0
 113         0x01, // bDataInterface: Data Class Interface 1
 114
 115         /* Endpoint 1 descriptor */
 116         0x07,   // bLength
 117         0x05,   // bDescriptorType
 118         0x83,   // bEndpointAddress, Endpoint 03 - IN
 119         0x03,   // bmAttributes      INT
 120         0x08,   // wMaxPacketSize
 121         0x00,
 122         0xFF,   // bInterval
 123
 124         /* Data Class Interface Descriptor Requirement */
 125         0x09, // bLength
 126         0x04, // bDescriptorType
 127         0x01, // bInterfaceNumber
 128         0x00, // bAlternateSetting
 129         0x02, // bNumEndpoints
 130         0x0A, // bInterfaceClass
 131         0x00, // bInterfaceSubclass
 132         0x00, // bInterfaceProtocol
 133         0x00, // iInterface
 134
 135         /* First alternate setting */
 136         /* Endpoint 1 descriptor */
 137         0x07,   // bLength
 138         0x05,   // bDescriptorType
 139         0x01,   // bEndpointAddress, Endpoint 01 - OUT
 140         0x02,   // bmAttributes      BULK
 141         AT91C_EP_OUT_SIZE,   // wMaxPacketSize
 142         0x00,
 143         0x00,   // bInterval
 144
 145         /* Endpoint 2 descriptor */
 146         0x07,   // bLength
 147         0x05,   // bDescriptorType
 148         0x82,   // bEndpointAddress, Endpoint 02 - IN
 149         0x02,   // bmAttributes      BULK
 150         AT91C_EP_IN_SIZE,   // wMaxPacketSize
 151         0x00,
 152         0x00    // bInterval
 153 };
 154
 155 const char strDescriptor[] = {
 156   26,                   // Length
 157   0x03,                 // Type is string
 158   'p', 0x00,
 159   'r', 0x00,
 160   'o', 0x00,
 161   'x', 0x00,
 162   'm', 0x00,
 163   'a', 0x00,
 164   'r', 0x00,
 165   'k', 0x00,
 166   '.', 0x00,
 167   'o', 0x00,
 168   'r', 0x00,
 169   'g', 0x00,
 170 };
 171
 172 // Bitmap for all status bits in CSR.
 173 #define REG_NO_EFFECT_1_ALL      AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1 \
 174                                 |AT91C_UDP_STALLSENT   | AT91C_UDP_RXSETUP \
 175                                 |AT91C_UDP_TXCOMP
 176
 177 // Clear flags in the UDP_CSR register and waits for synchronization
 178 #define UDP_CLEAR_EP_FLAGS(endpoint, flags) { \
 179         volatile unsigned int reg; \
 180         reg = pUdp->UDP_CSR[endpoint]; \
 181         reg |= REG_NO_EFFECT_1_ALL; \
 182         reg &= ~(flags); \
 183         pUdp->UDP_CSR[endpoint] = reg; \
 184         while ( (pUdp->UDP_CSR[endpoint] & (flags)) == (flags)); \
 185 }
 186
 187 // reset flags in the UDP_CSR register and waits for synchronization
 188 #define UDP_SET_EP_FLAGS(endpoint, flags) { \
 189         volatile unsigned int reg; \
 190         reg = pUdp->UDP_CSR[endpoint]; \
 191         reg |= REG_NO_EFFECT_1_ALL; \
 192         reg |= (flags); \
 193         pUdp->UDP_CSR[endpoint] = reg; \
 194         while ( ( pUdp->UDP_CSR[endpoint] & (flags)) != (flags)); \
 195 }
 196
 197
 198 /* USB standard request code */
 199 #define STD_GET_STATUS_ZERO           0x0080
 200 #define STD_GET_STATUS_INTERFACE      0x0081
 201 #define STD_GET_STATUS_ENDPOINT       0x0082
 202
 203 #define STD_CLEAR_FEATURE_ZERO        0x0100
 204 #define STD_CLEAR_FEATURE_INTERFACE   0x0101
 205 #define STD_CLEAR_FEATURE_ENDPOINT    0x0102
 206
 207 #define STD_SET_FEATURE_ZERO          0x0300
 208 #define STD_SET_FEATURE_INTERFACE     0x0301
 209 #define STD_SET_FEATURE_ENDPOINT      0x0302
 210
 211 #define STD_SET_ADDRESS               0x0500
 212 #define STD_GET_DESCRIPTOR            0x0680
 213 #define STD_SET_DESCRIPTOR            0x0700
 214 #define STD_GET_CONFIGURATION         0x0880
 215 #define STD_SET_CONFIGURATION         0x0900
 216 #define STD_GET_INTERFACE             0x0A81
 217 #define STD_SET_INTERFACE             0x0B01
 218 #define STD_SYNCH_FRAME               0x0C82
 219
 220 /* CDC Class Specific Request Code */
 221 #define GET_LINE_CODING               0x21A1
 222 #define SET_LINE_CODING               0x2021
 223 #define SET_CONTROL_LINE_STATE        0x2221
 224
 225 typedef struct {
 226         unsigned int dwDTERRate;
 227         char bCharFormat;
 228         char bParityType;
 229         char bDataBits;
 230 } AT91S_CDC_LINE_CODING, *AT91PS_CDC_LINE_CODING;
 231
 232 AT91S_CDC_LINE_CODING line = {
 233         115200, // baudrate
 234         0,      // 1 Stop Bit
 235         0,      // None Parity
 236         8};     // 8 Data bits
 237
 238 void AT91F_CDC_Enumerate();
 239
 240 AT91PS_UDP pUdp = AT91C_BASE_UDP;
 241 byte_t btConfiguration = 0;
 242 byte_t btConnection    = 0;
 243 byte_t btReceiveBank   = AT91C_UDP_RX_DATA_BK0;
 244
 245 //*----------------------------------------------------------------------------
 246 //* \fn    usb_disable
 247 //* \brief This function deactivates the USB device
 248 //*----------------------------------------------------------------------------
 249 void usb_disable() {
 250         // Disconnect the USB device
 251         AT91C_BASE_PIOA->PIO_ODR = GPIO_USB_PU;
 252
 253         // Clear all lingering interrupts
 254         if(pUdp->UDP_ISR & AT91C_UDP_ENDBUSRES) {
 255                 pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
 256         }
 257 }
 258
 259 //*----------------------------------------------------------------------------
 260 //* \fn    usb_enable
 261 //* \brief This function Activates the USB device
 262 //*----------------------------------------------------------------------------
 263 void usb_enable() {
 264         // Set the PLL USB Divider
 265         AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1 ;
 266
 267         // Specific Chip USB Initialisation
 268         // Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock
 269         AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_UDP;
 270         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
 271
 272         // Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
 273         // Set in PIO mode and Configure in Output
 274         AT91C_BASE_PIOA->PIO_PER = GPIO_USB_PU; // Set in PIO mode
 275         AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; // Configure as Output
 276
 277         // Clear for set the Pullup resistor
 278         AT91C_BASE_PIOA->PIO_CODR = GPIO_USB_PU;
 279
 280         // Disconnect and reconnect USB controller for 100ms
 281         usb_disable();
 282
 283         // Wait for a short while
 284         for (volatile size_t i=0; i<0x100000; i++);
 285     //sleep(1);
 286
 287         // Reconnect USB reconnect
 288         AT91C_BASE_PIOA->PIO_SODR = GPIO_USB_PU;
 289         AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU;
 290 }
 291
 292 //*----------------------------------------------------------------------------
 293 //* \fn    usb_check
 294 //* \brief Test if the device is configured and handle enumeration
 295 //*----------------------------------------------------------------------------
 296 bool usb_check() {
 297         AT91_REG isr = pUdp->UDP_ISR;
 298
 299         if (isr & AT91C_UDP_ENDBUSRES) {
 300                 pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
 301                 // reset all endpoints
 302                 pUdp->UDP_RSTEP  = (unsigned int)-1;
 303                 pUdp->UDP_RSTEP  = 0;
 304                 // Enable the function
 305                 pUdp->UDP_FADDR = AT91C_UDP_FEN;
 306                 // Configure endpoint 0
 307                 pUdp->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 308         }
 309         else if (isr & AT91C_UDP_EPINT0) {
 310                 pUdp->UDP_ICR = AT91C_UDP_EPINT0;
 311                 AT91F_CDC_Enumerate();
 312         }
 313         return (btConfiguration) ? true : false;
 314 }
 315
 316
 317 bool usb_poll()
 318 {
 319         if (!usb_check()) return false;
 320         return (pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank);
 321 }
 322
 323 /**
 324         In github PR #129, some users appears to get a false positive from
 325         usb_poll, which returns true, but the usb_read operation
 326         still returns 0.
 327         This check is basically the same as above, but also checks
 328         that the length available to read is non-zero, thus hopefully fixes the
 329         bug.
 330 **/
 331 bool usb_poll_validate_length()
 332 {
 333         if (!usb_check()) return false;
 334         if (!(pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false;
 335         return (pUdp->UDP_CSR[AT91C_EP_OUT] >> 16) >  0;
 336 }
 337
 338 //*----------------------------------------------------------------------------
 339 //* \fn    usb_read
 340 //* \brief Read available data from Endpoint OUT
 341 //*----------------------------------------------------------------------------
 342 uint32_t usb_read(byte_t* data, size_t len) {
 343         byte_t bank = btReceiveBank;
 344         uint32_t packetSize, nbBytesRcv = 0;
 345         uint32_t time_out = 0;
 346
 347         while (len)  {
 348                 if (!usb_check()) break;
 349
 350                 if ( pUdp->UDP_CSR[AT91C_EP_OUT] & bank ) {
 351                         packetSize = MIN(pUdp->UDP_CSR[AT91C_EP_OUT] >> 16, len);
 352                         len -= packetSize;
 353                         while(packetSize--)
 354                                 data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT];
 355
 356
 357                         UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank)
 358                         //pUdp->UDP_CSR[AT91C_EP_OUT] &= ~(bank);
 359
 360                         if (bank == AT91C_UDP_RX_DATA_BK0)
 361                                 bank = AT91C_UDP_RX_DATA_BK1;
 362                         else
 363                                 bank = AT91C_UDP_RX_DATA_BK0;
 364                 }
 365                 if (time_out++ == 0x1fff) break;
 366         }
 367
 368         btReceiveBank = bank;
 369         return nbBytesRcv;
 370 }
 371
 372 //*----------------------------------------------------------------------------
 373 //* \fn    usb_write
 374 //* \brief Send through endpoint 2
 375 //*----------------------------------------------------------------------------
 376 uint32_t usb_write(const byte_t* data, const size_t len) {
 377         size_t length = len;
 378         uint32_t cpt = 0;
 379
 380         if (!length) return 0;
 381         if (!usb_check()) return 0;
 382
 383         // Send the first packet
 384         cpt = MIN(length, AT91C_EP_IN_SIZE-1);
 385         length -= cpt;
 386         while (cpt--) pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
 387
 388         UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY)
 389         //pUdp->UDP_CSR[AT91C_EP_IN] |= AT91C_UDP_TXPKTRDY;
 390
 391         while (length) {
 392                 // Fill the second bank
 393                 cpt = MIN(length, AT91C_EP_IN_SIZE-1);
 394                 length -= cpt;
 395                 while (cpt--) pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
 396                 // Wait for the first bank to be sent
 397                 while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 398                         if (!usb_check()) return length;
 399                 }
 400
 401                 UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP)
 402                 //pUdp->UDP_CSR[AT91C_EP_IN] &= ~(AT91C_UDP_TXCOMP);
 403                 //while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP);
 404
 405                 UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY)
 406                 //pUdp->UDP_CSR[AT91C_EP_IN] |= AT91C_UDP_TXPKTRDY;
 407         }
 408
 409         // Wait for the end of transfer
 410         while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 411                 if (!usb_check()) return length;
 412         }
 413
 414         UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP)
 415         //pUdp->UDP_CSR[AT91C_EP_IN] &= ~(AT91C_UDP_TXCOMP);
 416         //while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP);
 417
 418         return length;
 419 }
 420
 421 //*----------------------------------------------------------------------------
 422 //* \fn    AT91F_USB_SendData
 423 //* \brief Send Data through the control endpoint
 424 //*----------------------------------------------------------------------------
 425 unsigned int csrTab[100] = {0x00};
 426 unsigned char csrIdx = 0;
 427
 428 static void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, uint32_t length) {
 429         uint32_t cpt = 0;
 430         AT91_REG csr;
 431
 432         do {
 433                 cpt = MIN(length, 8);
 434                 length -= cpt;
 435
 436                 while (cpt--)
 437                         pUdp->UDP_FDR[0] = *pData++;
 438
 439                 if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
 440
 441                         UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_TXCOMP)
 442                         //pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
 443                         //while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP);
 444                 }
 445
 446                 UDP_SET_EP_FLAGS(0, AT91C_UDP_TXPKTRDY)
 447                 // pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
 448                 do {
 449                         csr = pUdp->UDP_CSR[0];
 450
 451                         // Data IN stage has been stopped by a status OUT
 452                         if (csr & AT91C_UDP_RX_DATA_BK0) {
 453
 454                                 UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_RX_DATA_BK0)
 455                                 //pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
 456                                 return;
 457                         }
 458                 } while ( !(csr & AT91C_UDP_TXCOMP) );
 459
 460         } while (length);
 461
 462         if (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) {
 463
 464                 UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_TXCOMP)
 465                 //pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
 466                 //while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP);
 467         }
 468 }
 469
 470 //*----------------------------------------------------------------------------
 471 //* \fn    AT91F_USB_SendZlp
 472 //* \brief Send zero length packet through the control endpoint
 473 //*----------------------------------------------------------------------------
 474 void AT91F_USB_SendZlp(AT91PS_UDP pUdp) {
 475
 476         UDP_SET_EP_FLAGS(0, AT91C_UDP_TXPKTRDY)
 477         //pUdp->UDP_CSR[0] |= AT91C_UDP_TXPKTRDY;
 478
 479         while ( !(pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP) );
 480
 481         UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_TXCOMP)
 482         //pUdp->UDP_CSR[0] &= ~(AT91C_UDP_TXCOMP);
 483         //while (pUdp->UDP_CSR[0] & AT91C_UDP_TXCOMP);
 484 }
 485
 486 //*----------------------------------------------------------------------------
 487 //* \fn    AT91F_USB_SendStall
 488 //* \brief Stall the control endpoint
 489 //*----------------------------------------------------------------------------
 490 void AT91F_USB_SendStall(AT91PS_UDP pUdp) {
 491         pUdp->UDP_CSR[0] |= AT91C_UDP_FORCESTALL;
 492         while ( !(pUdp->UDP_CSR[0] & AT91C_UDP_ISOERROR) );
 493
 494         pUdp->UDP_CSR[0] &= ~(AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR);
 495         while (pUdp->UDP_CSR[0] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR));
 496 }
 497
 498 //*----------------------------------------------------------------------------
 499 //* \fn    AT91F_CDC_Enumerate
 500 //* \brief This function is a callback invoked when a SETUP packet is received
 501 //*----------------------------------------------------------------------------
 502 void AT91F_CDC_Enumerate() {
 503         byte_t bmRequestType, bRequest;
 504         uint16_t wValue, wIndex, wLength, wStatus;
 505
 506         if ( !(pUdp->UDP_CSR[0] & AT91C_UDP_RXSETUP) )
 507                 return;
 508
 509         bmRequestType = pUdp->UDP_FDR[0];
 510         bRequest      = pUdp->UDP_FDR[0];
 511         wValue        = (pUdp->UDP_FDR[0] & 0xFF);
 512         wValue       |= (pUdp->UDP_FDR[0] << 8);
 513         wIndex        = (pUdp->UDP_FDR[0] & 0xFF);
 514         wIndex       |= (pUdp->UDP_FDR[0] << 8);
 515         wLength       = (pUdp->UDP_FDR[0] & 0xFF);
 516         wLength      |= (pUdp->UDP_FDR[0] << 8);
 517
 518         if (bmRequestType & 0x80) {
 519
 520                 UDP_SET_EP_FLAGS(0, AT91C_UDP_DIR)
 521                 //pUdp->UDP_CSR[0] |= AT91C_UDP_DIR;
 522                 //while ( !(pUdp->UDP_CSR[0] & AT91C_UDP_DIR) );
 523         }
 524
 525         UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_RXSETUP)
 526         //pUdp->UDP_CSR[0] &= ~AT91C_UDP_RXSETUP;
 527         //while ( (pUdp->UDP_CSR[0]  & AT91C_UDP_RXSETUP)  );
 528
 529         // Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1
 530         switch ((bRequest << 8) | bmRequestType) {
 531         case STD_GET_DESCRIPTOR:
 532                 if (wValue == 0x100)       // Return Device Descriptor
 533                         AT91F_USB_SendData(pUdp, devDescriptor, MIN(sizeof(devDescriptor), wLength));
 534                 else if (wValue == 0x200)  // Return Configuration Descriptor
 535                         AT91F_USB_SendData(pUdp, cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength));
 536                 else if ((wValue & 0x300) == 0x300)  // Return String Descriptor
 537                         AT91F_USB_SendData(pUdp, strDescriptor, MIN(sizeof(strDescriptor), wLength));
 538                 else
 539                         AT91F_USB_SendStall(pUdp);
 540                 break;
 541         case STD_SET_ADDRESS:
 542                 AT91F_USB_SendZlp(pUdp);
 543                 pUdp->UDP_FADDR = (AT91C_UDP_FEN | wValue);
 544                 pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_FADDEN : 0;
 545                 break;
 546         case STD_SET_CONFIGURATION:
 547                 btConfiguration = wValue;
 548                 AT91F_USB_SendZlp(pUdp);
 549                 pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
 550                 pUdp->UDP_CSR[1] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0;
 551                 pUdp->UDP_CSR[2] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN)  : 0;
 552                 pUdp->UDP_CSR[3] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN)   : 0;
 553                 break;
 554         case STD_GET_CONFIGURATION:
 555                 AT91F_USB_SendData(pUdp, (char *) &(btConfiguration), sizeof(btConfiguration));
 556                 break;
 557         case STD_GET_STATUS_ZERO:
 558                 wStatus = 0;
 559                 AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 560                 break;
 561         case STD_GET_STATUS_INTERFACE:
 562                 wStatus = 0;
 563                 AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 564                 break;
 565         case STD_GET_STATUS_ENDPOINT:
 566                 wStatus = 0;
 567                 wIndex &= 0x0F;
 568                 if ((pUdp->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= 3)) {
 569                         wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
 570                         AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 571                 }
 572                 else if ((pUdp->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == 0)) {
 573                         wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
 574                         AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 575                 }
 576                 else
 577                         AT91F_USB_SendStall(pUdp);
 578                 break;
 579         case STD_SET_FEATURE_ZERO:
 580                 AT91F_USB_SendStall(pUdp);
 581             break;
 582         case STD_SET_FEATURE_INTERFACE:
 583                 AT91F_USB_SendZlp(pUdp);
 584                 break;
 585         case STD_SET_FEATURE_ENDPOINT:
 586                 wIndex &= 0x0F;
 587                 if ((wValue == 0) && wIndex && (wIndex <= 3)) {
 588                         pUdp->UDP_CSR[wIndex] = 0;
 589                         AT91F_USB_SendZlp(pUdp);
 590                 }
 591                 else
 592                         AT91F_USB_SendStall(pUdp);
 593                 break;
 594         case STD_CLEAR_FEATURE_ZERO:
 595                 AT91F_USB_SendStall(pUdp);
 596             break;
 597         case STD_CLEAR_FEATURE_INTERFACE:
 598                 AT91F_USB_SendZlp(pUdp);
 599                 break;
 600         case STD_CLEAR_FEATURE_ENDPOINT:
 601                 wIndex &= 0x0F;
 602                 if ((wValue == 0) && wIndex && (wIndex <= 3)) {
 603                         if (wIndex == 1)
 604                                 pUdp->UDP_CSR[1] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT);
 605                         else if (wIndex == 2)
 606                                 pUdp->UDP_CSR[2] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN);
 607                         else if (wIndex == 3)
 608                                 pUdp->UDP_CSR[3] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_ISO_IN);
 609                         AT91F_USB_SendZlp(pUdp);
 610                 }
 611                 else
 612                         AT91F_USB_SendStall(pUdp);
 613                 break;
 614
 615         // handle CDC class requests
 616         case SET_LINE_CODING:
 617                 while ( !(pUdp->UDP_CSR[0] & AT91C_UDP_RX_DATA_BK0) );
 618                 UDP_CLEAR_EP_FLAGS(0, AT91C_UDP_RX_DATA_BK0)
 619                 //pUdp->UDP_CSR[0] &= ~(AT91C_UDP_RX_DATA_BK0);
 620                 AT91F_USB_SendZlp(pUdp);
 621                 break;
 622         case GET_LINE_CODING:
 623                 AT91F_USB_SendData(pUdp, (char *) &line, MIN(sizeof(line), wLength));
 624                 break;
 625         case SET_CONTROL_LINE_STATE:
 626                 btConnection = wValue;
 627                 AT91F_USB_SendZlp(pUdp);
 628                 break;
 629         default:
 630                 AT91F_USB_SendStall(pUdp);
 631             break;
 632         }
 633 }