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 >= 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 }
1291