tools/tools/usbtest/usb_msc_test.c

   1 /* $FreeBSD$ */
   2 /*-
   3  * Copyright (c) 2007-2012 Hans Petter Selasky. All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  */
  26
  27 #include <stdio.h>
  28 #include <stdint.h>
  29 #include <stdlib.h>
  30 #include <err.h>
  31 #include <string.h>
  32 #include <errno.h>
  33 #include <unistd.h>
  34
  35 #include <sys/sysctl.h>
  36 #include <sys/time.h>
  37
  38 #include <libusb20.h>
  39 #include <libusb20_desc.h>
  40
  41 #include <dev/usb/usb_endian.h>
  42
  43 #include "usbtest.h"
  44
  45 #include "usb_msc_test.h"
  46
  47 /* Command Block Wrapper */
  48 typedef struct {
  49         uDWord  dCBWSignature;
  50 #define CBWSIGNATURE    0x43425355
  51         uDWord  dCBWTag;
  52         uDWord  dCBWDataTransferLength;
  53         uByte   bCBWFlags;
  54 #define CBWFLAGS_OUT    0x00
  55 #define CBWFLAGS_IN     0x80
  56         uByte   bCBWLUN;
  57         uByte   bCDBLength;
  58 #define CBWCDBLENGTH    16
  59         uByte   CBWCDB[CBWCDBLENGTH];
  60 } umass_bbb_cbw_t;
  61
  62 #define UMASS_BBB_CBW_SIZE      31
  63
  64 /* Command Status Wrapper */
  65 typedef struct {
  66         uDWord  dCSWSignature;
  67 #define CSWSIGNATURE    0x53425355
  68 #define CSWSIGNATURE_IMAGINATION_DBX1   0x43425355
  69 #define CSWSIGNATURE_OLYMPUS_C1 0x55425355
  70         uDWord  dCSWTag;
  71         uDWord  dCSWDataResidue;
  72         uByte   bCSWStatus;
  73 #define CSWSTATUS_GOOD  0x0
  74 #define CSWSTATUS_FAILED        0x1
  75 #define CSWSTATUS_PHASE 0x2
  76 } umass_bbb_csw_t;
  77
  78 #define UMASS_BBB_CSW_SIZE      13
  79
  80 #define SC_READ_6                       0x08
  81 #define SC_READ_10                      0x28
  82 #define SC_READ_12                      0xa8
  83 #define SC_WRITE_6                      0x0a
  84 #define SC_WRITE_10                     0x2a
  85 #define SC_WRITE_12                     0xaa
  86
  87 static struct stats {
  88         uint64_t xfer_error;
  89         uint64_t xfer_success;
  90         uint64_t xfer_reset;
  91         uint64_t xfer_rx_bytes;
  92         uint64_t xfer_tx_bytes;
  93         uint64_t data_error;
  94 }       stats;
  95
  96 static uint32_t xfer_current_id;
  97 static uint32_t xfer_wrapper_sig;
  98 static uint32_t block_size = 512;
  99
 100 static struct libusb20_transfer *xfer_in;
 101 static struct libusb20_transfer *xfer_out;
 102 static struct libusb20_device *usb_pdev;
 103 static uint8_t usb_iface;
 104 static int sense_recurse;
 105
 106 /*
 107  * SCSI commands sniffed off the wire - LUN maybe needs to be
 108  * adjusted!  Refer to "dev/usb/storage/ustorage_fs.c" for more
 109  * information.
 110  */
 111 static uint8_t mode_sense_6[0x6] = {0x1a, 0, 0x3f, 0, 0x0c};
 112 static uint8_t read_capacity[0xA] = {0x25,};
 113 static uint8_t request_sense[0xC] = {0x03, 0, 0, 0, 0x12};
 114 static uint8_t test_unit_ready[0x6] = {0};
 115 static uint8_t mode_page_inquiry[0x6] = {0x12, 1, 0x80, 0, 0xff, 0};
 116 static uint8_t request_invalid[0xC] = {0xEA, 0, 0, 0, 0};
 117 static uint8_t prevent_removal[0x6] = {0x1E, 0, 0, 0, 1};
 118 static uint8_t read_toc[0xA] = {0x43, 0x02, 0, 0, 0, 0xAA, 0, 0x0C};
 119
 120 #define TIMEOUT_FILTER(x) (x)
 121
 122 static void usb_request_sense(uint8_t lun);
 123
 124 static void
 125 do_msc_reset(uint8_t lun)
 126 {
 127         struct LIBUSB20_CONTROL_SETUP_DECODED setup;
 128
 129         LIBUSB20_INIT(LIBUSB20_CONTROL_SETUP, &setup);
 130
 131         setup.bmRequestType = LIBUSB20_REQUEST_TYPE_CLASS |
 132             LIBUSB20_RECIPIENT_INTERFACE;
 133         setup.bRequest = 0xFF;          /* BBB reset */
 134         setup.wValue = 0;
 135         setup.wIndex = usb_iface;
 136         setup.wLength = 0;
 137
 138         if (libusb20_dev_request_sync(usb_pdev, &setup, NULL, NULL, 5000, 0)) {
 139                 printf("ERROR: %s\n", __FUNCTION__);
 140                 stats.xfer_error++;
 141         }
 142         libusb20_tr_clear_stall_sync(xfer_in);
 143         libusb20_tr_clear_stall_sync(xfer_out);
 144
 145         stats.xfer_reset++;
 146
 147         usb_request_sense(lun);
 148 }
 149
 150 static uint8_t
 151 do_msc_cmd(uint8_t *pcmd, uint8_t cmdlen, void *pdata, uint32_t datalen,
 152     uint8_t isread, uint8_t isshort, uint8_t lun, uint8_t flags)
 153 {
 154         umass_bbb_cbw_t cbw;
 155         umass_bbb_csw_t csw;
 156         struct libusb20_transfer *xfer_io;
 157         uint32_t actlen;
 158         uint32_t timeout;
 159         int error;
 160
 161         memset(&cbw, 0, sizeof(cbw));
 162
 163         USETDW(cbw.dCBWSignature, xfer_wrapper_sig);
 164         USETDW(cbw.dCBWTag, xfer_current_id);
 165         xfer_current_id++;
 166         USETDW(cbw.dCBWDataTransferLength, datalen);
 167         cbw.bCBWFlags = (isread ? CBWFLAGS_IN : CBWFLAGS_OUT);
 168         cbw.bCBWLUN = lun;
 169         cbw.bCDBLength = cmdlen;
 170         bcopy(pcmd, cbw.CBWCDB, cmdlen);
 171
 172         actlen = 0;
 173
 174         timeout = ((datalen + 299999) / 300000) * 1000;
 175         timeout += 5000;
 176
 177         if ((error = libusb20_tr_bulk_intr_sync(xfer_out,
 178             &cbw, sizeof(cbw), &actlen, TIMEOUT_FILTER(1000)))) {
 179                 printf("ERROR: CBW reception: %d\n", error);
 180                 do_msc_reset(lun);
 181                 return (1);
 182         }
 183         if (actlen != sizeof(cbw)) {
 184                 printf("ERROR: CBW length: %d != %d\n",
 185                     actlen, (int)sizeof(cbw));
 186                 do_msc_reset(lun);
 187                 return (1);
 188         }
 189         if (flags & 1)
 190                 datalen /= 2;
 191
 192         if (datalen != 0) {
 193                 xfer_io = isread ? xfer_in : xfer_out;
 194
 195                 if ((error = libusb20_tr_bulk_intr_sync(xfer_io,
 196                     pdata, datalen, &actlen, TIMEOUT_FILTER(timeout)))) {
 197                         printf("ERROR: Data transfer: %d\n", error);
 198                         do_msc_reset(lun);
 199                         return (1);
 200                 }
 201                 if ((actlen != datalen) && (!isshort)) {
 202                         printf("ERROR: Short data: %d of %d bytes\n",
 203                             actlen, datalen);
 204                         do_msc_reset(lun);
 205                         return (1);
 206                 }
 207         }
 208         actlen = 0;
 209         timeout = 8;
 210
 211         do {
 212                 error = libusb20_tr_bulk_intr_sync(xfer_in, &csw,
 213                     sizeof(csw), &actlen, TIMEOUT_FILTER(1000));
 214                 if (error) {
 215                         if (error == LIBUSB20_TRANSFER_TIMED_OUT) {
 216                                 printf("TIMEOUT: Trying to get CSW again. "
 217                                     "%d tries left.\n", timeout);
 218                         } else {
 219                                 break;
 220                         }
 221                 } else {
 222                         break;
 223                 }
 224         } while (--timeout);
 225
 226         if (error) {
 227                 libusb20_tr_clear_stall_sync(xfer_in);
 228                 error = libusb20_tr_bulk_intr_sync(xfer_in, &csw,
 229                     sizeof(csw), &actlen, TIMEOUT_FILTER(1000));
 230                 if (error) {
 231                         libusb20_tr_clear_stall_sync(xfer_in);
 232                         printf("ERROR: Could not read CSW: Stalled or "
 233                             "timeout (%d).\n", error);
 234                         do_msc_reset(lun);
 235                         return (1);
 236                 }
 237         }
 238         if (UGETDW(csw.dCSWSignature) != CSWSIGNATURE) {
 239                 printf("ERROR: Wrong CSW signature\n");
 240                 do_msc_reset(lun);
 241                 return (1);
 242         }
 243         if (actlen != sizeof(csw)) {
 244                 printf("ERROR: Wrong CSW length: %d != %d\n",
 245                     actlen, (int)sizeof(csw));
 246                 do_msc_reset(lun);
 247                 return (1);
 248         }
 249         if (csw.bCSWStatus != 0) {
 250                 printf("ERROR: CSW status: %d\n", (int)csw.bCSWStatus);
 251                 return (1);
 252         } else {
 253                 stats.xfer_success++;
 254                 return (0);
 255         }
 256 }
 257
 258 static uint8_t
 259 do_read_10(uint32_t lba, uint32_t len, void *buf, uint8_t lun)
 260 {
 261         static uint8_t cmd[10];
 262         uint8_t retval;
 263
 264         cmd[0] = SC_READ_10;
 265
 266         len /= block_size;
 267
 268         cmd[2] = lba >> 24;
 269         cmd[3] = lba >> 16;
 270         cmd[4] = lba >> 8;
 271         cmd[5] = lba >> 0;
 272
 273         cmd[7] = len >> 8;
 274         cmd[8] = len;
 275
 276         retval = do_msc_cmd(cmd, 10, buf, len * block_size, 1, 0, lun, 0);
 277
 278         if (retval) {
 279                 printf("ERROR: %s\n", __FUNCTION__);
 280                 stats.xfer_error++;
 281         }
 282         return (retval);
 283 }
 284
 285 static uint8_t
 286 do_write_10(uint32_t lba, uint32_t len, void *buf, uint8_t lun)
 287 {
 288         static uint8_t cmd[10];
 289         uint8_t retval;
 290         uint8_t abort;
 291
 292         cmd[0] = SC_WRITE_10;
 293
 294         abort = len & 1;
 295
 296         len /= block_size;
 297
 298         cmd[2] = lba >> 24;
 299         cmd[3] = lba >> 16;
 300         cmd[4] = lba >> 8;
 301         cmd[5] = lba >> 0;
 302
 303         cmd[7] = len >> 8;
 304         cmd[8] = len;
 305
 306         retval = do_msc_cmd(cmd, 10, buf, (len * block_size), 0, 0, lun, abort);
 307
 308         if (retval) {
 309                 printf("ERROR: %s\n", __FUNCTION__);
 310                 stats.xfer_error++;
 311         }
 312         return (retval);
 313 }
 314
 315 static void
 316 do_io_test(struct usb_msc_params *p, uint8_t lun, uint32_t lba_max,
 317     uint8_t *buffer, uint8_t *reference)
 318 {
 319         uint32_t io_offset;
 320         uint32_t io_size;
 321         uint32_t temp;
 322         uint8_t do_read;
 323         uint8_t retval;
 324
 325         switch (p->io_mode) {
 326         case USB_MSC_IO_MODE_WRITE_ONLY:
 327                 do_read = 0;
 328                 break;
 329         case USB_MSC_IO_MODE_READ_WRITE:
 330                 do_read = (usb_ts_rand_noise() & 1);
 331                 break;
 332         default:
 333                 do_read = 1;
 334                 break;
 335         }
 336
 337         switch (p->io_offset) {
 338         case USB_MSC_IO_OFF_RANDOM:
 339                 io_offset = usb_ts_rand_noise();
 340                 break;
 341         default:
 342                 io_offset = 0;
 343                 break;
 344         }
 345
 346         switch (p->io_delay) {
 347         case USB_MSC_IO_DELAY_RANDOM_10MS:
 348                 usleep(((uint32_t)usb_ts_rand_noise()) % 10000U);
 349                 break;
 350         case USB_MSC_IO_DELAY_RANDOM_100MS:
 351                 usleep(((uint32_t)usb_ts_rand_noise()) % 100000U);
 352                 break;
 353         case USB_MSC_IO_DELAY_FIXED_10MS:
 354                 usleep(10000);
 355                 break;
 356         case USB_MSC_IO_DELAY_FIXED_100MS:
 357                 usleep(100000);
 358                 break;
 359         default:
 360                 break;
 361         }
 362
 363         switch (p->io_size) {
 364         case USB_MSC_IO_SIZE_RANDOM:
 365                 io_size = ((uint32_t)usb_ts_rand_noise()) & 65535U;
 366                 break;
 367         case USB_MSC_IO_SIZE_INCREASING:
 368                 io_size = (xfer_current_id & 65535U);
 369                 break;
 370         case USB_MSC_IO_SIZE_FIXED_1BLK:
 371                 io_size = 1;
 372                 break;
 373         case USB_MSC_IO_SIZE_FIXED_2BLK:
 374                 io_size = 2;
 375                 break;
 376         case USB_MSC_IO_SIZE_FIXED_4BLK:
 377                 io_size = 4;
 378                 break;
 379         case USB_MSC_IO_SIZE_FIXED_8BLK:
 380                 io_size = 8;
 381                 break;
 382         case USB_MSC_IO_SIZE_FIXED_16BLK:
 383                 io_size = 16;
 384                 break;
 385         case USB_MSC_IO_SIZE_FIXED_32BLK:
 386                 io_size = 32;
 387                 break;
 388         case USB_MSC_IO_SIZE_FIXED_64BLK:
 389                 io_size = 64;
 390                 break;
 391         case USB_MSC_IO_SIZE_FIXED_128BLK:
 392                 io_size = 128;
 393                 break;
 394         case USB_MSC_IO_SIZE_FIXED_256BLK:
 395                 io_size = 256;
 396                 break;
 397         case USB_MSC_IO_SIZE_FIXED_512BLK:
 398                 io_size = 512;
 399                 break;
 400         case USB_MSC_IO_SIZE_FIXED_1024BLK:
 401                 io_size = 1024;
 402                 break;
 403         default:
 404                 io_size = 1;
 405                 break;
 406         }
 407
 408         if (io_size == 0)
 409                 io_size = 1;
 410
 411         io_offset %= lba_max;
 412
 413         temp = (lba_max - io_offset);
 414
 415         if (io_size > temp)
 416                 io_size = temp;
 417
 418         if (do_read) {
 419                 retval = do_read_10(io_offset, io_size * block_size,
 420                     buffer + (io_offset * block_size), lun);
 421
 422                 if (retval == 0) {
 423                         if (bcmp(buffer + (io_offset * block_size),
 424                             reference + (io_offset * block_size),
 425                             io_size * block_size)) {
 426                                 printf("ERROR: Data comparison failure\n");
 427                                 stats.data_error++;
 428                                 retval = 1;
 429                         }
 430                 }
 431                 stats.xfer_rx_bytes += (io_size * block_size);
 432
 433         } else {
 434
 435                 retval = do_write_10(io_offset, io_size * block_size,
 436                     reference + (io_offset * block_size), lun);
 437
 438                 stats.xfer_tx_bytes += (io_size * block_size);
 439         }
 440
 441         if ((stats.xfer_error + stats.data_error +
 442             stats.xfer_reset) >= p->max_errors) {
 443                 printf("Maximum number of errors exceeded\n");
 444                 p->done = 1;
 445         }
 446 }
 447
 448 static void
 449 usb_request_sense(uint8_t lun)
 450 {
 451         uint8_t dummy_buf[255];
 452
 453         if (sense_recurse)
 454                 return;
 455
 456         sense_recurse++;
 457
 458         do_msc_cmd(request_sense, sizeof(request_sense),
 459             dummy_buf, 255, 1, 1, lun, 0);
 460
 461         sense_recurse--;
 462 }
 463
 464 static void
 465 usb_msc_test(struct usb_msc_params *p)
 466 {
 467         struct stats last_stat;
 468         struct timeval sub_tv;
 469         struct timeval ref_tv;
 470         struct timeval res_tv;
 471         uint8_t *buffer = NULL;
 472         uint8_t *reference = NULL;
 473         uint32_t dummy_buf[65536 / 4];
 474         uint32_t lba_max;
 475         uint32_t x;
 476         uint32_t y;
 477         uint32_t capacity_lba;
 478         uint32_t capacity_bs;
 479         time_t last_sec;
 480         uint8_t lun;
 481         int tries;
 482
 483         memset(&last_stat, 0, sizeof(last_stat));
 484
 485         switch (p->io_lun) {
 486         case USB_MSC_IO_LUN_0:
 487                 lun = 0;
 488                 break;
 489         case USB_MSC_IO_LUN_1:
 490                 lun = 1;
 491                 break;
 492         case USB_MSC_IO_LUN_2:
 493                 lun = 2;
 494                 break;
 495         case USB_MSC_IO_LUN_3:
 496                 lun = 3;
 497                 break;
 498         default:
 499                 lun = 0;
 500                 break;
 501         }
 502
 503         p->done = 0;
 504
 505         sense_recurse = p->try_sense_on_error ? 0 : 1;
 506
 507         printf("Resetting device ...\n");
 508
 509         do_msc_reset(lun);
 510
 511         printf("Testing SCSI commands ...\n");
 512
 513         if (p->try_all_lun) {
 514                 printf("Requesting sense from LUN 0..255 ... ");
 515                 for (x = y = 0; x != 256; x++) {
 516                         if (do_msc_cmd(mode_sense_6, sizeof(mode_sense_6),
 517                             dummy_buf, 255, 1, 1, x, 0))
 518                                 y++;
 519
 520                         if (libusb20_dev_check_connected(usb_pdev) != 0) {
 521                                 printf(" disconnect ");
 522                                 break;
 523                         }
 524                 }
 525                 printf("Passed=%d, Failed=%d\n", 256 - y, y);
 526         }
 527         do_msc_cmd(mode_sense_6, sizeof(mode_sense_6),
 528             dummy_buf, 255, 1, 1, lun, 0);
 529         do_msc_cmd(request_sense, sizeof(request_sense),
 530             dummy_buf, 255, 1, 1, lun, 0);
 531
 532         for (tries = 0; tries != 4; tries++) {
 533
 534                 memset(dummy_buf, 0, sizeof(dummy_buf));
 535
 536                 if (do_msc_cmd(read_capacity, sizeof(read_capacity),
 537                     dummy_buf, 255, 1, 1, lun, 0) != 0) {
 538                         printf("Cannot read disk capacity (%u / 4)\n", tries);
 539                         if (tries == 3)
 540                                 return;
 541                         usleep(50000);
 542                         continue;
 543                 } else {
 544                         break;
 545                 }
 546         }
 547
 548         capacity_lba = be32toh(dummy_buf[0]);
 549         capacity_bs = be32toh(dummy_buf[1]);
 550
 551         printf("Disk reports a capacity of LBA=%u and BS=%u\n",
 552             capacity_lba, capacity_bs);
 553
 554         block_size = capacity_bs;
 555
 556         if (capacity_bs > 65535) {
 557                 printf("Blocksize is too big\n");
 558                 return;
 559         }
 560         if (capacity_bs < 1) {
 561                 printf("Blocksize is too small\n");
 562                 return;
 563         }
 564         if (capacity_bs != 512)
 565                 printf("INFO: Blocksize is not 512 bytes\n");
 566
 567         if (p->try_invalid_scsi_command) {
 568                 int status;
 569
 570                 for (tries = 0; tries != 4; tries++) {
 571
 572                         printf("Trying invalid SCSI command: ");
 573
 574                         status = do_msc_cmd(request_invalid,
 575                             sizeof(request_invalid), dummy_buf,
 576                             255, 1, 1, lun, 0);
 577
 578                         printf("Result%s as expected\n", status ? "" : " NOT");
 579
 580                         usleep(50000);
 581                 }
 582         }
 583         if (p->try_invalid_wrapper_block) {
 584                 int status;
 585
 586                 for (tries = 0; tries != 4; tries++) {
 587
 588                         printf("Trying invalid USB wrapper block signature: ");
 589
 590                         xfer_wrapper_sig = 0x55663322;
 591
 592                         status = do_msc_cmd(read_capacity,
 593                             sizeof(read_capacity), dummy_buf,
 594                             255, 1, 1, lun, 0);
 595
 596                         printf("Result%s as expected\n", status ? "" : " NOT");
 597
 598                         xfer_wrapper_sig = CBWSIGNATURE;
 599
 600                         usleep(50000);
 601                 }
 602         }
 603         do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
 604         do_msc_cmd(read_capacity, sizeof(read_capacity), dummy_buf, 255, 1, 1, lun, 0);
 605         do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
 606         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 607         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 608         do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
 609         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 610         do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
 611         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 612         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 613         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 614         do_msc_cmd(request_sense, sizeof(request_sense), dummy_buf, 255, 1, 1, lun, 0);
 615         do_msc_cmd(test_unit_ready, sizeof(test_unit_ready), 0, 0, 1, 1, lun, 0);
 616         do_msc_cmd(read_capacity, sizeof(read_capacity), dummy_buf, 255, 1, 1, lun, 0);
 617         do_msc_cmd(mode_page_inquiry, sizeof(mode_page_inquiry), dummy_buf, 255, 1, 1, lun, 0);
 618         do_msc_cmd(mode_page_inquiry, sizeof(mode_page_inquiry), dummy_buf, 255, 1, 1, lun, 0);
 619
 620         if (do_msc_cmd(prevent_removal, sizeof(prevent_removal),
 621             0, 0, 1, 1, lun, 0)) {
 622                 printf("INFO: Prevent medium removal failed\n");
 623         }
 624         if (do_msc_cmd(read_toc, sizeof(read_toc),
 625             dummy_buf, 255, 1, 1, lun, 0)) {
 626                 printf("INFO: Read Table Of Content failed\n");
 627         }
 628         if (p->try_last_lba) {
 629
 630                 for (y = 0, x = (1UL << 31); x; x >>= 1) {
 631                         if (do_read_10(x | y, block_size, dummy_buf, lun) == 0)
 632                                 y |= x;
 633                 }
 634
 635                 printf("Highest readable LBA: %u (%s), "
 636                     "Capacity is %u MBytes\n", y,
 637                     (capacity_lba != y) ? "WRONG" : "OK",
 638                     (int)((((uint64_t)(y) * (uint64_t)block_size) +
 639                     (uint64_t)block_size) / 1000000ULL));
 640         } else {
 641
 642                 y = capacity_lba;
 643
 644                 printf("Highest readable LBA: %u (not "
 645                     "verified), Capacity is %u MBytes\n", y,
 646                     (int)((((uint64_t)(y) * (uint64_t)block_size) +
 647                     (uint64_t)block_size) / 1000000ULL));
 648         }
 649
 650         if (y != 0xFFFFFFFFU)
 651                 y++;
 652
 653         lba_max = y;
 654
 655         switch (p->io_area) {
 656         case USB_MSC_IO_AREA_1MB:
 657                 lba_max = 1024;
 658                 break;
 659         case USB_MSC_IO_AREA_16MB:
 660                 lba_max = 1024 * 16;
 661                 break;
 662         case USB_MSC_IO_AREA_256MB:
 663                 lba_max = 1024 * 256;
 664                 break;
 665         case USB_MSC_IO_AREA_COMPLETE:
 666         default:
 667                 break;
 668         }
 669
 670         if (lba_max > 65535)
 671                 lba_max = 65535;
 672
 673         printf("Highest testable LBA: %u\n", (int)lba_max);
 674
 675         buffer = malloc(block_size * lba_max);
 676         if (buffer == NULL) {
 677                 printf("ERROR: Could not allocate memory\n");
 678                 goto fail;
 679         }
 680         reference = malloc(block_size * lba_max);
 681         if (reference == NULL) {
 682                 printf("ERROR: Could not allocate memory\n");
 683                 goto fail;
 684         }
 685 retry_read_init:
 686
 687         printf("Setting up initial data pattern, "
 688             "LBA limit = %u ... ", lba_max);
 689
 690         switch (p->io_mode) {
 691         case USB_MSC_IO_MODE_WRITE_ONCE_READ_ONLY:
 692         case USB_MSC_IO_MODE_WRITE_ONLY:
 693         case USB_MSC_IO_MODE_READ_WRITE:
 694
 695                 switch (p->io_pattern) {
 696                 case USB_MSC_IO_PATTERN_FIXED:
 697                         for (x = 0; x != (block_size * lba_max); x += 8) {
 698                                 reference[x + 0] = x >> 24;
 699                                 reference[x + 1] = x >> 16;
 700                                 reference[x + 2] = x >> 8;
 701                                 reference[x + 3] = x >> 0;
 702                                 reference[x + 4] = 0xFF;
 703                                 reference[x + 5] = 0x00;
 704                                 reference[x + 6] = 0xFF;
 705                                 reference[x + 7] = 0x00;
 706                         }
 707                         if (do_write_10(0, lba_max * block_size,
 708                             reference, lun)) {
 709                                 printf("FAILED\n");
 710                                 lba_max /= 2;
 711                                 if (lba_max)
 712                                         goto retry_read_init;
 713                                 goto fail;
 714                         }
 715                         printf("SUCCESS\n");
 716                         break;
 717                 case USB_MSC_IO_PATTERN_RANDOM:
 718                         for (x = 0; x != (block_size * lba_max); x++) {
 719                                 reference[x] = usb_ts_rand_noise() % 255U;
 720                         }
 721                         if (do_write_10(0, lba_max * block_size,
 722                             reference, lun)) {
 723                                 printf("FAILED\n");
 724                                 lba_max /= 2;
 725                                 if (lba_max)
 726                                         goto retry_read_init;
 727                                 goto fail;
 728                         }
 729                         printf("SUCCESS\n");
 730                         break;
 731                 default:
 732                         if (do_read_10(0, lba_max * block_size,
 733                             reference, lun)) {
 734                                 printf("FAILED\n");
 735                                 lba_max /= 2;
 736                                 if (lba_max)
 737                                         goto retry_read_init;
 738                                 goto fail;
 739                         }
 740                         printf("SUCCESS\n");
 741                         break;
 742                 }
 743                 break;
 744
 745         default:
 746                 if (do_read_10(0, lba_max * block_size, reference, lun)) {
 747                         printf("FAILED\n");
 748                         lba_max /= 2;
 749                         if (lba_max)
 750                                 goto retry_read_init;
 751                         goto fail;
 752                 }
 753                 printf("SUCCESS\n");
 754                 break;
 755         }
 756
 757
 758         if (p->try_abort_data_write) {
 759                 if (do_write_10(0, (2 * block_size) | 1, reference, lun))
 760                         printf("Aborted data write failed (OK)!\n");
 761                 else
 762                         printf("Aborted data write did not fail (ERROR)!\n");
 763
 764                 if (do_read_10(0, (2 * block_size), reference, lun))
 765                         printf("Post-aborted data read failed (ERROR)\n");
 766                 else
 767                         printf("Post-aborted data read success (OK)!\n");
 768         }
 769         printf("Starting test ...\n");
 770
 771         gettimeofday(&ref_tv, 0);
 772
 773         last_sec = ref_tv.tv_sec;
 774
 775         printf("\n");
 776
 777         while (1) {
 778
 779                 gettimeofday(&sub_tv, 0);
 780
 781                 if (last_sec != sub_tv.tv_sec) {
 782
 783                         printf("STATUS: ID=%u, RX=%u bytes/sec, "
 784                             "TX=%u bytes/sec, ERR=%u, RST=%u, DERR=%u\n",
 785                             (int)xfer_current_id,
 786                             (int)(stats.xfer_rx_bytes -
 787                             last_stat.xfer_rx_bytes),
 788                             (int)(stats.xfer_tx_bytes -
 789                             last_stat.xfer_tx_bytes),
 790                             (int)(stats.xfer_error),
 791                             (int)(stats.xfer_reset),
 792                             (int)(stats.data_error));
 793
 794                         fflush(stdout);
 795
 796                         last_sec = sub_tv.tv_sec;
 797                         last_stat = stats;
 798                 }
 799                 timersub(&sub_tv, &ref_tv, &res_tv);
 800
 801                 if ((res_tv.tv_sec < 0) || (res_tv.tv_sec >= (int)p->duration))
 802                         break;
 803
 804                 do_io_test(p, lun, lba_max, buffer, reference);
 805
 806                 if (libusb20_dev_check_connected(usb_pdev) != 0) {
 807                         printf("Device disconnected\n");
 808                         break;
 809                 }
 810                 if (p->done) {
 811                         printf("Maximum number of errors exceeded\n");
 812                         break;
 813                 }
 814         }
 815
 816         printf("\nTest done!\n");
 817
 818 fail:
 819         if (buffer)
 820                 free(buffer);
 821         if (reference)
 822                 free(reference);
 823 }
 824
 825 void
 826 show_host_device_selection(uint8_t level, uint16_t *pvid, uint16_t *ppid)
 827 {
 828         struct libusb20_backend *pbe;
 829         struct libusb20_device *pdev;
 830         struct LIBUSB20_DEVICE_DESC_DECODED *ddesc;
 831
 832         uint16_t vid[USB_DEVICES_MAX];
 833         uint16_t pid[USB_DEVICES_MAX];
 834
 835         int index;
 836         int sel;
 837
 838         const char *ptr;
 839
 840 top:
 841         pbe = libusb20_be_alloc_default();
 842         pdev = NULL;
 843         index = 0;
 844
 845         printf("\n[] Select USB device:\n");
 846
 847         while ((pdev = libusb20_be_device_foreach(pbe, pdev))) {
 848
 849                 if (libusb20_dev_get_mode(pdev) != LIBUSB20_MODE_HOST)
 850                         continue;
 851
 852                 if (index < USB_DEVICES_MAX) {
 853                         ddesc = libusb20_dev_get_device_desc(pdev);
 854                         ptr = libusb20_dev_get_desc(pdev);
 855                         printf("%s%d) %s\n", indent[level], index, ptr);
 856                         vid[index] = ddesc->idVendor;
 857                         pid[index] = ddesc->idProduct;
 858                         index++;
 859                 } else {
 860                         break;
 861                 }
 862         }
 863
 864         printf("%sr) Refresh device list\n", indent[level]);
 865         printf("%sx) Return to previous menu\n", indent[level]);
 866
 867         /* release data */
 868         libusb20_be_free(pbe);
 869
 870         sel = get_integer();
 871
 872         if (sel == -2)
 873                 goto top;
 874
 875         if ((sel < 0) || (sel >= index)) {
 876                 *pvid = 0;
 877                 *ppid = 0;
 878                 return;
 879         }
 880         *pvid = vid[sel];
 881         *ppid = pid[sel];
 882 }
 883
 884 struct libusb20_device *
 885 find_usb_device(uint16_t vid, uint16_t pid)
 886 {
 887         struct libusb20_backend *pbe = libusb20_be_alloc_default();
 888         struct libusb20_device *pdev = NULL;
 889         struct LIBUSB20_DEVICE_DESC_DECODED *ddesc;
 890
 891         while ((pdev = libusb20_be_device_foreach(pbe, pdev))) {
 892
 893                 if (libusb20_dev_get_mode(pdev) != LIBUSB20_MODE_HOST)
 894                         continue;
 895
 896                 ddesc = libusb20_dev_get_device_desc(pdev);
 897
 898                 if ((vid == ddesc->idVendor) &&
 899                     (pid == ddesc->idProduct)) {
 900                         libusb20_be_dequeue_device(pbe, pdev);
 901                         break;
 902                 }
 903         }
 904
 905         /* release data */
 906         libusb20_be_free(pbe);
 907
 908         return (pdev);
 909 }
 910
 911 void
 912 find_usb_endpoints(struct libusb20_device *pdev, uint8_t class,
 913     uint8_t subclass, uint8_t protocol, uint8_t alt_setting,
 914     uint8_t *pif, uint8_t *in_ep, uint8_t *out_ep, uint8_t next_if)
 915 {
 916         struct libusb20_config *pcfg;
 917         struct libusb20_interface *iface;
 918         struct libusb20_endpoint *ep;
 919         uint8_t x;
 920         uint8_t y;
 921         uint8_t z;
 922
 923         *in_ep = 0;
 924         *out_ep = 0;
 925         *pif = 0;
 926
 927         pcfg = libusb20_dev_alloc_config(pdev,
 928             libusb20_dev_get_config_index(pdev));
 929
 930         if (pcfg == NULL)
 931                 return;
 932
 933         for (x = 0; x != pcfg->num_interface; x++) {
 934
 935                 y = alt_setting;
 936
 937                 iface = (pcfg->interface + x);
 938
 939                 if ((iface->desc.bInterfaceClass == class) &&
 940                     (iface->desc.bInterfaceSubClass == subclass ||
 941                     subclass == 255) &&
 942                     (iface->desc.bInterfaceProtocol == protocol ||
 943                     protocol == 255)) {
 944
 945                         if (next_if) {
 946                                 x++;
 947                                 if (x == pcfg->num_interface)
 948                                         break;
 949                                 iface = (pcfg->interface + x);
 950                         }
 951                         *pif = x;
 952
 953                         for (z = 0; z != iface->num_endpoints; z++) {
 954                                 ep = iface->endpoints + z;
 955
 956                                 /* BULK only */
 957                                 if ((ep->desc.bmAttributes & 3) != 2)
 958                                         continue;
 959
 960                                 if (ep->desc.bEndpointAddress & 0x80)
 961                                         *in_ep = ep->desc.bEndpointAddress;
 962                                 else
 963                                         *out_ep = ep->desc.bEndpointAddress;
 964                         }
 965                         break;
 966                 }
 967         }
 968
 969         free(pcfg);
 970 }
 971
 972 static void
 973 exec_host_msc_test(struct usb_msc_params *p, uint16_t vid, uint16_t pid)
 974 {
 975         struct libusb20_device *pdev;
 976
 977         uint8_t in_ep;
 978         uint8_t out_ep;
 979         uint8_t iface;
 980
 981         int error;
 982
 983         memset(&stats, 0, sizeof(stats));
 984
 985         xfer_current_id = 0;
 986         xfer_wrapper_sig = CBWSIGNATURE;
 987
 988         pdev = find_usb_device(vid, pid);
 989         if (pdev == NULL) {
 990                 printf("USB device not found\n");
 991                 return;
 992         }
 993         find_usb_endpoints(pdev, 8, 6, 0x50, 0, &iface, &in_ep, &out_ep, 0);
 994
 995         if ((in_ep == 0) || (out_ep == 0)) {
 996                 printf("Could not find USB endpoints\n");
 997                 libusb20_dev_free(pdev);
 998                 return;
 999         }
1000         printf("Attaching to: %s @ iface %d\n",
1001             libusb20_dev_get_desc(pdev), iface);
1002
1003         if (libusb20_dev_open(pdev, 2)) {
1004                 printf("Could not open USB device\n");
1005                 libusb20_dev_free(pdev);
1006                 return;
1007         }
1008         if (libusb20_dev_detach_kernel_driver(pdev, iface)) {
1009                 printf("WARNING: Could not detach kernel driver\n");
1010         }
1011         xfer_in = libusb20_tr_get_pointer(pdev, 0);
1012         error = libusb20_tr_open(xfer_in, 65536, 1, in_ep);
1013         if (error) {
1014                 printf("Could not open USB endpoint %d\n", in_ep);
1015                 libusb20_dev_free(pdev);
1016                 return;
1017         }
1018         xfer_out = libusb20_tr_get_pointer(pdev, 1);
1019         error = libusb20_tr_open(xfer_out, 65536, 1, out_ep);
1020         if (error) {
1021                 printf("Could not open USB endpoint %d\n", out_ep);
1022                 libusb20_dev_free(pdev);
1023                 return;
1024         }
1025         usb_pdev = pdev;
1026         usb_iface = iface;
1027
1028         usb_msc_test(p);
1029
1030         libusb20_dev_free(pdev);
1031 }
1032
1033 static void
1034 set_defaults(struct usb_msc_params *p)
1035 {
1036         memset(p, 0, sizeof(*p));
1037
1038         p->duration = 60;               /* seconds */
1039         p->try_invalid_scsi_command = 1;
1040         p->try_invalid_wrapper_block = 1;
1041         p->try_last_lba = 1;
1042         p->max_errors = -1;
1043 }
1044
1045 static const char *
1046 get_io_mode(const struct usb_msc_params *p)
1047 {
1048         ;                               /* indent fix */
1049         switch (p->io_mode) {
1050         case USB_MSC_IO_MODE_READ_ONLY:
1051                 return ("Read Only");
1052         case USB_MSC_IO_MODE_WRITE_ONCE_READ_ONLY:
1053                 return ("Write Once, Read Only");
1054         case USB_MSC_IO_MODE_WRITE_ONLY:
1055                 return ("Write Only");
1056         case USB_MSC_IO_MODE_READ_WRITE:
1057                 return ("Read and Write");
1058         default:
1059                 return ("Unknown");
1060         }
1061 }
1062
1063 static const char *
1064 get_io_pattern(const struct usb_msc_params *p)
1065 {
1066         ;                               /* indent fix */
1067         switch (p->io_pattern) {
1068         case USB_MSC_IO_PATTERN_FIXED:
1069                 return ("Fixed");
1070         case USB_MSC_IO_PATTERN_RANDOM:
1071                 return ("Random");
1072         case USB_MSC_IO_PATTERN_PRESERVE:
1073                 return ("Preserve");
1074         default:
1075                 return ("Unknown");
1076         }
1077 }
1078
1079 static const char *
1080 get_io_size(const struct usb_msc_params *p)
1081 {
1082         ;                               /* indent fix */
1083         switch (p->io_size) {
1084         case USB_MSC_IO_SIZE_RANDOM:
1085                 return ("Random");
1086         case USB_MSC_IO_SIZE_INCREASING:
1087                 return ("Increasing");
1088         case USB_MSC_IO_SIZE_FIXED_1BLK:
1089                 return ("Single block");
1090         case USB_MSC_IO_SIZE_FIXED_2BLK:
1091                 return ("2 blocks");
1092         case USB_MSC_IO_SIZE_FIXED_4BLK:
1093                 return ("4 blocks");
1094         case USB_MSC_IO_SIZE_FIXED_8BLK:
1095                 return ("8 blocks");
1096         case USB_MSC_IO_SIZE_FIXED_16BLK:
1097                 return ("16 blocks");
1098         case USB_MSC_IO_SIZE_FIXED_32BLK:
1099                 return ("32 blocks");
1100         case USB_MSC_IO_SIZE_FIXED_64BLK:
1101                 return ("64 blocks");
1102         case USB_MSC_IO_SIZE_FIXED_128BLK:
1103                 return ("128 blocks");
1104         case USB_MSC_IO_SIZE_FIXED_256BLK:
1105                 return ("256 blocks");
1106         case USB_MSC_IO_SIZE_FIXED_512BLK:
1107                 return ("512 blocks");
1108         case USB_MSC_IO_SIZE_FIXED_1024BLK:
1109                 return ("1024 blocks");
1110         default:
1111                 return ("Unknown");
1112         }
1113 }
1114
1115 static const char *
1116 get_io_delay(const struct usb_msc_params *p)
1117 {
1118         ;                               /* indent fix */
1119         switch (p->io_delay) {
1120         case USB_MSC_IO_DELAY_NONE:
1121                 return ("None");
1122         case USB_MSC_IO_DELAY_RANDOM_10MS:
1123                 return ("Random 10ms");
1124         case USB_MSC_IO_DELAY_RANDOM_100MS:
1125                 return ("Random 100ms");
1126         case USB_MSC_IO_DELAY_FIXED_10MS:
1127                 return ("Fixed 10ms");
1128         case USB_MSC_IO_DELAY_FIXED_100MS:
1129                 return ("Fixed 100ms");
1130         default:
1131                 return ("Unknown");
1132         }
1133 }
1134
1135 static const char *
1136 get_io_offset(const struct usb_msc_params *p)
1137 {
1138         ;                               /* indent fix */
1139         switch (p->io_offset) {
1140         case USB_MSC_IO_OFF_START_OF_DISK:
1141                 return ("Start Of Disk");
1142         case USB_MSC_IO_OFF_RANDOM:
1143                 return ("Random Offset");
1144         default:
1145                 return ("Unknown");
1146         }
1147 }
1148
1149 static const char *
1150 get_io_area(const struct usb_msc_params *p)
1151 {
1152         ;                               /* indent fix */
1153         switch (p->io_area) {
1154         case USB_MSC_IO_AREA_COMPLETE:
1155                 return ("Complete Disk");
1156         case USB_MSC_IO_AREA_1MB:
1157                 return ("First MegaByte");
1158         case USB_MSC_IO_AREA_16MB:
1159                 return ("First 16 MegaBytes");
1160         case USB_MSC_IO_AREA_256MB:
1161                 return ("First 256 MegaBytes");
1162         default:
1163                 return ("Unknown");
1164         }
1165 }
1166
1167 void
1168 show_host_msc_test(uint8_t level, uint16_t vid,
1169     uint16_t pid, uint32_t duration)
1170 {
1171         struct usb_msc_params params;
1172         uint8_t retval;
1173
1174         set_defaults(&params);
1175
1176         params.duration = duration;
1177
1178         while (1) {
1179
1180                 retval = usb_ts_show_menu(level,
1181                     "Mass Storage Test Parameters",
1182                     " 1) Toggle I/O mode: <%s>\n"
1183                     " 2) Toggle I/O size: <%s>\n"
1184                     " 3) Toggle I/O delay: <%s>\n"
1185                     " 4) Toggle I/O offset: <%s>\n"
1186                     " 5) Toggle I/O area: <%s>\n"
1187                     " 6) Toggle I/O pattern: <%s>\n"
1188                     " 7) Toggle try invalid SCSI command: <%s>\n"
1189                     " 8) Toggle try invalid wrapper block: <%s>\n"
1190                     " 9) Toggle try invalid MaxPacketSize: <%s>\n"
1191                     "10) Toggle try last Logical Block Address: <%s>\n"
1192                     "11) Toggle I/O lun: <%d>\n"
1193                     "12) Set maximum number of errors: <%d>\n"
1194                     "13) Set test duration: <%d> seconds\n"
1195                     "14) Toggle try aborted write transfer: <%s>\n"
1196                     "15) Toggle request sense on error: <%s>\n"
1197                     "16) Toggle try all LUN: <%s>\n"
1198                     "20) Reset parameters\n"
1199                     "30) Start test (VID=0x%04x, PID=0x%04x)\n"
1200                     "40) Select another device\n"
1201                     " x) Return to previous menu \n",
1202                     get_io_mode(&params),
1203                     get_io_size(&params),
1204                     get_io_delay(&params),
1205                     get_io_offset(&params),
1206                     get_io_area(&params),
1207                     get_io_pattern(&params),
1208                     (params.try_invalid_scsi_command ? "YES" : "NO"),
1209                     (params.try_invalid_wrapper_block ? "YES" : "NO"),
1210                     (params.try_invalid_max_packet_size ? "YES" : "NO"),
1211                     (params.try_last_lba ? "YES" : "NO"),
1212                     params.io_lun,
1213                     (int)params.max_errors,
1214                     (int)params.duration,
1215                     (params.try_abort_data_write ? "YES" : "NO"),
1216                     (params.try_sense_on_error ? "YES" : "NO"),
1217                     (params.try_all_lun ? "YES" : "NO"),
1218                     vid, pid);
1219                 switch (retval) {
1220                 case 0:
1221                         break;
1222                 case 1:
1223                         params.io_mode++;
1224                         params.io_mode %= USB_MSC_IO_MODE_MAX;
1225                         break;
1226                 case 2:
1227                         params.io_size++;
1228                         params.io_size %= USB_MSC_IO_SIZE_MAX;
1229                         break;
1230                 case 3:
1231                         params.io_delay++;
1232                         params.io_delay %= USB_MSC_IO_DELAY_MAX;
1233                         break;
1234                 case 4:
1235                         params.io_offset++;
1236                         params.io_offset %= USB_MSC_IO_OFF_MAX;
1237                         break;
1238                 case 5:
1239                         params.io_area++;
1240                         params.io_area %= USB_MSC_IO_AREA_MAX;
1241                         break;
1242                 case 6:
1243                         params.io_pattern++;
1244                         params.io_pattern %= USB_MSC_IO_PATTERN_MAX;
1245                         break;
1246                 case 7:
1247                         params.try_invalid_scsi_command ^= 1;
1248                         break;
1249                 case 8:
1250                         params.try_invalid_wrapper_block ^= 1;
1251                         break;
1252                 case 9:
1253                         params.try_invalid_max_packet_size ^= 1;
1254                         break;
1255                 case 10:
1256                         params.try_last_lba ^= 1;
1257                         break;
1258                 case 11:
1259                         params.io_lun++;
1260                         params.io_lun %= USB_MSC_IO_LUN_MAX;
1261                         break;
1262                 case 12:
1263                         params.max_errors = get_integer();
1264                         break;
1265                 case 13:
1266                         params.duration = get_integer();
1267                         break;
1268                 case 14:
1269                         params.try_abort_data_write ^= 1;
1270                         break;
1271                 case 15:
1272                         params.try_sense_on_error ^= 1;
1273                         break;
1274                 case 16:
1275                         params.try_all_lun ^= 1;
1276                         break;
1277                 case 20:
1278                         set_defaults(&params);
1279                         break;
1280                 case 30:
1281                         exec_host_msc_test(&params, vid, pid);
1282                         break;
1283                 case 40:
1284                         show_host_device_selection(level + 1, &vid, &pid);
1285                         break;
1286                 default:
1287                         return;
1288                 }
1289         }
1290 }