#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <getopt.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <scsi/sg.h>

#ifndef BSG_IOC_ADD
#define BSG_IOC_ADD     0x8900
#define BSG_IOC_DEL     0x8901
#endif

#define NR_QUEUE	(64)
#define NR_VECS		(4)

#if 1
#define SECTORS		(768)
#define BS		(512)
#define ACCOUNT		(100)
#else
#define SECTORS		(32)
#define BS		(2048)
#define ACCOUNT		(1)
#endif

#define ALIGN(buf)	(char *) (((unsigned long) (buf) + 4095) & ~(4095))

struct sg_io_hdr *hdr;
char *cdb;
char *buffer[NR_QUEUE];
#if NR_VECS > 1
static struct sg_iovec vecs[NR_QUEUE][NR_VECS];
#endif
char *sense;
static int nr_queue = NR_QUEUE;

static unsigned long ms;
static unsigned long kb;

typedef unsigned char u8;
#define __LITTLE_ENDIAN_BITFIELD

struct request_sense {
#if defined(__BIG_ENDIAN_BITFIELD)
	u8 valid		: 1;
	u8 error_code		: 7;
#elif defined(__LITTLE_ENDIAN_BITFIELD)
	u8 error_code		: 7;
	u8 valid		: 1;
#endif
	u8 segment_number;
#if defined(__BIG_ENDIAN_BITFIELD)
	u8 reserved1		: 2;
	u8 ili			: 1;
	u8 reserved2		: 1;
	u8 sense_key		: 4;
#elif defined(__LITTLE_ENDIAN_BITFIELD)
	u8 sense_key		: 4;
	u8 reserved2		: 1;
	u8 ili			: 1;
	u8 reserved1		: 2;
#endif
	u8 information[4];
	u8 add_sense_len;
	u8 command_info[4];
	u8 asc;
	u8 ascq;
	u8 fruc;
	u8 sks[3];
	u8 asb[46];
};

int queue_cmds(int fd, struct sg_io_hdr *hdr, int nr)
{
	return write(fd, hdr, sizeof(struct sg_io_hdr) * nr);
}

inline void account_hdr(struct sg_io_hdr *hdr)
{
	kb += hdr->dxfer_len / 1024;
	ms += hdr->duration;
	if (kb > ACCOUNT*1024) {
		printf("data rate %luKiB/sec\n", kb * 1000 / ms);
		ms = kb = 0;
	}
}

void sense_decode(struct sg_io_hdr *h)
{
	struct request_sense *s = (struct request_sense *) h->sbp;
	int i;

	printf("cdb: ");
	for (i = 0; i < h->cmd_len; i++)
		printf("%02x ", h->cmdp[i]);
	printf("\n");
	printf("0x%02x/0x%02x/0x%02x\n", s->sense_key, s->asc, s->ascq);
}

int wait_cmd(int fd, struct sg_io_hdr *hdr, int nr)
{
	int ret = read(fd, hdr, sizeof(struct sg_io_hdr) * nr);
	int i;

	if (ret == -1)
		return ret;

	for (i = 0; i < nr; i++) {
		struct sg_io_hdr *h = hdr + i;

		//printf("%d completed\n", h->pack_id);
		if (h->status == 0x02)
			sense_decode(h);
		account_hdr(h);
	}

	return ret;
}

void do_read10(int fd, struct sg_io_hdr *hdr, unsigned long lba)
{
	hdr->interface_id = 'S';
	hdr->cmd_len = 10;
	hdr->dxfer_direction = SG_DXFER_FROM_DEV;
	hdr->dxfer_len = SECTORS * BS;

	hdr->cmdp[0] = 0x28;
	hdr->cmdp[2] = (lba >> 24) & 0xff;
	hdr->cmdp[3] = (lba >> 16) & 0xff;
	hdr->cmdp[4] = (lba >>  8) & 0xff;
	hdr->cmdp[5] = (lba      ) & 0xff;
	hdr->cmdp[7] = (SECTORS >> 8) & 0xff;
	hdr->cmdp[8] = (SECTORS     ) & 0xff;
}

int bind_device(const char *device)
{
	int fd = open(device, O_RDONLY | O_NONBLOCK);
	int bsg_fd;

	if (fd == -1) {
		perror("open input");
		return -1;
	}

	bsg_fd = open("/dev/bsg", O_RDWR);
	if (bsg_fd == -1) {
		perror("open bsg");
		return -1;
	}

	if (ioctl(bsg_fd, BSG_IOC_ADD, fd) == -1) {
		perror("BSG_IOC_ADD");
		return -1;
	}

	return bsg_fd;
}

int main(int argc, const char *argv[])
{
	unsigned long lba = 0, read_kb;
	int fd, i, ret;

	if (argc < 2) {
		printf("%s: <device>\n", argv[0]);
		return 1;
	}

	fd = bind_device(argv[1]);
	if (fd == -1)
		return 1;

	cdb = malloc(10 * NR_QUEUE);
	memset(cdb, 0, 10 * NR_QUEUE);

	hdr = malloc(NR_QUEUE * sizeof(struct sg_io_hdr));
	memset(hdr, 0, NR_QUEUE * sizeof(struct sg_io_hdr));

	sense = malloc(NR_QUEUE * 64);
	memset(sense, 0, 64);
	
	for (i = 0; i < NR_QUEUE; i++) {
#if NR_VECS > 1
		int j;
		hdr[i].dxferp = vecs[i];
		hdr[i].iovec_count = NR_VECS;
		for (j = 0; j < NR_VECS; j++) {
			int size = SECTORS*BS/NR_VECS;
			vecs[i][j].iov_base = ALIGN(malloc(size*2));
			vecs[i][j].iov_len = size;
		}
#else
		buffer[i] = ALIGN(malloc(SECTORS*BS+BS-1));
#endif
	}

	for (read_kb = 0; read_kb < 256*1024;) {
		for (i = 0; i < nr_queue; i++) {
			struct sg_io_hdr *h = hdr + i;
#if NR_VECS <= 1
			h->dxferp = buffer[i];
#endif
			h->cmdp = cdb + i*10;
			h->pack_id = i;
			h->sbp = sense + i*64;
			h->mx_sb_len = 64;

			lba = (1+(int) (5120000.0*rand()/(RAND_MAX+1.0)));

			do_read10(fd, h, lba);
			lba += SECTORS;
		}

		ret = queue_cmds(fd, hdr, nr_queue);
		if (ret == -1) {
			perror("write cmds\n");
			break;
		} else if (!ret) {
			printf("strange, nothing queued but no error\n");
			break;
		} else if ((ret / sizeof(struct sg_io_hdr)) < nr_queue) {
			nr_queue = ret / sizeof(struct sg_io_hdr);
			printf("adjusting depth %d\n", nr_queue);
		}
		if (wait_cmd(fd, hdr, nr_queue) == -1) {
			perror("read cmd\n");
			break;
		}
		read_kb += nr_queue * (SECTORS >> 1);
	}

	close(fd);
	return 0;
}