common_arm/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 "proxmark3_arm.h"
  37 #include "usart_defs.h"
  38
  39 /*
  40 AT91SAM7S256  USB Device Port
  41 • Embedded 328-byte dual-port RAM for endpoints
  42 • Four endpoints
  43 – Endpoint 0: 8 bytes
  44 – Endpoint 1 and 2: 64 bytes ping-pong
  45 – Endpoint 3: 64 bytes
  46 – Ping-pong Mode (two memory banks) for bulk endpoints
  47 */
  48
  49 //
  50 #define AT91C_EP_CONTROL        0
  51 #define AT91C_EP_OUT            1  // cfg bulk out
  52 #define AT91C_EP_IN             2  // cfg bulk in
  53 #define AT91C_EP_NOTIFY         3  // cfg cdc notification interrup
  54
  55 #define AT91C_EP_CONTROL_SIZE   8
  56 #define AT91C_EP_OUT_SIZE       64
  57 #define AT91C_EP_IN_SIZE        64
  58
  59
  60 // Section: USB Descriptors
  61 #define USB_DESCRIPTOR_DEVICE           0x01    // DescriptorType for a Device Descriptor.
  62 #define USB_DESCRIPTOR_CONFIGURATION    0x02    // DescriptorType for a Configuration Descriptor.
  63 #define USB_DESCRIPTOR_STRING           0x03    // DescriptorType for a String Descriptor.
  64 #define USB_DESCRIPTOR_INTERFACE        0x04    // DescriptorType for an Interface Descriptor.
  65 #define USB_DESCRIPTOR_ENDPOINT         0x05    // DescriptorType for an Endpoint Descriptor.
  66 #define USB_DESCRIPTOR_DEVICE_QUALIFIER 0x06    // DescriptorType for a Device Qualifier.
  67 #define USB_DESCRIPTOR_OTHER_SPEED      0x07    // DescriptorType for a Other Speed Configuration.
  68 #define USB_DESCRIPTOR_INTERFACE_POWER  0x08    // DescriptorType for Interface Power.
  69 #define USB_DESCRIPTOR_OTG              0x09    // DescriptorType for an OTG Descriptor.
  70 #define USB_DESCRIPTOR_IAD              0x0B    // DescriptorType for a Interface Association Descriptor
  71 #define USB_DESCRIPTOR_TYPE_BO          0x0F    // DescriptorType for a BOS Descriptor.
  72
  73 /* Configuration Attributes */
  74 #define _DEFAULT    (0x01<<7)       //Default Value (Bit 7 is set)
  75 #define _SELF       (0x01<<6)       //Self-powered (Supports if set)
  76 #define _RWU        (0x01<<5)       //Remote Wakeup (Supports if set)
  77 #define _HNP        (0x01 << 1)     //HNP (Supports if set)
  78 #define _SRP        (0x01)          //SRP (Supports if set)
  79
  80 /* Endpoint Transfer Type */
  81 #define _CTRL       0x00            //Control Transfer
  82 #define _ISO        0x01            //Isochronous Transfer
  83 #define _BULK       0x02            //Bulk Transfer
  84 #define _INTERRUPT  0x03            //Interrupt Transfer
  85
  86 // (bit7 | 0 = OUT, 1 = IN)
  87 #define _EP_IN      0x80
  88 #define _EP_OUT     0x00
  89 #define _EP01_OUT   0x01
  90 #define _EP01_IN    0x81
  91 #define _EP02_OUT   0x02
  92 #define _EP02_IN    0x82
  93 #define _EP03_OUT   0x03
  94 #define _EP03_IN    0x83
  95
  96
  97 /* WCID specific Request Code */
  98 #define MS_OS_DESCRIPTOR_INDEX          0xEE
  99 #define MS_VENDOR_CODE                  0x1C
 100 #define MS_EXTENDED_COMPAT_ID           0x04
 101 #define MS_EXTENDED_PROPERTIES          0x05
 102 #define MS_WCID_GET_DESCRIPTOR          0xC0
 103 #define MS_WCID_GET_FEATURE_DESCRIPTOR  0xC1
 104
 105 /* USB standard request code */
 106 #define STD_GET_STATUS_ZERO           0x0080
 107 #define STD_GET_STATUS_INTERFACE      0x0081
 108 #define STD_GET_STATUS_ENDPOINT       0x0082
 109
 110 #define STD_CLEAR_FEATURE_ZERO        0x0100
 111 #define STD_CLEAR_FEATURE_INTERFACE   0x0101
 112 #define STD_CLEAR_FEATURE_ENDPOINT    0x0102
 113
 114 #define STD_SET_FEATURE_ZERO          0x0300
 115 #define STD_SET_FEATURE_INTERFACE     0x0301
 116 #define STD_SET_FEATURE_ENDPOINT      0x0302
 117
 118 #define STD_SET_ADDRESS               0x0500
 119 #define STD_GET_DESCRIPTOR            0x0680
 120 #define STD_SET_DESCRIPTOR            0x0700
 121 #define STD_GET_CONFIGURATION         0x0880
 122 #define STD_SET_CONFIGURATION         0x0900
 123 #define STD_GET_INTERFACE             0x0A81
 124 #define STD_SET_INTERFACE             0x0B01
 125 #define STD_SYNCH_FRAME               0x0C82
 126
 127 /* CDC Class Specific Request Code */
 128 #define GET_LINE_CODING               0x21A1
 129 #define SET_LINE_CODING               0x2021
 130 #define SET_CONTROL_LINE_STATE        0x2221
 131
 132 AT91PS_UDP pUdp = AT91C_BASE_UDP;
 133 static uint8_t btConfiguration = 0;
 134 static uint8_t btConnection    = 0;
 135 static uint8_t btReceiveBank   = AT91C_UDP_RX_DATA_BK0;
 136
 137 static const char devDescriptor[] = {
 138     /* Device descriptor */
 139     0x12,      // Length
 140     USB_DESCRIPTOR_DEVICE,      // Descriptor Type (DEVICE)
 141     0x00, 0x02, // Complies with USB Spec. Release (0200h = release 2.00)  0210 == release 2.10
 142     2,      // Device Class:    Communication Device Class
 143     0,      // Device Subclass: CDC class sub code ACM [ice 0x02 = win10 virtual comport ]
 144     0,      // Device Protocol: CDC Device protocol (unused)
 145     AT91C_EP_CONTROL_SIZE,      // MaxPacketSize0
 146     0xc4, 0x9a, // Vendor ID  [0x9ac4 = J. Westhues]
 147     0x8f, 0x4b, // Product ID [0x4b8f = Proxmark-3 RFID Instrument]
 148     0x00, 0x01, // BCD Device release number (1.00)
 149     1,      // index Manufacturer
 150     2,      // index Product
 151     3,      // index SerialNumber
 152     1       // Number of Configs
 153 };
 154
 155 static const char cfgDescriptor[] = {
 156
 157     /* Configuration 1 descriptor */
 158     // -----------------------------
 159     9,         // Length
 160     USB_DESCRIPTOR_CONFIGURATION, // Descriptor Type
 161     (9 + 9 + 5 + 5 + 4 + 5 + 7 + 9 + 7 + 7), 0, // Total Length 2 EP + Control
 162     2,         // Number of Interfaces
 163     1,         // Index value of this Configuration (used in SetConfiguration from Host)
 164     0,         // Configuration string index
 165     _DEFAULT,      // Attributes 0xA0
 166     0xFA,      // Max Power consumption
 167
 168     // IAD to associate the one CDC interface
 169     // --------------------------------------
 170     /*
 171         8,         // Length
 172         USB_DESCRIPTOR_IAD, // IAD_DESCRIPTOR (0x0B)
 173         0,         // CDC_INT_INTERFACE NUMBER  (
 174         2,         // IAD INTERFACE COUNT (two interfaces)
 175         2,         // Function Class: CDC_CLASS
 176         2,         // Function SubClass: ACM
 177         1,         // Function Protocol: v.25term
 178         0,         // iInterface
 179     */
 180
 181     /* Interface 0 Descriptor */
 182     /* CDC Communication Class Interface Descriptor Requirement for Notification*/
 183     // -----------------------------------------------------------
 184     9,         // Length
 185     USB_DESCRIPTOR_INTERFACE, // Descriptor Type
 186     0,         // Interface Number
 187     0,         // Alternate Setting
 188     1,         // Number of Endpoints in this interface
 189     2,         // Interface Class code    (Communication Interface Class)
 190     2,         // Interface Subclass code (Abstract Control Model)
 191     1,         // InterfaceProtocol       (Common AT Commands, V.25term)
 192     0,         // iInterface
 193
 194     /* Header Functional Descriptor */
 195     5,         // Function Length
 196     0x24,      // Descriptor type:    CS_INTERFACE
 197     0,         // Descriptor subtype: Header Functional Descriptor
 198     0x10, 0x01, // bcd CDC:1.1
 199
 200     /* ACM Functional Descriptor */
 201     4,         // Function Length
 202     0x24,      // Descriptor Type:    CS_INTERFACE
 203     2,         // Descriptor Subtype: Abstract Control Management Functional Descriptor
 204     2,         // Capabilities        D1, Device supports the request combination of Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, and the notification Serial_State
 205
 206     /* Union Functional Descriptor */
 207     5,         // Function Length
 208     0x24,      // Descriptor Type:    CS_INTERFACE
 209     6,         // Descriptor Subtype: Union Functional Descriptor
 210     0,         // MasterInterface:    Communication Class Interface
 211     1,         // SlaveInterface0:    Data Class Interface
 212
 213     /* Call Management Functional Descriptor */
 214     5,         // Function Length
 215     0x24,      // Descriptor Type:    CS_INTERFACE
 216     1,         // Descriptor Subtype: Call Management Functional Descriptor
 217     0,         // Capabilities:       Device sends/receives call management information only over the Communication Class interface. Device does not handle call management itself
 218     1,         // Data Interface:     Data Class Interface
 219
 220     /* Protocol Functional Descriptor */
 221     /*
 222     6,
 223     0x24,      // Descriptor Type: CS_INTERFACE
 224     0x0B,      // Descriptor Subtype: Protocol Unit functional Descriptor
 225     0xDD,      // constant uniq ID of unit
 226     0xFE,      // protocol
 227     */
 228
 229     /* CDC Notification Endpoint descriptor */
 230     // ---------------------------------------
 231     7,                           // Length
 232     USB_DESCRIPTOR_ENDPOINT,     // Descriptor Type
 233     _EP03_IN,                    // EndpointAddress:   Endpoint 03 - IN
 234     _INTERRUPT,                  // Attributes
 235     AT91C_EP_CONTROL_SIZE, 0x00, // MaxPacket Size:    EP0 - 8
 236     0xFF,                        // Interval polling
 237
 238
 239     /* Interface 1 Descriptor */
 240     /* CDC Data Class Interface 1 Descriptor Requirement */
 241     9,                           // Length
 242     USB_DESCRIPTOR_INTERFACE,    // Descriptor Type
 243     1,                           // Interface Number
 244     0,                           // Alternate Setting
 245     2,                           // Number of Endpoints
 246     0x0A,                        // Interface Class:     CDC Data interface class
 247     0,                           // Interface Subclass:  not used
 248     0,                           // Interface Protocol:  No class specific protocol required (usb spec)
 249     0,                           // Interface
 250
 251     /* Endpoint descriptor */
 252     7,                           // Length
 253     USB_DESCRIPTOR_ENDPOINT,     // Descriptor Type
 254     _EP01_OUT,                   // Endpoint Address:    Endpoint 01 - OUT
 255     _BULK,                       // Attributes:          BULK
 256     AT91C_EP_OUT_SIZE, 0x00,     // MaxPacket Size:      64 bytes
 257     0,                           // Interval:            ignored for bulk
 258
 259     /* Endpoint descriptor */
 260     7,                           // Length
 261     USB_DESCRIPTOR_ENDPOINT,     // Descriptor Type
 262     _EP02_IN,                    // Endpoint Address:    Endpoint 02 - IN
 263     _BULK,                       // Attribute:           BULK
 264     AT91C_EP_IN_SIZE, 0x00,      // MaxPacket Size:      64 bytes
 265     0                            // Interval:            ignored for bulk
 266 };
 267
 268 // BOS descriptor
 269 static const char bosDescriptor[] = {
 270     0x5,
 271     USB_DESCRIPTOR_TYPE_BO,
 272     0xC,
 273     0x0,
 274     0x1,  // 1 device capability
 275     0x7,
 276     0x10, // USB_DEVICE_CAPABITY_TYPE,
 277     0x2,
 278     0x2,  // LPM capability bit set
 279     0x0,
 280     0x0,
 281     0x0
 282 };
 283
 284 // Microsoft OS Extended Configuration Compatible ID Descriptor
 285 /*
 286 static const char CompatIDFeatureDescriptor[] = {
 287         0x28, 0x00, 0x00, 0x00,                         // Descriptor Length 40bytes (0x28)
 288         0x00, 0x01,                                     // Version ('1.0')
 289         MS_EXTENDED_COMPAT_ID, 0x00,                    // Compatibility ID Descriptor Index  0x0004
 290         0x01,                                           // Number of sections. 0x1
 291         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,       // Reserved (7bytes)
 292         // -----function section 1------
 293         0x00,                                           // Interface Number #0
 294         0x01,                                           // reserved (0x1)
 295         0x57, 0x49, 0x4E, 0x55, 0x53, 0x42, 0x00, 0x00, // Compatible ID  ('WINUSB\0\0')  (8bytes)
 296         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-Compatible ID (8byte)
 297         0x00, 0x00, 0x00, 0x00, 0x00, 0x00              // Reserved (6bytes)
 298 };
 299 */
 300
 301 // Microsoft Extended Properties Feature Descriptor
 302 /*
 303 static const char OSprop[] = {
 304         // u32 Descriptor Length (10+132+64+102 == 308
 305         0x34, 0x01, 0, 0,
 306         // u16 Version ('1.0')
 307         0, 1,
 308         // u16 wIndex
 309         MS_EXTENDED_PROPERTIES, 0,
 310         // u16 wCount  -- three section
 311         3, 0,
 312
 313         // -----property section 1------
 314         // u32 size  ( 14+40+78 == 132)
 315         132, 0, 0, 0,
 316         // u32 type
 317         1, 0, 0, 0,  // unicode string
 318         // u16 namelen  (20*2 = 40)
 319         40, 0,
 320         // name  DeviceInterfaceGUID
 321         'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,'I',0,'n',0,'t',0,'e',0,'r',0,'f',0,'a',0,'c',0,'e',0,'G',0,'U',0,'I',0,'D',0,0,0,
 322         // u32 datalen  (39*2 = 78)
 323         78, 0, 0, 0,
 324         // data {4D36E978-E325-11CE-BFC1-08002BE10318}
 325         '{',0,'4',0,'d',0,'3',0,'6',0,'e',0,'9',0,'7',0,'8',0,'-',0,'e',0,'3',0,'2',0,'5',0,
 326         '-',0,'1',0,'1',0,'c',0,'e',0,'-',0,'b',0,'f',0,'c',0,'1',0,'-',0,'0',0,'8',0,'0',0,
 327         '0',0,'2',0,'b',0,'e',0,'1',0,'0',0,'3',0,'1',0,'8',0,'}',0,0,0,
 328
 329         // -----property section 2------
 330         // u32 size  ( 14+12+38 == 64)
 331         64, 0, 0, 0,
 332         // u32 type
 333         1, 0, 0, 0,  // unicode string
 334         // u16 namelen (12)
 335         12, 0,
 336         // name Label
 337         'L',0,'a',0,'b',0,'e',0,'l',0,0,0,
 338         // u32 datalen ( 19*2 = 38 )
 339         38, 0, 0, 0,
 340         // data 'Awesome PM3 Device'
 341         'A',0,'w',0,'e',0,'s',0,'o',0,'m',0,'e',0,' ',0,'P',0,'M',0,'3',0,' ',0,'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,0,0,
 342
 343         // -----property section 3------
 344         // u32 size ( 14+12+76 == 102)
 345         102, 0, 0, 0,
 346         // u32 type
 347         2, 0, 0, 0,  //Unicode string with environment variables
 348         // u16 namelen (12)
 349         12, 0,
 350         // name Icons
 351         'I',0,'c',0,'o',0,'n',0,'s',0,0,0,
 352         // u32 datalen ( 38*2 ==  76)
 353         76, 0, 0, 0,
 354         // data '%SystemRoot%\\system32\\Shell32.dll,-13'
 355         '%',0,'S',0,'y',0,'s',0,'t',0,'e',0,'m',0,'R',0,'o',0,'o',0,'t',0,'%',0,
 356         '\\',0,'s',0,'y',0,'s',0,'t',0,'e',0,'m',0,'3',0,'2',0,'\\',0,
 357         'S',0,'h',0,'e',0,'l',0,'l',0,'3',0,'2',0,'.',0,'d',0,'l',0,'l',0,',',0,
 358         '-',0,'1',0,'3',0,0,0
 359 };
 360
 361 */
 362
 363 static const char StrLanguageCodes[] = {
 364     4,          // Length
 365     0x03,       // Type is string
 366     0x09, 0x04  // supported language Code 0 = 0x0409 (English)
 367 };
 368
 369 // Note: ModemManager (Linux) ignores Proxmark3 devices by matching the
 370 // manufacturer string "proxmark.org". Don't change this.
 371 // or use the blacklisting file.
 372 static const char StrManufacturer[] = {
 373     26,         // Length
 374     0x03,       // Type is string
 375     'p', 0, 'r', 0, 'o', 0, 'x', 0, 'm', 0, 'a', 0, 'r', 0, 'k', 0, '.', 0, 'o', 0, 'r', 0, 'g', 0,
 376 };
 377
 378 static const char StrProduct[] = {
 379     20,         // Length
 380     0x03,       // Type is string
 381     'p', 0, 'r', 0, 'o', 0, 'x', 0, 'm', 0, 'a', 0, 'r', 0, 'k', 0, '3', 0
 382 };
 383
 384 static const char StrSerialNumber[] = {
 385     14,         // Length
 386     0x03,       // Type is string
 387     'i', 0, 'c', 0, 'e', 0, 'm', 0, 'a', 0, 'n', 0
 388 };
 389
 390 // size includes their own field.
 391 static const char StrMS_OSDescriptor[] = {
 392     18,         // length 0x12
 393     0x03,       // Type is string
 394     'M', 0, 'S', 0, 'F', 0, 'T', 0, '1', 0, '0', 0, '0', 0, MS_VENDOR_CODE, 0
 395 };
 396
 397 static const char *getStringDescriptor(uint8_t idx) {
 398     switch (idx) {
 399         case 0:
 400             return StrLanguageCodes;
 401         case 1:
 402             return StrManufacturer;
 403         case 2:
 404             return StrProduct;
 405         case 3:
 406             return StrSerialNumber;
 407         case MS_OS_DESCRIPTOR_INDEX:
 408             return StrMS_OSDescriptor;
 409         default:
 410             return (NULL);
 411     }
 412 }
 413
 414 // Bitmap for all status bits in CSR which must be written as 1 to cause no effect
 415 #define REG_NO_EFFECT_1_ALL      AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1 \
 416     |AT91C_UDP_STALLSENT   | AT91C_UDP_RXSETUP \
 417     |AT91C_UDP_TXCOMP
 418
 419 // Clear flags in the UDP_CSR register and waits for synchronization
 420 #define UDP_CLEAR_EP_FLAGS(endpoint, flags) { \
 421         volatile unsigned int reg; \
 422         reg = pUdp->UDP_CSR[(endpoint)]; \
 423         reg |= REG_NO_EFFECT_1_ALL; \
 424         reg &= ~(flags); \
 425         pUdp->UDP_CSR[(endpoint)] = reg; \
 426     }
 427
 428 // reset flags in the UDP_CSR register and waits for synchronization
 429 #define UDP_SET_EP_FLAGS(endpoint, flags) { \
 430         volatile unsigned int reg; \
 431         reg = pUdp->UDP_CSR[(endpoint)]; \
 432         reg |= REG_NO_EFFECT_1_ALL; \
 433         reg |= (flags); \
 434         pUdp->UDP_CSR[(endpoint)] = reg; \
 435     }
 436
 437
 438 typedef struct {
 439     uint32_t BitRate;
 440     uint8_t Format;
 441     uint8_t ParityType;
 442     uint8_t DataBits;
 443 } AT91S_CDC_LINE_CODING, *AT91PS_CDC_LINE_CODING;
 444
 445 AT91S_CDC_LINE_CODING line = { // purely informative, actual values don't matter
 446     USART_BAUD_RATE, // baudrate
 447     0,               // 1 Stop Bit
 448     0,               // None Parity
 449     8                // 8 Data bits
 450 };
 451
 452 // timer counts in 21.3us increments (1024/48MHz), rounding applies
 453 // WARNING: timer can't measure more than 1.39s (21.3us * 0xffff)
 454 static void SpinDelayUs(int us) {
 455     int ticks = ((MCK / 1000000) * us + 512) >> 10;
 456
 457     // Borrow a PWM unit for my real-time clock
 458     AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0);
 459
 460     // 48 MHz / 1024 gives 46.875 kHz
 461     AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10);      // Channel Mode Register
 462     AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0;                            // Channel Duty Cycle Register
 463     AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff;                       // Channel Period Register
 464
 465     uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 466
 467     for (;;) {
 468         uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR;
 469         if (now == (uint16_t)(start + ticks))
 470             return;
 471
 472         WDT_HIT();
 473     }
 474 }
 475
 476 /*
 477  *----------------------------------------------------------------------------
 478  * \fn    usb_disable
 479  * \brief This function deactivates the USB device
 480  *----------------------------------------------------------------------------
 481 */
 482 void usb_disable(void) {
 483     // Disconnect the USB device
 484     AT91C_BASE_PIOA->PIO_ODR = GPIO_USB_PU;
 485
 486     // Clear all lingering interrupts
 487     if (pUdp->UDP_ISR & AT91C_UDP_ENDBUSRES) {
 488         pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
 489     }
 490 }
 491
 492 /*
 493  *----------------------------------------------------------------------------
 494  * \fn    usb_enable
 495  * \brief This function Activates the USB device
 496  *----------------------------------------------------------------------------
 497 */
 498 void usb_enable(void) {
 499     // Set the PLL USB Divider
 500     AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1 ;
 501
 502     // Specific Chip USB Initialisation
 503     // Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock
 504     AT91C_BASE_PMC->PMC_SCER |= AT91C_PMC_UDP;
 505     AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP);
 506
 507     AT91C_BASE_UDP->UDP_FADDR = 0;
 508     AT91C_BASE_UDP->UDP_GLBSTATE = 0;
 509
 510     // Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO
 511     // Set in PIO mode and Configure in Output
 512     AT91C_BASE_PIOA->PIO_PER = GPIO_USB_PU; // Set in PIO mode
 513     AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; // Configure as Output
 514
 515     // Clear for set the Pullup resistor
 516     AT91C_BASE_PIOA->PIO_CODR = GPIO_USB_PU;
 517
 518     // Disconnect and reconnect USB controller for 100ms
 519     usb_disable();
 520
 521     SpinDelayUs(100 * 1000);
 522     // Wait for a short while
 523     //for (volatile size_t i=0; i<0x100000; i++) {};
 524
 525     // Reconnect USB reconnect
 526     AT91C_BASE_PIOA->PIO_SODR = GPIO_USB_PU;
 527     AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU;
 528 }
 529
 530 /*
 531  *----------------------------------------------------------------------------
 532  * \fn    usb_check
 533  * \brief Test if the device is configured and handle enumeration
 534  *----------------------------------------------------------------------------
 535 */
 536 static int usb_reconnect = 0;
 537 static int usb_configured = 0;
 538 void SetUSBreconnect(int value) {
 539     usb_reconnect = value;
 540 }
 541 int GetUSBreconnect(void) {
 542     return usb_reconnect;
 543 }
 544 void SetUSBconfigured(int value) {
 545     usb_configured = value;
 546 }
 547 int GetUSBconfigured(void) {
 548     return usb_configured;
 549 }
 550
 551 bool usb_check(void) {
 552
 553     /*
 554     // reconnected ONCE and
 555     if ( !USB_ATTACHED() ){
 556         usb_reconnect = 1;
 557         return false;
 558     }
 559
 560     // only one time after USB been disengaged and re-engaged
 561     if ( USB_ATTACHED() && usb_reconnect == 1 ) {
 562
 563         if ( usb_configured == 0) {
 564             usb_disable();
 565             usb_enable();
 566
 567             AT91F_CDC_Enumerate();
 568
 569             usb_configured = 1;
 570             return false;
 571         }
 572     }
 573     */
 574
 575     // interrupt status register
 576     AT91_REG isr = pUdp->UDP_ISR;
 577
 578     // end of bus reset
 579     if (isr & AT91C_UDP_ENDBUSRES) {
 580         pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES;
 581         // reset all endpoints
 582         pUdp->UDP_RSTEP  = (unsigned int) - 1;
 583         pUdp->UDP_RSTEP  = 0;
 584         // Enable the function
 585         pUdp->UDP_FADDR = AT91C_UDP_FEN;
 586         // Configure endpoint 0  (enable control endpoint)
 587         pUdp->UDP_CSR[AT91C_EP_CONTROL] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 588     } else if (isr & AT91C_UDP_EPINT0) {
 589         pUdp->UDP_ICR = AT91C_UDP_EPINT0;
 590         AT91F_CDC_Enumerate();
 591     }
 592     /*
 593     else if (isr & AT91C_UDP_EPINT3 ) {
 594         pUdp->UDP_ICR = AT91C_UDP_EPINT3;
 595         AT91F_CDC_Enumerate();
 596         //pUdp->UDP_ICR |= AT91C_UDP_EPINT3;
 597     }
 598     */
 599     return (btConfiguration) ? true : false;
 600 }
 601
 602 bool usb_poll(void) {
 603     if (!usb_check()) return false;
 604     return (pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank);
 605 }
 606
 607 /**
 608     In github PR #129, some users appears to get a false positive from
 609     usb_poll, which returns true, but the usb_read operation
 610     still returns 0.
 611     This check is basically the same as above, but also checks
 612     that the length available to read is non-zero, thus hopefully fixes the
 613     bug.
 614 **/
 615 bool usb_poll_validate_length(void) {
 616     if (!usb_check()) return false;
 617     if (!(pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false;
 618     return ((pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16) >  0;
 619 }
 620
 621 /*
 622  *----------------------------------------------------------------------------
 623  * \fn    usb_read
 624  * \brief Read available data from Endpoint 1 OUT (host to device)
 625  *----------------------------------------------------------------------------
 626 */
 627 uint32_t usb_read(uint8_t *data, size_t len) {
 628
 629     if (len == 0) return 0;
 630
 631     uint8_t bank = btReceiveBank;
 632     uint32_t packetSize, nbBytesRcv = 0;
 633     uint32_t time_out = 0;
 634
 635     while (len)  {
 636         if (!usb_check()) break;
 637
 638         if (pUdp->UDP_CSR[AT91C_EP_OUT] & bank) {
 639
 640             packetSize = (pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16;
 641             packetSize = MIN(packetSize, len);
 642             len -= packetSize;
 643             while (packetSize--)
 644                 data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT];
 645
 646             // flip bank
 647             UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank)
 648
 649             if (bank == AT91C_UDP_RX_DATA_BK0)
 650                 bank = AT91C_UDP_RX_DATA_BK1;
 651             else
 652                 bank = AT91C_UDP_RX_DATA_BK0;
 653         }
 654         if (time_out++ == 0x1fff) break;
 655     }
 656
 657     btReceiveBank = bank;
 658     return nbBytesRcv;
 659 }
 660
 661 static uint8_t usb_read_ng_buffer[64];
 662 static size_t usb_read_ng_bufoff = 0;
 663 static size_t usb_read_ng_buflen = 0;
 664
 665 uint32_t usb_read_ng(uint8_t *data, size_t len) {
 666
 667     if (len == 0) return 0;
 668
 669     uint8_t bank = btReceiveBank;
 670     uint32_t packetSize, nbBytesRcv = 0;
 671     uint32_t time_out = 0;
 672
 673     // take first from local buffer
 674     if (len <= usb_read_ng_buflen) {
 675         for (uint32_t i = 0; i < len; i++)
 676             data[nbBytesRcv++] = usb_read_ng_buffer[usb_read_ng_bufoff + i];
 677         usb_read_ng_buflen -= len;
 678         if (usb_read_ng_buflen == 0)
 679             usb_read_ng_bufoff = 0;
 680         else
 681             usb_read_ng_bufoff += len;
 682         return nbBytesRcv;
 683     } else {
 684         for (uint32_t i = 0; i < usb_read_ng_buflen; i++)
 685             data[nbBytesRcv++] = usb_read_ng_buffer[usb_read_ng_bufoff + i];
 686         len -= usb_read_ng_buflen;
 687         usb_read_ng_buflen = 0;
 688         usb_read_ng_bufoff = 0;
 689     }
 690
 691     while (len)  {
 692         if (!usb_check()) break;
 693
 694         if ((pUdp->UDP_CSR[AT91C_EP_OUT] & bank)) {
 695
 696             uint32_t available = (pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16;
 697             packetSize = MIN(available, len);
 698             available -= packetSize;
 699             len -= packetSize;
 700             while (packetSize--)
 701                 data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT];
 702             // fill the local buffer with the remaining bytes
 703             for (uint32_t i = 0; i < available; i++)
 704                 usb_read_ng_buffer[i] = pUdp->UDP_FDR[AT91C_EP_OUT];
 705             usb_read_ng_buflen = available;
 706             // flip bank
 707             UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank)
 708             if (bank == AT91C_UDP_RX_DATA_BK0)
 709                 bank = AT91C_UDP_RX_DATA_BK1;
 710             else
 711                 bank = AT91C_UDP_RX_DATA_BK0;
 712         }
 713         if (time_out++ == 0x1fff) break;
 714     }
 715
 716     btReceiveBank = bank;
 717     return nbBytesRcv;
 718 }
 719
 720 /*
 721  *----------------------------------------------------------------------------
 722  * \fn    usb_write
 723  * \brief Send through endpoint 2 (device to host)
 724  *----------------------------------------------------------------------------
 725 */
 726 int usb_write(const uint8_t *data, const size_t len) {
 727
 728     if (!len) return PM3_EINVARG;
 729     if (!usb_check()) return PM3_EIO;
 730
 731     // can we write?
 732     if ((pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY) != 0) return PM3_EIO;
 733
 734     size_t length = len;
 735     uint32_t cpt = 0;
 736
 737
 738     // send first chunk
 739     cpt = MIN(length, AT91C_EP_IN_SIZE);
 740     length -= cpt;
 741     while (cpt--) {
 742         pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
 743     }
 744
 745     UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
 746     while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY) {};
 747
 748     while (length) {
 749         // Send next chunk
 750         cpt = MIN(length, AT91C_EP_IN_SIZE);
 751         length -= cpt;
 752         while (cpt--) {
 753             pUdp->UDP_FDR[AT91C_EP_IN] = *data++;
 754         }
 755
 756         // Wait for previous chunk to be sent
 757         // (iceman) when is the bankswapping done?
 758         while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 759             if (!usb_check()) return PM3_EIO;
 760         }
 761
 762         UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
 763         while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP) {};
 764
 765         UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
 766         while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY) {};
 767     }
 768
 769     // Wait for the end of transfer
 770     while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {
 771         if (!usb_check()) return PM3_EIO;
 772     }
 773
 774     UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
 775     while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP) {};
 776
 777
 778     if (len % AT91C_EP_IN_SIZE == 0) {
 779
 780         UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY);
 781         while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) {};
 782
 783         UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP);
 784         while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP) {};
 785     }
 786
 787     return PM3_SUCCESS;
 788 }
 789
 790 /*
 791  *----------------------------------------------------------------------------
 792  * \fn    AT91F_USB_SendData
 793  * \brief Send Data through the control endpoint
 794  *----------------------------------------------------------------------------
 795 */
 796 void AT91F_USB_SendData(AT91PS_UDP pudp, const char *pData, uint32_t length) {
 797     AT91_REG csr;
 798
 799     do {
 800         uint32_t cpt = MIN(length, AT91C_EP_CONTROL_SIZE);
 801         length -= cpt;
 802
 803         while (cpt--)
 804             pudp->UDP_FDR[AT91C_EP_CONTROL] = *pData++;
 805
 806         if (pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
 807             UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
 808             while (pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP);
 809         }
 810
 811         UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY);
 812
 813         do {
 814             csr = pudp->UDP_CSR[AT91C_EP_CONTROL];
 815             // Data IN stage has been stopped by a status OUT
 816             if (csr & AT91C_UDP_RX_DATA_BK0) {
 817
 818                 UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0)
 819                 return;
 820             }
 821         } while (!(csr & AT91C_UDP_TXCOMP));
 822
 823     } while (length);
 824
 825     if (pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {
 826         UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
 827         while (pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP);
 828     }
 829 }
 830
 831
 832 //*----------------------------------------------------------------------------
 833 //* \fn    AT91F_USB_SendZlp
 834 //* \brief Send zero length packet through the control endpoint
 835 //*----------------------------------------------------------------------------
 836 void AT91F_USB_SendZlp(AT91PS_UDP pudp) {
 837     UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY);
 838     while (!(pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP)) {};
 839     UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP);
 840     while (pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) {};
 841 }
 842
 843 //*----------------------------------------------------------------------------
 844 //* \fn    AT91F_USB_SendStall
 845 //* \brief Stall the control endpoint
 846 //*----------------------------------------------------------------------------
 847 void AT91F_USB_SendStall(AT91PS_UDP pudp) {
 848     UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL);
 849     while (!(pudp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_ISOERROR)) {};
 850     UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR));
 851     while (pudp->UDP_CSR[AT91C_EP_CONTROL] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)) {};
 852 }
 853
 854 //*----------------------------------------------------------------------------
 855 //* \fn    AT91F_CDC_Enumerate
 856 //* \brief This function is a callback invoked when a SETUP packet is received
 857 //* problem:
 858 //* 1. this is for USB endpoint0.  the control endpoint.
 859 //* 2. mixed with CDC ACM endpoint3 , interrupt, control endpoint
 860 //*----------------------------------------------------------------------------
 861 void AT91F_CDC_Enumerate(void) {
 862     uint8_t bmRequestType, bRequest;
 863     uint16_t wValue, wIndex, wLength, wStatus;
 864
 865     if (!(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP))
 866         return;
 867
 868     bmRequestType = pUdp->UDP_FDR[AT91C_EP_CONTROL];
 869     bRequest      = pUdp->UDP_FDR[AT91C_EP_CONTROL];
 870     wValue        = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
 871     wValue       |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
 872     wIndex        = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
 873     wIndex       |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
 874     wLength       = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF);
 875     wLength      |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8);
 876
 877     if (bmRequestType & 0x80) {        // Data Phase Transfer Direction Device to Host
 878         UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_DIR);
 879         while (!(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_DIR)) {};
 880     }
 881     UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RXSETUP);
 882     while ((pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP)) {};
 883
 884     /*
 885     if ( bRequest == MS_VENDOR_CODE) {
 886         if ( bmRequestType == MS_WCID_GET_DESCRIPTOR ) { // C0
 887             if ( wIndex == MS_EXTENDED_COMPAT_ID ) {  // 4
 888                 //AT91F_USB_SendData(pUdp, CompatIDFeatureDescriptor, MIN(sizeof(CompatIDFeatureDescriptor), wLength));
 889                 //return;
 890             }
 891         }
 892
 893         if ( bmRequestType == MS_WCID_GET_FEATURE_DESCRIPTOR ) {  //C1
 894             // if ( wIndex == MS_EXTENDED_PROPERTIES ) { // 5  - winusb bug with wIndex == interface index,  so I just send it always)
 895                 //AT91F_USB_SendData(pUdp, OSprop, MIN(sizeof(OSprop), wLength));
 896                 //return;
 897             // }
 898         }
 899     }
 900     */
 901
 902     // Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1
 903     switch ((bRequest << 8) | bmRequestType) {
 904         case STD_GET_DESCRIPTOR: {
 905
 906             if (wValue == 0x100)         // Return Device Descriptor
 907                 AT91F_USB_SendData(pUdp, devDescriptor, MIN(sizeof(devDescriptor), wLength));
 908             else if (wValue == 0x200)    // Return Configuration Descriptor
 909                 AT91F_USB_SendData(pUdp, cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength));
 910             else if ((wValue & 0xF00) == 0xF00)  // Return BOS Descriptor
 911                 AT91F_USB_SendData(pUdp, bosDescriptor, MIN(sizeof(bosDescriptor), wLength));
 912             else if ((wValue & 0x300) == 0x300) {  // Return String Descriptor
 913
 914                 const char *strDescriptor = getStringDescriptor(wValue & 0xff);
 915                 if (strDescriptor != NULL) {
 916                     AT91F_USB_SendData(pUdp, strDescriptor, MIN(strDescriptor[0], wLength));
 917                 } else {
 918                     AT91F_USB_SendStall(pUdp);
 919                 }
 920             } else {
 921                 AT91F_USB_SendStall(pUdp);
 922             }
 923         }
 924         break;
 925         case STD_SET_ADDRESS:
 926             AT91F_USB_SendZlp(pUdp);
 927             pUdp->UDP_FADDR = (AT91C_UDP_FEN | (wValue & 0x7F));
 928             pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_FADDEN : 0;
 929             break;
 930         case STD_SET_CONFIGURATION:
 931
 932             /*
 933             *   Set or clear the device "configured" state.
 934             *   The LSB of wValue is the "Configuration Number". If this value is non-zero,
 935             *   it should be the same number as defined in the Configuration Descriptor;
 936             *   otherwise an error must have occurred.
 937             *   This device has only one configuration and its Config Number is CONF_NB (= 1).
 938             */
 939             AT91F_USB_SendZlp(pUdp);
 940             btConfiguration = wValue;
 941             pUdp->UDP_GLBSTATE  = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN;
 942
 943             // make sure we are not stalled
 944             /*
 945             UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT   , AT91C_UDP_FORCESTALL);
 946             UDP_CLEAR_EP_FLAGS(AT91C_EP_IN    , AT91C_UDP_FORCESTALL);
 947             UDP_CLEAR_EP_FLAGS(AT91C_EP_NOTIFY, AT91C_UDP_FORCESTALL);
 948             */
 949
 950             // enable endpoints
 951             pUdp->UDP_CSR[AT91C_EP_OUT]    = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0;
 952             pUdp->UDP_CSR[AT91C_EP_IN]     = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN)  : 0;
 953             pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN)   : 0;
 954             break;
 955         case STD_GET_CONFIGURATION:
 956             AT91F_USB_SendData(pUdp, (char *) & (btConfiguration), sizeof(btConfiguration));
 957             break;
 958         case STD_GET_STATUS_ZERO:
 959             wStatus = 0;   // Device is Bus powered, remote wakeup disabled
 960             AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 961             break;
 962         case STD_GET_STATUS_INTERFACE:
 963             wStatus = 0;   // reserved for future use
 964             AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 965             break;
 966         case STD_GET_STATUS_ENDPOINT:
 967             wStatus = 0;
 968             wIndex &= 0x0F;
 969             if ((pUdp->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= AT91C_EP_NOTIFY)) {
 970                 wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
 971                 AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 972             } else if ((pUdp->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == AT91C_EP_CONTROL)) {
 973                 wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1;
 974                 AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus));
 975             } else {
 976                 AT91F_USB_SendStall(pUdp);
 977             }
 978             break;
 979         case STD_SET_FEATURE_ZERO:
 980             AT91F_USB_SendStall(pUdp);
 981             break;
 982         case STD_SET_FEATURE_INTERFACE:
 983             AT91F_USB_SendZlp(pUdp);
 984             break;
 985         case STD_SET_FEATURE_ENDPOINT:
 986             wIndex &= 0x0F;
 987             if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
 988                 pUdp->UDP_CSR[wIndex] = 0;
 989                 AT91F_USB_SendZlp(pUdp);
 990             } else {
 991                 AT91F_USB_SendStall(pUdp);
 992             }
 993             break;
 994         case STD_CLEAR_FEATURE_ZERO:
 995             AT91F_USB_SendStall(pUdp);
 996             break;
 997         case STD_CLEAR_FEATURE_INTERFACE:
 998             AT91F_USB_SendZlp(pUdp);
 999             break;
1000         case STD_CLEAR_FEATURE_ENDPOINT:
1001             wIndex &= 0x0F;
1002             if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) {
1003
1004                 if (wIndex == AT91C_EP_OUT)         pUdp->UDP_CSR[AT91C_EP_OUT] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT);
1005                 else if (wIndex == AT91C_EP_IN)     pUdp->UDP_CSR[AT91C_EP_IN] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN);
1006                 else if (wIndex == AT91C_EP_NOTIFY) pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN);
1007
1008                 AT91F_USB_SendZlp(pUdp);
1009             } else {
1010                 AT91F_USB_SendStall(pUdp);
1011             }
1012             break;
1013
1014         // handle CDC class requests
1015         case SET_LINE_CODING: {
1016             /*
1017                 uint8_t i;
1018                 for ( i = 0 ; i < 7 ; i++ )  {
1019                     ((uint8_t*)&line)[i] =  pUdp->UDP_FDR[AT91C_EP_CONTROL];
1020                 }  */
1021             // ignore SET_LINE_CODING...
1022             while (!(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RX_DATA_BK0)) {};
1023             UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0);
1024             AT91F_USB_SendZlp(pUdp);
1025             break;
1026         }
1027         case GET_LINE_CODING:
1028             AT91F_USB_SendData(pUdp, (char *) &line, MIN(sizeof(line), wLength));
1029             break;
1030         case SET_CONTROL_LINE_STATE:
1031             btConnection = wValue;
1032             AT91F_USB_SendZlp(pUdp);
1033             break;
1034         default:
1035             AT91F_USB_SendStall(pUdp);
1036             break;
1037     }
1038 }